diff --git a/.gitignore b/.gitignore index e1edf598..12736869 100644 --- a/.gitignore +++ b/.gitignore @@ -45,20 +45,20 @@ Examples/Stereo-Inertial/stereo_inertial_realsense_t265 Examples/Stereo-Inertial/stereo_inertial_realsense_D435i -Examples/ROS/ORB_SLAM3/Mono -Examples/ROS/ORB_SLAM3/Mono_Inertial -Examples/ROS/ORB_SLAM3/MonoAR -Examples/ROS/ORB_SLAM3/RGBD -Examples/ROS/ORB_SLAM3/Stereo -Examples/ROS/ORB_SLAM3/Stereo_Inertial +Examples/ROS/MORB_SLAM/Mono +Examples/ROS/MORB_SLAM/Mono_Inertial +Examples/ROS/MORB_SLAM/MonoAR +Examples/ROS/MORB_SLAM/RGBD +Examples/ROS/MORB_SLAM/Stereo +Examples/ROS/MORB_SLAM/Stereo_Inertial Examples/Calibration/recorder_realsense_D435i Examples/Tests/viewer_dataset Examples/Tests/sophus_test -Examples/ROS/ORB_SLAM3/CMakeLists.txt.user -Examples/ROS/ORB_SLAM3/build/ +Examples/ROS/MORB_SLAM/CMakeLists.txt.user +Examples/ROS/MORB_SLAM/build/ KeyFrameTrajectory.txt CameraTrajectory.txt diff --git a/CMakeLists.txt b/CMakeLists.txt index 4b7d46ec..c027fa14 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -24,7 +24,7 @@ endif() LIST(APPEND CMAKE_MODULE_PATH ${PROJECT_SOURCE_DIR}/cmake_modules) -find_package(OpenCV 4.4) +find_package(OpenCV 4.2) if(NOT OpenCV_FOUND) message(FATAL_ERROR "OpenCV > 4.4 not found.") endif() @@ -50,6 +50,7 @@ find_package(realsense2) add_library(${PROJECT_NAME} SHARED src/System.cc +src/Camera.cpp src/Tracking.cc src/LocalMapping.cc src/LoopClosing.cc @@ -75,38 +76,37 @@ src/OptimizableTypes.cpp src/MLPnPsolver.cpp src/GeometricTools.cc src/TwoViewReconstruction.cc -src/Config.cc src/Settings.cc -include/System.h -include/Tracking.h -include/LocalMapping.h -include/LoopClosing.h -include/ORBextractor.h -include/ORBmatcher.h -include/FrameDrawer.h -include/Converter.h -include/MapPoint.h -include/KeyFrame.h -include/Atlas.h -include/Map.h -include/MapDrawer.h -include/Optimizer.h -include/Frame.h -include/KeyFrameDatabase.h -include/Sim3Solver.h -include/Viewer.h -include/ImuTypes.h -include/G2oTypes.h -include/CameraModels/GeometricCamera.h -include/CameraModels/Pinhole.h -include/CameraModels/KannalaBrandt8.h -include/OptimizableTypes.h -include/MLPnPsolver.h -include/GeometricTools.h -include/TwoViewReconstruction.h -include/SerializationUtils.h -include/Config.h -include/Settings.h) +include/MORB_SLAM/System.h +include/MORB_SLAM/Camera.hpp +include/MORB_SLAM/Tracking.h +include/MORB_SLAM/LocalMapping.h +include/MORB_SLAM/LoopClosing.h +include/MORB_SLAM/ORBextractor.h +include/MORB_SLAM/ORBmatcher.h +include/MORB_SLAM/FrameDrawer.h +include/MORB_SLAM/Converter.h +include/MORB_SLAM/MapPoint.h +include/MORB_SLAM/KeyFrame.h +include/MORB_SLAM/Atlas.h +include/MORB_SLAM/Map.h +include/MORB_SLAM/MapDrawer.h +include/MORB_SLAM/Optimizer.h +include/MORB_SLAM/Frame.h +include/MORB_SLAM/KeyFrameDatabase.h +include/MORB_SLAM/Sim3Solver.h +include/MORB_SLAM/Viewer.h +include/MORB_SLAM/ImuTypes.h +include/MORB_SLAM/G2oTypes.h +include/MORB_SLAM/CameraModels/GeometricCamera.h +include/MORB_SLAM/CameraModels/Pinhole.h +include/MORB_SLAM/CameraModels/KannalaBrandt8.h +include/MORB_SLAM/OptimizableTypes.h +include/MORB_SLAM/MLPnPsolver.h +include/MORB_SLAM/GeometricTools.h +include/MORB_SLAM/TwoViewReconstruction.h +include/MORB_SLAM/SerializationUtils.h +include/MORB_SLAM/Settings.h) add_subdirectory(Thirdparty/g2o) diff --git a/Examples/Calibration/recorder_realsense_D435i.cc b/Examples/Calibration/recorder_realsense_D435i.cc index ac292ae4..763e2478 100644 --- a/Examples/Calibration/recorder_realsense_D435i.cc +++ b/Examples/Calibration/recorder_realsense_D435i.cc @@ -103,7 +103,7 @@ int main(int argc, char **argv) { sigemptyset(&sigIntHandler.sa_mask); sigIntHandler.sa_flags = 0; - sigaction(SIGINT, &sigIntHandler, NULL); + sigaction(SIGINT, &sigIntHandler, nullptr); b_continue_session = true; double offset = 0; // ms diff --git a/Examples/Calibration/recorder_realsense_T265.cc b/Examples/Calibration/recorder_realsense_T265.cc index 352b070a..000857f1 100644 --- a/Examples/Calibration/recorder_realsense_T265.cc +++ b/Examples/Calibration/recorder_realsense_T265.cc @@ -110,7 +110,7 @@ int main(int argc, char **argv) { sigemptyset(&sigIntHandler.sa_mask); sigIntHandler.sa_flags = 0; - sigaction(SIGINT, &sigIntHandler, NULL); + sigaction(SIGINT, &sigIntHandler, nullptr); b_continue_session = true; double offset = 0; // ms diff --git a/Examples/Monocular-Inertial/mono_inertial_euroc.cc b/Examples/Monocular-Inertial/mono_inertial_euroc.cc index 8c02bb52..0ae51f69 100644 --- a/Examples/Monocular-Inertial/mono_inertial_euroc.cc +++ b/Examples/Monocular-Inertial/mono_inertial_euroc.cc @@ -26,9 +26,9 @@ #include -#include -#include -#include "ImuTypes.h" +#include +#include +#include "MORB_SLAM/ImuTypes.h" using namespace std; @@ -118,8 +118,8 @@ int main(int argc, char *argv[]) cout.precision(17); // Create SLAM system. It initializes all system threads and gets ready to process frames. - ORB_SLAM3::System_ptr SLAM = std::make_shared(argv[1],argv[2],ORB_SLAM3::CameraType::IMU_MONOCULAR); - ORB_SLAM3::Viewer viewer(SLAM, argv[2]); + MORB_SLAM::System_ptr SLAM = std::make_shared(argv[1],argv[2],MORB_SLAM::CameraType::IMU_MONOCULAR); + MORB_SLAM::Viewer viewer(SLAM, argv[2]); float imageScale = SLAM->GetImageScale(); #ifdef REGISTER_TIMES @@ -132,7 +132,7 @@ int main(int argc, char *argv[]) // Main loop cv::Mat im; - vector vImuMeas; + vector vImuMeas; proccIm = 0; for(int ni=0; ni #include "librealsense2/rsutil.h" -#include -#include +#include +#include using namespace std; @@ -116,7 +116,7 @@ int main(int argc, char **argv) { sigemptyset(&sigIntHandler.sa_mask); sigIntHandler.sa_flags = 0; - sigaction(SIGINT, &sigIntHandler, NULL); + sigaction(SIGINT, &sigIntHandler, nullptr); b_continue_session = true; double offset = 0; // ms @@ -258,7 +258,7 @@ int main(int argc, char **argv) { rs2::pipeline_profile pipe_profile = pipe.start(cfg, imu_callback); - vector vImuMeas; + vector vImuMeas; rs2::stream_profile cam_stream = pipe_profile.get_stream(RS2_STREAM_INFRARED, 1); @@ -288,8 +288,8 @@ int main(int argc, char **argv) { // Create SLAM system. It initializes all system threads and gets ready to process frames. - ORB_SLAM3::System_ptr SLAM = std::make_shared(argv[1],argv[2],ORB_SLAM3::CameraType::IMU_MONOCULAR, file_name); - ORB_SLAM3::Viewer viewer(SLAM, argv[2]); + MORB_SLAM::System_ptr SLAM = std::make_shared(argv[1],argv[2],MORB_SLAM::CameraType::IMU_MONOCULAR, file_name); + MORB_SLAM::Viewer viewer(SLAM, argv[2]); float imageScale = SLAM->GetImageScale(); double timestamp; @@ -356,7 +356,7 @@ int main(int argc, char **argv) { for(size_t i=0; i -#include -#include +#include +#include #include -#include "ImuTypes.h" +#include "MORB_SLAM/ImuTypes.h" using namespace std; @@ -65,8 +65,8 @@ int main(int argc, char **argv) // bFileName = true; // UNUSED } - ORB_SLAM3::System_ptr SLAM = std::make_shared(argv[1],argv[2],ORB_SLAM3::CameraType::IMU_MONOCULAR, file_name); - ORB_SLAM3::Viewer viewer(SLAM, argv[2]); + MORB_SLAM::System_ptr SLAM = std::make_shared(argv[1],argv[2],MORB_SLAM::CameraType::IMU_MONOCULAR, file_name); + MORB_SLAM::Viewer viewer(SLAM, argv[2]); float imageScale = SLAM->GetImageScale(); struct sigaction sigIntHandler; @@ -75,7 +75,7 @@ int main(int argc, char **argv) sigemptyset(&sigIntHandler.sa_mask); sigIntHandler.sa_flags = 0; - sigaction(SIGINT, &sigIntHandler, NULL); + sigaction(SIGINT, &sigIntHandler, nullptr); b_continue_session = true; double offset = 0; // ms @@ -206,7 +206,7 @@ int main(int argc, char **argv) // Create SLAM system. It initializes all system threads and gets ready to process frames. - vector vImuMeas; + vector vImuMeas; double timestamp; cv::Mat im; @@ -286,7 +286,7 @@ int main(int argc, char **argv) for(size_t i=0; i -#include -#include -#include "ImuTypes.h" +#include +#include +#include "MORB_SLAM/ImuTypes.h" using namespace std; @@ -117,8 +117,8 @@ int main(int argc, char **argv) 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_ptr SLAM = std::make_shared(argv[1],argv[2],ORB_SLAM3::CameraType::IMU_MONOCULAR, file_name); - ORB_SLAM3::Viewer viewer(SLAM, argv[2]); + MORB_SLAM::System_ptr SLAM = std::make_shared(argv[1],argv[2],MORB_SLAM::CameraType::IMU_MONOCULAR, file_name); + MORB_SLAM::Viewer viewer(SLAM, argv[2]); float imageScale = SLAM->GetImageScale(); #ifdef REGISTER_TIMES @@ -131,7 +131,7 @@ int main(int argc, char **argv) // Main loop cv::Mat im; - vector vImuMeas; + vector vImuMeas; proccIm = 0; cv::Ptr clahe = cv::createCLAHE(3.0, cv::Size(8, 8)); for(int ni=0; ni -#include +#include +#include using namespace std; @@ -112,7 +112,7 @@ int main(int argc, char **argv) { sigemptyset(&sigIntHandler.sa_mask); sigIntHandler.sa_flags = 0; - sigaction(SIGINT, &sigIntHandler, NULL); + sigaction(SIGINT, &sigIntHandler, nullptr); b_continue_session = true; // double offset = 0; // UNUSED // ms @@ -208,8 +208,8 @@ int main(int argc, char **argv) { std::cout << " Model = " << intrinsics_left.model << std::endl; // Create SLAM system. It initializes all system threads and gets ready to process frames. - ORB_SLAM3::System_ptr SLAM = std::make_shared(argv[1],argv[2],ORB_SLAM3::CameraType::MONOCULAR, file_name); - ORB_SLAM3::Viewer viewer(SLAM, argv[2]); + MORB_SLAM::System_ptr SLAM = std::make_shared(argv[1],argv[2],MORB_SLAM::CameraType::MONOCULAR, file_name); + MORB_SLAM::Viewer viewer(SLAM, argv[2]); float imageScale = SLAM->GetImageScale(); double timestamp; diff --git a/Examples/Monocular/mono_realsense_t265.cc b/Examples/Monocular/mono_realsense_t265.cc index f784356d..f691e170 100644 --- a/Examples/Monocular/mono_realsense_t265.cc +++ b/Examples/Monocular/mono_realsense_t265.cc @@ -29,8 +29,8 @@ #include -#include -#include +#include +#include using namespace std; @@ -66,7 +66,7 @@ int main(int argc, char **argv) sigemptyset(&sigIntHandler.sa_mask); sigIntHandler.sa_flags = 0; - sigaction(SIGINT, &sigIntHandler, NULL); + sigaction(SIGINT, &sigIntHandler, nullptr); b_continue_session = true; @@ -88,8 +88,8 @@ int main(int argc, char **argv) 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_ptr SLAM = std::make_shared(argv[1],argv[2],ORB_SLAM3::CameraType::MONOCULAR); - ORB_SLAM3::Viewer viewer(SLAM, argv[2]); + MORB_SLAM::System_ptr SLAM = std::make_shared(argv[1],argv[2],MORB_SLAM::CameraType::MONOCULAR); + MORB_SLAM::Viewer viewer(SLAM, argv[2]); float imageScale = SLAM->GetImageScale(); cv::Mat imCV; diff --git a/Examples/Monocular/mono_tum.cc b/Examples/Monocular/mono_tum.cc index f528f93d..4eb56d54 100644 --- a/Examples/Monocular/mono_tum.cc +++ b/Examples/Monocular/mono_tum.cc @@ -23,8 +23,8 @@ #include -#include -#include +#include +#include using namespace std; @@ -48,8 +48,8 @@ int main(int argc, char **argv) int nImages = vstrImageFilenames.size(); // Create SLAM system. It initializes all system threads and gets ready to process frames. - ORB_SLAM3::System_ptr SLAM = std::make_shared(argv[1],argv[2],ORB_SLAM3::CameraType::MONOCULAR); - ORB_SLAM3::Viewer viewer(SLAM, argv[2]); + MORB_SLAM::System_ptr SLAM = std::make_shared(argv[1],argv[2],MORB_SLAM::CameraType::MONOCULAR); + MORB_SLAM::Viewer viewer(SLAM, argv[2]); float imageScale = SLAM->GetImageScale(); // Vector for tracking time statistics diff --git a/Examples/Monocular/mono_tum_vi.cc b/Examples/Monocular/mono_tum_vi.cc index a2faedb4..4b184755 100644 --- a/Examples/Monocular/mono_tum_vi.cc +++ b/Examples/Monocular/mono_tum_vi.cc @@ -25,9 +25,9 @@ #include -#include -#include -#include "Converter.h" +#include +#include +#include "MORB_SLAM/Converter.h" using namespace std; @@ -90,8 +90,8 @@ int main(int argc, char **argv) cout.precision(17); // Create SLAM system. It initializes all system threads and gets ready to process frames. - ORB_SLAM3::System_ptr SLAM = std::make_shared(argv[1],argv[2],ORB_SLAM3::CameraType::MONOCULAR, file_name); - ORB_SLAM3::Viewer viewer(SLAM, argv[2]); + MORB_SLAM::System_ptr SLAM = std::make_shared(argv[1],argv[2],MORB_SLAM::CameraType::MONOCULAR, file_name); + MORB_SLAM::Viewer viewer(SLAM, argv[2]); float imageScale = SLAM->GetImageScale(); #ifdef REGISTER_TIMES diff --git a/Examples/RGB-D-Inertial/rgbd_inertial_realsense_D435i.cc b/Examples/RGB-D-Inertial/rgbd_inertial_realsense_D435i.cc index 90ff063c..c328e3ad 100644 --- a/Examples/RGB-D-Inertial/rgbd_inertial_realsense_D435i.cc +++ b/Examples/RGB-D-Inertial/rgbd_inertial_realsense_D435i.cc @@ -33,8 +33,8 @@ #include "librealsense2/rsutil.h" -#include -#include +#include +#include using namespace std; @@ -120,7 +120,7 @@ int main(int argc, char **argv) { sigemptyset(&sigIntHandler.sa_mask); sigIntHandler.sa_flags = 0; - sigaction(SIGINT, &sigIntHandler, NULL); + sigaction(SIGINT, &sigIntHandler, nullptr); b_continue_session = true; double offset = 0; // ms @@ -291,7 +291,7 @@ int main(int argc, char **argv) { pipe_profile = pipe.start(cfg, imu_callback); - vector vImuMeas; + vector vImuMeas; rs2::stream_profile cam_stream = pipe_profile.get_stream(RS2_STREAM_COLOR); rs2::stream_profile imu_stream = pipe_profile.get_stream(RS2_STREAM_GYRO); @@ -319,8 +319,8 @@ int main(int argc, char **argv) { // Create SLAM system. It initializes all system threads and gets ready to process frames. - ORB_SLAM3::System_ptr SLAM = std::make_shared(argv[1],argv[2],ORB_SLAM3::CameraType::IMU_RGBD, file_name); - ORB_SLAM3::Viewer viewer(SLAM, argv[2]); + MORB_SLAM::System_ptr SLAM = std::make_shared(argv[1],argv[2],MORB_SLAM::CameraType::IMU_RGBD, file_name); + MORB_SLAM::Viewer viewer(SLAM, argv[2]); float imageScale = SLAM->GetImageScale(); double timestamp; @@ -404,7 +404,7 @@ int main(int argc, char **argv) { for(size_t i=0; i -#include +#include +#include using namespace std; @@ -122,7 +122,7 @@ int main(int argc, char **argv) { sigemptyset(&sigIntHandler.sa_mask); sigIntHandler.sa_flags = 0; - sigaction(SIGINT, &sigIntHandler, NULL); + sigaction(SIGINT, &sigIntHandler, nullptr); b_continue_session = true; // double offset = 0; // UNUSED // ms @@ -307,8 +307,8 @@ int main(int argc, char **argv) { // Create SLAM system. It initializes all system threads and gets ready to process frames. - ORB_SLAM3::System_ptr SLAM = std::make_shared(argv[1],argv[2],ORB_SLAM3::CameraType::RGBD, file_name); - ORB_SLAM3::Viewer viewer(SLAM, argv[2]); + MORB_SLAM::System_ptr SLAM = std::make_shared(argv[1],argv[2],MORB_SLAM::CameraType::RGBD, file_name); + MORB_SLAM::Viewer viewer(SLAM, argv[2]); float imageScale = SLAM->GetImageScale(); double timestamp; diff --git a/Examples/RGB-D/rgbd_tum.cc b/Examples/RGB-D/rgbd_tum.cc index 6274e76c..de6196c2 100644 --- a/Examples/RGB-D/rgbd_tum.cc +++ b/Examples/RGB-D/rgbd_tum.cc @@ -23,8 +23,8 @@ #include -#include -#include +#include +#include using namespace std; @@ -60,8 +60,8 @@ int main(int argc, char **argv) } // Create SLAM system. It initializes all system threads and gets ready to process frames. - ORB_SLAM3::System_ptr SLAM = std::make_shared(argv[1],argv[2],ORB_SLAM3::CameraType::RGBD); - ORB_SLAM3::Viewer viewer(SLAM, argv[2]); + MORB_SLAM::System_ptr SLAM = std::make_shared(argv[1],argv[2],MORB_SLAM::CameraType::RGBD); + MORB_SLAM::Viewer viewer(SLAM, argv[2]); float imageScale = SLAM->GetImageScale(); // Vector for tracking time statistics diff --git a/Examples/Stereo-Inertial/RealSense_D435i.yaml b/Examples/Stereo-Inertial/RealSense_D435i.yaml index e69ba166..0f5dfc20 100755 --- a/Examples/Stereo-Inertial/RealSense_D435i.yaml +++ b/Examples/Stereo-Inertial/RealSense_D435i.yaml @@ -42,10 +42,10 @@ IMU.T_b_c1: !!opencv-matrix IMU.InsertKFsWhenLost: 0 # IMU noise (Use those from VINS-mono) -IMU.NoiseGyro: 1e-3 # 2.44e-4 #1e-3 # rad/s^0.5 -IMU.NoiseAcc: 1e-2 # 1.47e-3 #1e-2 # m/s^1.5 -IMU.GyroWalk: 1e-6 # rad/s^1.5 -IMU.AccWalk: 1e-4 # m/s^2.5 +IMU.NoiseGyro: 0.008449 # 1e-3 # rad/s^0.5 # 1e-3 2.44e-4 +IMU.NoiseAcc: 0.01066 # 1e-2 # m/s^1.5 # 1e-2 1.47e-3 +IMU.GyroWalk: 0.000754 # rad/s^1.5 1e-6 +IMU.AccWalk: 0.00930 # m/s^2.5 1e-4 IMU.Frequency: 200.0 #-------------------------------------------------------------------------------------------- @@ -82,9 +82,9 @@ Viewer.ViewpointZ: -3.5 Viewer.ViewpointF: 500.0 # Save/load Map -System.LoadAtlasFromFile: "/home/ryan/MartinRea/MORB_SLAM/foxtrot5" +System.LoadAtlasFromFile: "/home/landson/MORB_SLAM/trial" # System.LoadAtlasFromFile: "/home/ryan/MartinRea/MORB_SLAM/loopy" -System.SaveAtlasToFile: "/home/ryan/MartinRea/MORB_SLAM/rando" +# System.SaveAtlasToFile: "/home/ryan/MartinRea/MORB_SLAM/rando" diff --git a/Examples/Stereo-Inertial/stereo_inertial_euroc.cc b/Examples/Stereo-Inertial/stereo_inertial_euroc.cc index fdfa1aec..4b20a4ce 100644 --- a/Examples/Stereo-Inertial/stereo_inertial_euroc.cc +++ b/Examples/Stereo-Inertial/stereo_inertial_euroc.cc @@ -27,10 +27,10 @@ #include -#include -#include -#include "ImuTypes.h" -#include "Optimizer.h" +#include +#include +#include "MORB_SLAM/ImuTypes.h" +#include "MORB_SLAM/Optimizer.h" using namespace std; @@ -130,14 +130,14 @@ int main(int argc, char **argv) cout.precision(17); // Create SLAM system. It initializes all system threads and gets ready to process frames. - ORB_SLAM3::System_ptr SLAM = std::make_shared(argv[1],argv[2],ORB_SLAM3::CameraType::IMU_STEREO); - ORB_SLAM3::Viewer viewer(SLAM, argv[2]); + MORB_SLAM::System_ptr SLAM = std::make_shared(argv[1],argv[2],MORB_SLAM::CameraType::IMU_STEREO); + MORB_SLAM::Viewer viewer(SLAM, argv[2]); cv::Mat imLeft, imRight; for (seq = 0; seq vImuMeas; + vector vImuMeas; #ifdef REGISTER_TIMES double t_rect = 0.f; @@ -174,7 +174,7 @@ int main(int argc, char **argv) if(ni>0) while(vTimestampsImu[seq][first_imu[seq]]<=vTimestampsCam[seq][ni]) // while(vTimestampsImu[first_imu]<=vTimestampsCam[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, + vImuMeas.push_back(MORB_SLAM::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]])); first_imu[seq]++; diff --git a/Examples/Stereo-Inertial/stereo_inertial_realsense_D435i.cc b/Examples/Stereo-Inertial/stereo_inertial_realsense_D435i.cc old mode 100644 new mode 100755 index 58b3ab87..3f027014 --- a/Examples/Stereo-Inertial/stereo_inertial_realsense_D435i.cc +++ b/Examples/Stereo-Inertial/stereo_inertial_realsense_D435i.cc @@ -32,8 +32,8 @@ #include #include "librealsense2/rsutil.h" -#include -#include +#include +#include using namespace std; @@ -116,7 +116,7 @@ int main(int argc, char **argv) { sigemptyset(&sigIntHandler.sa_mask); sigIntHandler.sa_flags = 0; - sigaction(SIGINT, &sigIntHandler, NULL); + sigaction(SIGINT, &sigIntHandler, nullptr); b_continue_session = true; double offset = 0; // ms @@ -262,7 +262,7 @@ int main(int argc, char **argv) { rs2::pipeline_profile pipe_profile = pipe.start(cfg, imu_callback); - vector vImuMeas; + vector vImuMeas; rs2::stream_profile cam_left = pipe_profile.get_stream(RS2_STREAM_INFRARED, 1); rs2::stream_profile cam_right = pipe_profile.get_stream(RS2_STREAM_INFRARED, 2); @@ -318,8 +318,8 @@ int main(int argc, char **argv) { // Create SLAM system. It initializes all system threads and gets ready to process frames. - ORB_SLAM3::System_ptr SLAM = std::make_shared(argv[1],argv[2],ORB_SLAM3::CameraType::IMU_STEREO, file_name); - ORB_SLAM3::Viewer viewer(SLAM, argv[2]); + MORB_SLAM::System_ptr SLAM = std::make_shared(argv[1],argv[2],MORB_SLAM::CameraType::IMU_STEREO, file_name); + MORB_SLAM::Viewer viewer(SLAM, argv[2]); float imageScale = SLAM->GetImageScale(); double timestamp; @@ -388,7 +388,7 @@ int main(int argc, char **argv) { for(size_t i=0; iTrackStereo(im, imRight, timestamp, vImuMeas); + std::cout << "POSE: " << std::endl << pos.inverse().translation() << std::endl; #ifdef REGISTER_TIMES std::chrono::steady_clock::time_point t_End_Track = std::chrono::steady_clock::now(); t_track = t_resize + std::chrono::duration_cast >(t_End_Track - t_Start_Track).count(); diff --git a/Examples/Stereo-Inertial/stereo_inertial_realsense_t265.cc b/Examples/Stereo-Inertial/stereo_inertial_realsense_t265.cc index f00b1f40..be3422b5 100644 --- a/Examples/Stereo-Inertial/stereo_inertial_realsense_t265.cc +++ b/Examples/Stereo-Inertial/stereo_inertial_realsense_t265.cc @@ -29,10 +29,10 @@ #include -#include -#include +#include +#include #include -#include "ImuTypes.h" +#include "MORB_SLAM/ImuTypes.h" using namespace std; @@ -67,8 +67,8 @@ int main(int argc, char **argv) // bFileName = true; // UNUSED } - ORB_SLAM3::System_ptr SLAM = std::make_shared(argv[1],argv[2],ORB_SLAM3::CameraType::IMU_STEREO, file_name); - ORB_SLAM3::Viewer viewer(SLAM, argv[2]); + MORB_SLAM::System_ptr SLAM = std::make_shared(argv[1],argv[2],MORB_SLAM::CameraType::IMU_STEREO, file_name); + MORB_SLAM::Viewer viewer(SLAM, argv[2]); float imageScale = SLAM->GetImageScale(); struct sigaction sigIntHandler; @@ -77,7 +77,7 @@ int main(int argc, char **argv) sigemptyset(&sigIntHandler.sa_mask); sigIntHandler.sa_flags = 0; - sigaction(SIGINT, &sigIntHandler, NULL); + sigaction(SIGINT, &sigIntHandler, nullptr); b_continue_session = true; double offset = 0; // ms @@ -196,7 +196,7 @@ int main(int argc, char **argv) } // Create SLAM system. It initializes all system threads and gets ready to process frames. - vector vImuMeas; + vector vImuMeas; double timestamp; cv::Mat im,imright; @@ -278,7 +278,7 @@ int main(int argc, char **argv) for(size_t i=0; i -#include -#include -#include "ImuTypes.h" +#include +#include +#include "MORB_SLAM/ImuTypes.h" using namespace std; @@ -120,8 +120,8 @@ int main(int argc, char **argv) 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_ptr SLAM = std::make_shared(argv[1],argv[2],ORB_SLAM3::CameraType::IMU_STEREO, file_name); - ORB_SLAM3::Viewer viewer(SLAM, argv[2]); + MORB_SLAM::System_ptr SLAM = std::make_shared(argv[1],argv[2],MORB_SLAM::CameraType::IMU_STEREO, file_name); + MORB_SLAM::Viewer viewer(SLAM, argv[2]); float imageScale = SLAM->GetImageScale(); #ifdef REGISTER_TIMES @@ -135,7 +135,7 @@ int main(int argc, char **argv) // Main loop cv::Mat imLeft, imRight; - vector vImuMeas; + vector vImuMeas; proccIm = 0; cv::Ptr clahe = cv::createCLAHE(3.0, cv::Size(8, 8)); for(int ni=0; ni(argv[1],argv[2],ORB_SLAM3::CameraType::STEREO, /*0, */file_name); - ORB_SLAM3::Viewer viewer(SLAM, argv[2]); + MORB_SLAM::System_ptr SLAM = std::make_shared(argv[1],argv[2],MORB_SLAM::CameraType::STEREO, /*0, */file_name); + MORB_SLAM::Viewer viewer(SLAM, argv[2]); float imageScale = SLAM->GetImageScale(); cv::Mat imLeft, imRight; - vector vImuMeas; + vector vImuMeas; rs2::stream_profile fisheye_stream_left = pipe_profile.get_stream(RS2_STREAM_FISHEYE, 1); rs2_intrinsics intrinsics_left = fisheye_stream_left.as().get_intrinsics(); diff --git a/Examples/Stereo/stereo_tum_vi.cc b/Examples/Stereo/stereo_tum_vi.cc index 85719981..dd64c5ff 100644 --- a/Examples/Stereo/stereo_tum_vi.cc +++ b/Examples/Stereo/stereo_tum_vi.cc @@ -25,8 +25,8 @@ #include -#include -#include +#include +#include using namespace std; @@ -91,8 +91,8 @@ int main(int argc, char **argv) cout.precision(17); // Create SLAM system. It initializes all system threads and gets ready to process frames. - ORB_SLAM3::System_ptr SLAM = std::make_shared(argv[1],argv[2],ORB_SLAM3::CameraType::STEREO); - ORB_SLAM3::Viewer viewer(SLAM, argv[2]); + MORB_SLAM::System_ptr SLAM = std::make_shared(argv[1],argv[2],MORB_SLAM::CameraType::STEREO); + MORB_SLAM::Viewer viewer(SLAM, argv[2]); float imageScale = SLAM->GetImageScale(); cout << endl << "-------" << endl; diff --git a/README.md b/README.md index e4907a44..3bda943d 100644 --- a/README.md +++ b/README.md @@ -2,7 +2,7 @@ [![CMake Build](https://github.com/Soldann/MORB_SLAM/actions/workflows/cmake.yml/badge.svg)](https://github.com/Soldann/MORB_SLAM/actions/workflows/cmake.yml) -This fork of [ORB_SLAM3](https://github.com/UZ-SLAMLab/ORB_SLAM3) converts it into a CMake package that can be imported into other projects. Run `morbslam_installer.sh` to install. +This fork of [MORB_SLAM](https://github.com/UZ-SLAMLab/MORB_SLAM) converts it into a CMake package that can be imported into other projects. Run `morbslam_installer.sh` to install. In your other projects, import using: ``` @@ -17,7 +17,7 @@ TARGET_LINK_LIBRARIES(${PROJECT_NAME} MORB_SLAM::MORB_SLAM) ### V1.0, December 22th, 2021 **Authors:** Carlos Campos, Richard Elvira, Juan J. Gómez Rodríguez, [José M. M. Montiel](http://webdiis.unizar.es/~josemari/), [Juan D. Tardos](http://webdiis.unizar.es/~jdtardos/). -The [Changelog](https://github.com/UZ-SLAMLab/ORB_SLAM3/blob/master/Changelog.md) describes the features of each version. +The [Changelog](https://github.com/UZ-SLAMLab/MORB_SLAM/blob/master/Changelog.md) describes the features of each version. ORB-SLAM3 is the first real-time SLAM library able to perform **Visual, Visual-Inertial and Multi-Map SLAM** with **monocular, stereo and RGB-D** cameras, using **pin-hole and fisheye** lens models. In all sensor configurations, ORB-SLAM3 is as robust as the best systems available in the literature, and significantly more accurate. @@ -46,7 +46,7 @@ alt="ORB-SLAM3" width="240" height="180" border="10" /> # 1. License -ORB-SLAM3 is released under [GPLv3 license](https://github.com/UZ-SLAMLab/ORB_SLAM3/LICENSE). For a list of all code/library dependencies (and associated licenses), please see [Dependencies.md](https://github.com/UZ-SLAMLab/ORB_SLAM3/blob/master/Dependencies.md). +ORB-SLAM3 is released under [GPLv3 license](https://github.com/UZ-SLAMLab/MORB_SLAM/LICENSE). For a list of all code/library dependencies (and associated licenses), please see [Dependencies.md](https://github.com/UZ-SLAMLab/MORB_SLAM/blob/master/Dependencies.md). For a closed-source version of ORB-SLAM3 for commercial purposes, please contact the authors: orbslam (at) unizar (dot) es. @@ -97,12 +97,12 @@ We provide some examples to process input of a monocular, monocular-inertial, st Clone the repository: ``` -git clone https://github.com/UZ-SLAMLab/ORB_SLAM3.git ORB_SLAM3 +git clone https://github.com/UZ-SLAMLab/MORB_SLAM.git MORB_SLAM ``` We provide a script `build.sh` to build the *Thirdparty* libraries and *ORB-SLAM3*. Please make sure you have installed all required dependencies (see section 2). Execute: ``` -cd ORB_SLAM3 +cd MORB_SLAM chmod +x build.sh ./build.sh ``` @@ -170,14 +170,14 @@ Execute the following script to process sequences and compute the RMS ATE: ### Building the nodes for mono, mono-inertial, stereo, stereo-inertial and RGB-D Tested with ROS Melodic and ubuntu 18.04. -1. Add the path including *Examples/ROS/ORB_SLAM3* to the ROS_PACKAGE_PATH environment variable. Open .bashrc file: +1. Add the path including *Examples/ROS/MORB_SLAM* to the ROS_PACKAGE_PATH environment variable. Open .bashrc file: ``` gedit ~/.bashrc ``` -and add at the end the following line. Replace PATH by the folder where you cloned ORB_SLAM3: +and add at the end the following line. Replace PATH by the folder where you cloned MORB_SLAM: ``` - export ROS_PACKAGE_PATH=${ROS_PACKAGE_PATH}:PATH/ORB_SLAM3/Examples/ROS + export ROS_PACKAGE_PATH=${ROS_PACKAGE_PATH}:PATH/MORB_SLAM/Examples/ROS ``` 2. Execute `build_ros.sh` script: @@ -188,38 +188,38 @@ and add at the end the following line. Replace PATH by the folder where you clon ``` ### Running Monocular Node -For a monocular input from topic `/camera/image_raw` run node ORB_SLAM3/Mono. You will need to provide the vocabulary file and a settings file. See the monocular examples above. +For a monocular input from topic `/camera/image_raw` run node MORB_SLAM/Mono. You will need to provide the vocabulary file and a settings file. See the monocular examples above. ``` - rosrun ORB_SLAM3 Mono PATH_TO_VOCABULARY PATH_TO_SETTINGS_FILE + rosrun MORB_SLAM Mono PATH_TO_VOCABULARY PATH_TO_SETTINGS_FILE ``` ### Running Monocular-Inertial Node -For a monocular input from topic `/camera/image_raw` and an inertial input from topic `/imu`, run node ORB_SLAM3/Mono_Inertial. Setting the optional third argument to true will apply CLAHE equalization to images (Mainly for TUM-VI dataset). +For a monocular input from topic `/camera/image_raw` and an inertial input from topic `/imu`, run node MORB_SLAM/Mono_Inertial. Setting the optional third argument to true will apply CLAHE equalization to images (Mainly for TUM-VI dataset). ``` - rosrun ORB_SLAM3 Mono PATH_TO_VOCABULARY PATH_TO_SETTINGS_FILE [EQUALIZATION] + rosrun MORB_SLAM Mono PATH_TO_VOCABULARY PATH_TO_SETTINGS_FILE [EQUALIZATION] ``` ### Running Stereo Node -For a stereo input from topic `/camera/left/image_raw` and `/camera/right/image_raw` run node ORB_SLAM3/Stereo. You will need to provide the vocabulary file and a settings file. For Pinhole camera model, if you **provide rectification matrices** (see Examples/Stereo/EuRoC.yaml example), the node will recitify the images online, **otherwise images must be pre-rectified**. For FishEye camera model, rectification is not required since system works with original images: +For a stereo input from topic `/camera/left/image_raw` and `/camera/right/image_raw` run node MORB_SLAM/Stereo. You will need to provide the vocabulary file and a settings file. For Pinhole camera model, if you **provide rectification matrices** (see Examples/Stereo/EuRoC.yaml example), the node will recitify the images online, **otherwise images must be pre-rectified**. For FishEye camera model, rectification is not required since system works with original images: ``` - rosrun ORB_SLAM3 Stereo PATH_TO_VOCABULARY PATH_TO_SETTINGS_FILE ONLINE_RECTIFICATION + rosrun MORB_SLAM Stereo PATH_TO_VOCABULARY PATH_TO_SETTINGS_FILE ONLINE_RECTIFICATION ``` ### Running Stereo-Inertial Node -For a stereo input from topics `/camera/left/image_raw` and `/camera/right/image_raw`, and an inertial input from topic `/imu`, run node ORB_SLAM3/Stereo_Inertial. You will need to provide the vocabulary file and a settings file, including rectification matrices if required in a similar way to Stereo case: +For a stereo input from topics `/camera/left/image_raw` and `/camera/right/image_raw`, and an inertial input from topic `/imu`, run node MORB_SLAM/Stereo_Inertial. You will need to provide the vocabulary file and a settings file, including rectification matrices if required in a similar way to Stereo case: ``` - rosrun ORB_SLAM3 Stereo_Inertial PATH_TO_VOCABULARY PATH_TO_SETTINGS_FILE ONLINE_RECTIFICATION [EQUALIZATION] + rosrun MORB_SLAM Stereo_Inertial PATH_TO_VOCABULARY PATH_TO_SETTINGS_FILE ONLINE_RECTIFICATION [EQUALIZATION] ``` ### Running RGB_D Node -For an RGB-D input from topics `/camera/rgb/image_raw` and `/camera/depth_registered/image_raw`, run node ORB_SLAM3/RGBD. You will need to provide the vocabulary file and a settings file. See the RGB-D example above. +For an RGB-D input from topics `/camera/rgb/image_raw` and `/camera/depth_registered/image_raw`, run node MORB_SLAM/RGBD. You will need to provide the vocabulary file and a settings file. See the RGB-D example above. ``` - rosrun ORB_SLAM3 RGBD PATH_TO_VOCABULARY PATH_TO_SETTINGS_FILE + rosrun MORB_SLAM RGBD PATH_TO_VOCABULARY PATH_TO_SETTINGS_FILE ``` **Running ROS example:** Download a rosbag (e.g. V1_02_medium.bag) from the EuRoC dataset (http://projects.asl.ethz.ch/datasets/doku.php?id=kmavvisualinertialdatasets). Open 3 tabs on the terminal and run the following command at each tab for a Stereo-Inertial configuration: @@ -228,7 +228,7 @@ For an RGB-D input from topics `/camera/rgb/image_raw` and `/camera/depth_regist ``` ``` - rosrun ORB_SLAM3 Stereo_Inertial Vocabulary/ORBvoc.txt Examples/Stereo-Inertial/EuRoC.yaml true + rosrun MORB_SLAM Stereo_Inertial Vocabulary/ORBvoc.txt Examples/Stereo-Inertial/EuRoC.yaml true ``` ``` diff --git a/Thirdparty/DBoW2/DBoW2/TemplatedVocabulary.h b/Thirdparty/DBoW2/DBoW2/TemplatedVocabulary.h index 01959344..f1f95cd0 100644 --- a/Thirdparty/DBoW2/DBoW2/TemplatedVocabulary.h +++ b/Thirdparty/DBoW2/DBoW2/TemplatedVocabulary.h @@ -353,7 +353,7 @@ class TemplatedVocabulary * @param levelsup */ virtual void transform(const TDescriptor &feature, - WordId &id, WordValue &weight, NodeId* nid = NULL, int levelsup = 0) const; + WordId &id, WordValue &weight, NodeId* nid = nullptr, int levelsup = 0) const; /** * Returns the word id associated to a feature @@ -434,7 +434,7 @@ template TemplatedVocabulary::TemplatedVocabulary (int k, int L, WeightingType weighting, ScoringType scoring) : m_k(k), m_L(L), m_weighting(weighting), m_scoring(scoring), - m_scoring_object(NULL) + m_scoring_object(nullptr) { createScoringObject(); } @@ -443,7 +443,7 @@ TemplatedVocabulary::TemplatedVocabulary template TemplatedVocabulary::TemplatedVocabulary - (const std::string &filename): m_scoring_object(NULL) + (const std::string &filename): m_scoring_object(nullptr) { load(filename); } @@ -452,7 +452,7 @@ TemplatedVocabulary::TemplatedVocabulary template TemplatedVocabulary::TemplatedVocabulary - (const char *filename): m_scoring_object(NULL) + (const char *filename): m_scoring_object(nullptr) { load(filename); } @@ -463,7 +463,7 @@ template void TemplatedVocabulary::createScoringObject() { delete m_scoring_object; - m_scoring_object = NULL; + m_scoring_object = nullptr; switch(m_scoring) { @@ -516,7 +516,7 @@ void TemplatedVocabulary::setWeightingType(WeightingType type) template TemplatedVocabulary::TemplatedVocabulary( const TemplatedVocabulary &voc) - : m_scoring_object(NULL) + : m_scoring_object(nullptr) { *this = voc; } @@ -1224,7 +1224,7 @@ void TemplatedVocabulary::transform(const TDescriptor &feature, // level at which the node must be stored in nid, if given const int nid_level = m_L - levelsup; - if(nid_level <= 0 && nid != NULL) *nid = 0; // root + if(nid_level <= 0 && nid != nullptr) *nid = 0; // root NodeId final_id = 0; // root int current_level = 0; @@ -1248,7 +1248,7 @@ void TemplatedVocabulary::transform(const TDescriptor &feature, } } - if(nid != NULL && current_level == nid_level) + if(nid != nullptr && current_level == nid_level) *nid = final_id; } while( !m_nodes[final_id].isLeaf() ); diff --git a/Thirdparty/Sophus/test/ceres/test_ceres_se3.cpp b/Thirdparty/Sophus/test/ceres/test_ceres_se3.cpp index 6304b74d..45aee1ef 100644 --- a/Thirdparty/Sophus/test/ceres/test_ceres_se3.cpp +++ b/Thirdparty/Sophus/test/ceres/test_ceres_se3.cpp @@ -103,13 +103,13 @@ bool test(Sophus::SE3d const& T_w_targ, Sophus::SE3d const& T_w_init, new ceres::AutoDiffCostFunction( new TestSE3CostFunctor(T_w_targ.inverse())); - problem.AddResidualBlock(cost_function1, NULL, T_wr.data()); + problem.AddResidualBlock(cost_function1, nullptr, T_wr.data()); ceres::CostFunction* cost_function2 = new ceres::AutoDiffCostFunction( new TestPointCostFunctor(T_w_targ.inverse(), point_b)); - problem.AddResidualBlock(cost_function2, NULL, T_wr.data(), point_a.data()); + problem.AddResidualBlock(cost_function2, nullptr, T_wr.data(), point_a.data()); // Set solver options (precision / method) ceres::Solver::Options options; diff --git a/Thirdparty/g2o/g2o/core/estimate_propagator.cpp b/Thirdparty/g2o/g2o/core/estimate_propagator.cpp index 010dac1c..9f2a4189 100644 --- a/Thirdparty/g2o/g2o/core/estimate_propagator.cpp +++ b/Thirdparty/g2o/g2o/core/estimate_propagator.cpp @@ -207,7 +207,7 @@ namespace g2o { void EstimatePropagator::PriorityQueue::push(AdjacencyMapEntry* entry) { - assert(entry != NULL); + assert(entry != nullptr); if (entry->inQueue) { assert(entry->queueIt->second == entry); erase(entry->queueIt); @@ -225,7 +225,7 @@ namespace g2o { AdjacencyMapEntry* entry = it->second; erase(it); - assert(entry != NULL); + assert(entry != nullptr); entry->queueIt = end(); entry->inQueue = false; return entry; diff --git a/Thirdparty/g2o/g2o/core/optimizable_graph.h b/Thirdparty/g2o/g2o/core/optimizable_graph.h index 9d9b561c..82d83cda 100644 --- a/Thirdparty/g2o/g2o/core/optimizable_graph.h +++ b/Thirdparty/g2o/g2o/core/optimizable_graph.h @@ -411,7 +411,7 @@ namespace g2o { */ virtual bool setMeasurementFromState(); - //! if NOT NULL, error of this edge will be robustifed with the kernel + //! if NOT nullptr, error of this edge will be robustifed with the kernel RobustKernel* robustKernel() const { return _robustKernel;} /** * specify the robust kernel to be used in this edge diff --git a/Thirdparty/g2o/g2o/core/sparse_block_matrix.hpp b/Thirdparty/g2o/g2o/core/sparse_block_matrix.hpp index 8dfa99c1..f351ecc9 100644 --- a/Thirdparty/g2o/g2o/core/sparse_block_matrix.hpp +++ b/Thirdparty/g2o/g2o/core/sparse_block_matrix.hpp @@ -464,7 +464,7 @@ namespace g2o { template int SparseBlockMatrix::fillCCS(double* Cx, bool upperTriangle) const { - assert(Cx && "Target destination is NULL"); + assert(Cx && "Target destination is nullptr"); double* CxStart = Cx; for (size_t i=0; i<_blockCols.size(); ++i){ int cstart=i ? _colBlockIndices[i-1] : 0; @@ -489,7 +489,7 @@ namespace g2o { template int SparseBlockMatrix::fillCCS(int* Cp, int* Ci, double* Cx, bool upperTriangle) const { - assert(Cp && Ci && Cx && "Target destination is NULL"); + assert(Cp && Ci && Cx && "Target destination is nullptr"); int nz=0; for (size_t i=0; i<_blockCols.size(); ++i){ int cstart=i ? _colBlockIndices[i-1] : 0; diff --git a/Thirdparty/g2o/g2o/core/sparse_block_matrix_ccs.h b/Thirdparty/g2o/g2o/core/sparse_block_matrix_ccs.h index eb9042c2..f0c75fe4 100644 --- a/Thirdparty/g2o/g2o/core/sparse_block_matrix_ccs.h +++ b/Thirdparty/g2o/g2o/core/sparse_block_matrix_ccs.h @@ -142,7 +142,7 @@ namespace g2o { */ int fillCCS(int* Cp, int* Ci, double* Cx, bool upperTriangle = false) const { - assert(Cp && Ci && Cx && "Target destination is NULL"); + assert(Cp && Ci && Cx && "Target destination is nullptr"); int nz=0; for (size_t i=0; i<_blockCols.size(); ++i){ int cstart=i ? _colBlockIndices[i-1] : 0; @@ -175,7 +175,7 @@ namespace g2o { */ int fillCCS(double* Cx, bool upperTriangle = false) const { - assert(Cx && "Target destination is NULL"); + assert(Cx && "Target destination is nullptr"); double* CxStart = Cx; int cstart = 0; for (size_t i=0; i<_blockCols.size(); ++i){ diff --git a/Thirdparty/g2o/g2o/stuff/os_specific.c b/Thirdparty/g2o/g2o/stuff/os_specific.c index 85f93689..9a18673b 100644 --- a/Thirdparty/g2o/g2o/stuff/os_specific.c +++ b/Thirdparty/g2o/g2o/stuff/os_specific.c @@ -35,7 +35,7 @@ int vasprintf(char** strp, const char* fmt, va_list ap) char* p; char* np; - if ((p = (char*)malloc(size * sizeof(char))) == NULL) + if ((p = (char*)malloc(size * sizeof(char))) == nullptr) return -1; while (1) { @@ -52,7 +52,7 @@ int vasprintf(char** strp, const char* fmt, va_list ap) size = n+1; else size *= 2; - if ((np = (char*)realloc (p, size * sizeof(char))) == NULL) { + if ((np = (char*)realloc (p, size * sizeof(char))) == nullptr) { free(p); return -1; } else diff --git a/Thirdparty/g2o/g2o/stuff/string_tools.cpp b/Thirdparty/g2o/g2o/stuff/string_tools.cpp index 0a4f60a2..0bd8744c 100644 --- a/Thirdparty/g2o/g2o/stuff/string_tools.cpp +++ b/Thirdparty/g2o/g2o/stuff/string_tools.cpp @@ -94,7 +94,7 @@ std::string strToUpper(const std::string& s) std::string formatString(const char* fmt, ...) { - char* auxPtr = NULL; + char* auxPtr = nullptr; va_list arg_list; va_start(arg_list, fmt); int numChar = vasprintf(&auxPtr, fmt, arg_list); @@ -111,7 +111,7 @@ std::string formatString(const char* fmt, ...) int strPrintf(std::string& str, const char* fmt, ...) { - char* auxPtr = NULL; + char* auxPtr = nullptr; va_list arg_list; va_start(arg_list, fmt); int numChars = vasprintf(&auxPtr, fmt, arg_list); diff --git a/build.sh b/build.sh index f5c2b3f7..e95d3111 100755 --- a/build.sh +++ b/build.sh @@ -10,7 +10,7 @@ echo "Configuring and building Thirdparty/DBoW2 ..." cd Thirdparty/DBoW2 mkdir build 2> /dev/null cd build -cmake .. -DCMAKE_BUILD_TYPE=Release +cmake .. -DCMAKE_BUILD_TYPE=RelWithDebInfo make ${jobs} cd ../../g2o @@ -19,7 +19,7 @@ echo "Configuring and building Thirdparty/g2o ..." mkdir build 2> /dev/null cd build -cmake .. -DCMAKE_BUILD_TYPE=Release +cmake .. -DCMAKE_BUILD_TYPE=RelWithDebInfo make ${jobs} cd ../../Sophus @@ -28,7 +28,7 @@ echo "Configuring and building Thirdparty/Sophus ..." mkdir build 2> /dev/null cd build -cmake .. -DCMAKE_BUILD_TYPE=Release +cmake .. -DCMAKE_BUILD_TYPE=RelWithDebInfo make ${jobs} cd ../../../ @@ -41,7 +41,7 @@ if [ ! -f "ORBvoc.txt" ]; then fi cd .. -echo "Configuring and building ORB_SLAM3 ..." +echo "Configuring and building MORB_SLAM ..." mkdir build 2> /dev/null cd build diff --git a/build_ros.sh b/build_ros.sh index 1f13d215..886b5b1c 100755 --- a/build_ros.sh +++ b/build_ros.sh @@ -1,6 +1,6 @@ echo "Building ROS nodes" -cd Examples/ROS/ORB_SLAM3 +cd Examples/ROS/MORB_SLAM mkdir build cd build cmake .. -DROS_BUILD_TYPE=Release diff --git a/clean.sh b/clean.sh index c7a11891..d1b72f2d 100755 --- a/clean.sh +++ b/clean.sh @@ -40,18 +40,18 @@ rm Examples/RGB-D/rgbd_tum \ Examples/Stereo-Inertial/stereo_inertial_tum_vi \ Examples/Stereo-Inertial/stereo_inertial_realsense_t265 \ Examples/Stereo-Inertial/stereo_inertial_realsense_D435i \ - Examples/ROS/ORB_SLAM3/Mono \ - Examples/ROS/ORB_SLAM3/Mono_Inertial \ - Examples/ROS/ORB_SLAM3/MonoAR \ - Examples/ROS/ORB_SLAM3/RGBD \ - Examples/ROS/ORB_SLAM3/Stereo \ - Examples/ROS/ORB_SLAM3/Stereo_Inertial \ + Examples/ROS/MORB_SLAM/Mono \ + Examples/ROS/MORB_SLAM/Mono_Inertial \ + Examples/ROS/MORB_SLAM/MonoAR \ + Examples/ROS/MORB_SLAM/RGBD \ + Examples/ROS/MORB_SLAM/Stereo \ + Examples/ROS/MORB_SLAM/Stereo_Inertial \ Examples/Calibration/recorder_realsense_D435i \ Examples/Calibration/recorder_realsense_T265 \ Examples/Tests/viewer_dataset \ Examples/Tests/sophus_test \ 2> /dev/null -rm -r Examples/ROS/ORB_SLAM3/build 2> /dev/null +rm -r Examples/ROS/MORB_SLAM/build 2> /dev/null echo cleaning complete \ No newline at end of file diff --git a/include/Config.h b/include/Config.h deleted file mode 100644 index a3f6484a..00000000 --- a/include/Config.h +++ /dev/null @@ -1,54 +0,0 @@ -/** - * This file is part of ORB-SLAM3 - * - * Copyright (C) 2017-2021 Carlos Campos, Richard Elvira, Juan J. Gómez - * Rodríguez, José M.M. Montiel and Juan D. Tardós, University of Zaragoza. - * Copyright (C) 2014-2016 Raúl Mur-Artal, José M.M. Montiel and Juan D. Tardós, - * University of Zaragoza. - * - * ORB-SLAM3 is free software: you can redistribute it and/or modify it under - * the terms of the GNU General Public License as published by the Free Software - * Foundation, either version 3 of the License, or (at your option) any later - * version. - * - * ORB-SLAM3 is distributed in the hope that it will be useful, but WITHOUT ANY - * WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS FOR - * A PARTICULAR PURPOSE. See the GNU General Public License for more details. - * - * You should have received a copy of the GNU General Public License along with - * ORB-SLAM3. If not, see . - */ - -#ifndef CONFIG_H -#define CONFIG_H - -#include -#include -#include - -#include - -namespace ORB_SLAM3 { - -class ViewerConfig {}; - -class CameraConfig {}; - -class ORBExtractorConfig {}; - -class IMUConfig {}; - -class ConfigParser { - public: - bool ParseConfigFile(std::string &strConfigFile); - - private: - ViewerConfig mViewerConfig; - CameraConfig mCameraConfig; - ORBExtractorConfig mORBConfig; - IMUConfig mIMUConfig; -}; - -} // namespace ORB_SLAM3 - -#endif // CONFIG_H diff --git a/include/Atlas.h b/include/MORB_SLAM/Atlas.h similarity index 78% rename from include/Atlas.h rename to include/MORB_SLAM/Atlas.h index 65685e34..27cc9812 100644 --- a/include/Atlas.h +++ b/include/MORB_SLAM/Atlas.h @@ -23,17 +23,20 @@ #include #include +#include #include #include +#include +#include -#include "GeometricCamera.h" -#include "KannalaBrandt8.h" -#include "KeyFrame.h" -#include "Map.h" -#include "MapPoint.h" -#include "Pinhole.h" +#include "MORB_SLAM/CameraModels/GeometricCamera.h" +#include "MORB_SLAM/CameraModels/KannalaBrandt8.h" +#include "MORB_SLAM/KeyFrame.h" +#include "MORB_SLAM/Map.h" +#include "MORB_SLAM/MapPoint.h" +#include "MORB_SLAM/CameraModels/Pinhole.h" -namespace ORB_SLAM3 { +namespace MORB_SLAM { class Map; class MapPoint; class KeyFrame; @@ -75,7 +78,7 @@ class Atlas { ~Atlas(); void CreateNewMap(); - void ChangeMap(Map* pMap); + void ChangeMap(std::shared_ptr pMap); unsigned long int GetLastInitKFid(); @@ -85,8 +88,8 @@ class Atlas { // void EraseMapPoint(MapPoint* pMP); // void EraseKeyFrame(KeyFrame* pKF); - GeometricCamera* AddCamera(GeometricCamera* pCam); - std::vector GetAllCameras(); + std::shared_ptr AddCamera(const std::shared_ptr &pCam); + std::vector> GetAllCameras(); /* All methods without Map pointer work on current map */ void SetReferenceMapPoints(const std::vector& vpMPs); @@ -101,7 +104,7 @@ class Atlas { std::vector GetAllMapPoints(); std::vector GetReferenceMapPoints(); - vector GetAllMaps(); + std::vector> GetAllMaps(); int CountMaps(); @@ -109,9 +112,9 @@ class Atlas { void clearAtlas(); - Map* GetCurrentMap(System* sys = nullptr); + std::shared_ptr GetCurrentMap(System* sys = nullptr); - void SetMapBad(Map* pMap); + void SetMapBad(std::shared_ptr pMap); void RemoveBadMaps(); bool isInertial(); @@ -123,7 +126,7 @@ class Atlas { void PreSave(); void PostLoad(); - map GetAtlasKeyframes(); + std::map GetAtlasKeyframes(); void SetKeyFrameDababase(KeyFrameDatabase* pKFDB); KeyFrameDatabase* GetKeyFrameDatabase(); @@ -136,15 +139,15 @@ class Atlas { long unsigned int GetNumLivedMP(); protected: - std::set mspMaps; - std::set mspBadMaps; + 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; + std::vector> mvpBackupMaps; - Map* mpCurrentMap; + std::shared_ptr mpCurrentMap; - std::vector mvpCameras; + std::vector> mvpCameras; unsigned long int mnLastInitKFidMap; @@ -157,4 +160,4 @@ class Atlas { }; // class Atlas -} // namespace ORB_SLAM3 +} // namespace MORB_SLAM diff --git a/include/MORB_SLAM/Camera.hpp b/include/MORB_SLAM/Camera.hpp new file mode 100644 index 00000000..7d94ca1e --- /dev/null +++ b/include/MORB_SLAM/Camera.hpp @@ -0,0 +1,92 @@ +#pragma once +#include +#include +#include "ImprovedTypes.hpp" +#include +#include +#include +#include +#include +#include +#include +#include +#include + +namespace MORB_SLAM{ + + +template +class ManagedFuture{ +protected: + std::shared_ptr> promise; + std::shared_future future; + + ManagedFuture(): promise{std::make_shared>()}, future{promise->get_future()} {} // make default constructor protected +public: + virtual ~ManagedFuture(){} + + // functions from std::shared_future + + const T& get() const { return future.get(); } + void wait() const { future.wait(); } + + template + std::future_status wait_for( const std::chrono::duration& timeout_duration ) const { return future.wait_for(timeout_duration); } + + template + std::future_status wait_until( const std::chrono::time_point& timeout_time ) const { return future.wait_until(timeout_time); } +}; + +template +class ManagedPromise: public ManagedFuture{ + std::shared_ptr mutex; +public: + ManagedPromise(): mutex{std::make_shared()} {} + virtual ~ManagedPromise(){} + + // functions from std::promise + + void set_value_at_thread_exit(const T& value){ std::scoped_lock lock(*mutex); ManagedFuture::promise->set_value_at_thread_exit(value); } + void set_value_at_thread_exit(T&& value){ std::scoped_lock lock(*mutex); ManagedFuture::promise->set_value_at_thread_exit(value); } + void set_value_at_thread_exit(T& value){ std::scoped_lock lock(*mutex); ManagedFuture::promise->set_value_at_thread_exit(value); } + void set_value_at_thread_exit(){ std::scoped_lock lock(*mutex); ManagedFuture::promise->set_value_at_thread_exit(); } + void set_exception_at_thread_exit(const std::exception_ptr &p ){ std::scoped_lock lock(*mutex); ManagedFuture::promise->set_exception_at_thread_exit(p); } + void set_exception(const std::exception_ptr &p ){ std::scoped_lock lock(*mutex); ManagedFuture::promise->set_exception(p); } + void set_value(const T& value){ std::scoped_lock lock(*mutex); ManagedFuture::promise->set_value(value); } + void set_value(T&& value){ std::scoped_lock lock(*mutex); ManagedFuture::promise->set_value(value); } + void set_value(T& value){ std::scoped_lock lock(*mutex); ManagedFuture::promise->set_value(value); } + void set_value(){ std::scoped_lock lock(*mutex); ManagedFuture::promise->set_value(); } +}; + +class Camera{ + std::deque, std::function>> ljobs; + std::deque, std::function>> rjobs; + + std::string name; + CameraType::eSensor type; + std::mutex camMutex; + + std::condition_variable camCV; + + bool shouldStop; + + std::thread lthread; + std::thread rthread; + + void threadExec(std::deque, std::function>> *jobs); +public: + Camera(CameraType::eSensor type, const std::string &name="camera"); + virtual ~Camera(); + + ManagedFuture queue(std::function func, bool isLeft=true); + ManagedFuture queueLeft(const std::function &func); + ManagedFuture queueRight(const std::function &func); + + CameraType::eSensor getType() const; + const std::string &getName() const; +}; + +typedef std::shared_ptr Camera_ptr; +typedef std::weak_ptr Camera_wptr; + +} // namespace MORB_SLAM \ No newline at end of file diff --git a/include/CameraModels/GeometricCamera.h b/include/MORB_SLAM/CameraModels/GeometricCamera.h similarity index 83% rename from include/CameraModels/GeometricCamera.h rename to include/MORB_SLAM/CameraModels/GeometricCamera.h index 8b39f584..9fdde438 100644 --- a/include/CameraModels/GeometricCamera.h +++ b/include/MORB_SLAM/CameraModels/GeometricCamera.h @@ -19,8 +19,7 @@ * ORB-SLAM3. If not, see . */ -#ifndef CAMERAMODELS_GEOMETRICCAMERA_H -#define CAMERAMODELS_GEOMETRICCAMERA_H +#pragma once #include #include @@ -35,10 +34,10 @@ #include #include -#include "Converter.h" -#include "GeometricTools.h" +#include "MORB_SLAM/Converter.h" +#include "MORB_SLAM/GeometricTools.h" -namespace ORB_SLAM3 { +namespace MORB_SLAM { class GeometricCamera { friend class boost::serialization::access; @@ -55,15 +54,15 @@ class GeometricCamera { : mvParameters(_vParameters) {} ~GeometricCamera() {} - virtual cv::Point2f project(const cv::Point3f& p3D) = 0; - virtual Eigen::Vector2d project(const Eigen::Vector3d& v3D) = 0; - virtual Eigen::Vector2f project(const Eigen::Vector3f& v3D) = 0; - virtual Eigen::Vector2f projectMat(const cv::Point3f& p3D) = 0; + virtual cv::Point2f project(const cv::Point3f& p3D) const = 0; + virtual Eigen::Vector2d project(const Eigen::Vector3d& v3D) const = 0; + virtual Eigen::Vector2f project(const Eigen::Vector3f& v3D) const = 0; + virtual Eigen::Vector2f projectMat(const cv::Point3f& p3D) const = 0; virtual float uncertainty2(const Eigen::Matrix& p2D) = 0; - virtual Eigen::Vector3f unprojectEig(const cv::Point2f& p2D) = 0; - virtual cv::Point3f unproject(const cv::Point2f& p2D) = 0; + virtual Eigen::Vector3f unprojectEig(const cv::Point2f& p2D) const = 0; + virtual cv::Point3f unproject(const cv::Point2f& p2D) const = 0; virtual Eigen::Matrix projectJac( const Eigen::Vector3d& v3D) = 0; @@ -75,10 +74,10 @@ class GeometricCamera { std::vector& vP3D, std::vector& vbTriangulated) = 0; - virtual cv::Mat toK() = 0; - virtual Eigen::Matrix3f toK_() = 0; + virtual cv::Mat toK() const = 0; + virtual Eigen::Matrix3f toK_() const = 0; - virtual bool epipolarConstrain(GeometricCamera* otherCamera, + virtual bool epipolarConstrain(const std::shared_ptr &otherCamera, const cv::KeyPoint& kp1, const cv::KeyPoint& kp2, const Eigen::Matrix3f& R12, @@ -113,6 +112,4 @@ class GeometricCamera { unsigned int mnType; }; -} // namespace ORB_SLAM3 - -#endif // CAMERAMODELS_GEOMETRICCAMERA_H +} // namespace MORB_SLAM diff --git a/include/CameraModels/KannalaBrandt8.h b/include/MORB_SLAM/CameraModels/KannalaBrandt8.h similarity index 82% rename from include/CameraModels/KannalaBrandt8.h rename to include/MORB_SLAM/CameraModels/KannalaBrandt8.h index 8cb57b71..b77c68af 100644 --- a/include/CameraModels/KannalaBrandt8.h +++ b/include/MORB_SLAM/CameraModels/KannalaBrandt8.h @@ -19,15 +19,14 @@ * ORB-SLAM3. If not, see . */ -#ifndef CAMERAMODELS_KANNALABRANDT8_H -#define CAMERAMODELS_KANNALABRANDT8_H +#pragma once #include +#include +#include "MORB_SLAM/CameraModels/GeometricCamera.h" +#include "MORB_SLAM/TwoViewReconstruction.h" -#include "GeometricCamera.h" -#include "TwoViewReconstruction.h" - -namespace ORB_SLAM3 { +namespace MORB_SLAM { class KannalaBrandt8 : public GeometricCamera { friend class boost::serialization::access; @@ -71,15 +70,15 @@ class KannalaBrandt8 : public GeometricCamera { mnType = CAM_FISHEYE; } - cv::Point2f project(const cv::Point3f& p3D); - Eigen::Vector2d project(const Eigen::Vector3d& v3D); - Eigen::Vector2f project(const Eigen::Vector3f& v3D); - Eigen::Vector2f projectMat(const cv::Point3f& p3D); + cv::Point2f project(const cv::Point3f& p3D) const; + Eigen::Vector2d project(const Eigen::Vector3d& v3D) const; + Eigen::Vector2f project(const Eigen::Vector3f& v3D) const; + Eigen::Vector2f projectMat(const cv::Point3f& p3D) const; float uncertainty2(const Eigen::Matrix& p2D); - Eigen::Vector3f unprojectEig(const cv::Point2f& p2D); - cv::Point3f unproject(const cv::Point2f& p2D); + Eigen::Vector3f unprojectEig(const cv::Point2f& p2D) const; + cv::Point3f unproject(const cv::Point2f& p2D) const; Eigen::Matrix projectJac(const Eigen::Vector3d& v3D); @@ -90,15 +89,15 @@ class KannalaBrandt8 : public GeometricCamera { std::vector& vP3D, std::vector& vbTriangulated); - cv::Mat toK(); - Eigen::Matrix3f toK_(); + cv::Mat toK() const; + Eigen::Matrix3f toK_() const; - bool epipolarConstrain(GeometricCamera* pCamera2, const cv::KeyPoint& kp1, + bool epipolarConstrain(const std::shared_ptr &pCamera2, const cv::KeyPoint& kp1, const cv::KeyPoint& kp2, const Eigen::Matrix3f& R12, const Eigen::Vector3f& t12, const float sigmaLevel, const float unc); - float TriangulateMatches(GeometricCamera* pCamera2, const cv::KeyPoint& kp1, + float TriangulateMatches(const std::shared_ptr &pCamera2, const cv::KeyPoint& kp1, const cv::KeyPoint& kp2, const Eigen::Matrix3f& R12, const Eigen::Vector3f& t12, const float sigmaLevel, const float unc, Eigen::Vector3f& p3D); @@ -116,7 +115,7 @@ class KannalaBrandt8 : public GeometricCamera { float GetPrecision() { return precision; } - bool IsEqual(GeometricCamera* pCam); + bool IsEqual(const std::shared_ptr &pCam); private: const float precision; @@ -131,6 +130,5 @@ class KannalaBrandt8 : public GeometricCamera { const Eigen::Matrix& Tcw2, Eigen::Vector3f& x3D); }; -} // namespace ORB_SLAM3 +} // namespace MORB_SLAM -#endif // CAMERAMODELS_KANNALABRANDT8_H diff --git a/include/CameraModels/Pinhole.h b/include/MORB_SLAM/CameraModels/Pinhole.h similarity index 80% rename from include/CameraModels/Pinhole.h rename to include/MORB_SLAM/CameraModels/Pinhole.h index 9ae6b39f..f5533854 100644 --- a/include/CameraModels/Pinhole.h +++ b/include/MORB_SLAM/CameraModels/Pinhole.h @@ -19,15 +19,14 @@ * ORB-SLAM3. If not, see . */ -#ifndef CAMERAMODELS_PINHOLE_H -#define CAMERAMODELS_PINHOLE_H +#pragma once #include +#include +#include "MORB_SLAM/CameraModels/GeometricCamera.h" +#include "MORB_SLAM/TwoViewReconstruction.h" -#include "GeometricCamera.h" -#include "TwoViewReconstruction.h" - -namespace ORB_SLAM3 { +namespace MORB_SLAM { class Pinhole : public GeometricCamera { friend class boost::serialization::access; @@ -60,15 +59,15 @@ class Pinhole : public GeometricCamera { if (tvr) delete tvr; } - cv::Point2f project(const cv::Point3f& p3D); - Eigen::Vector2d project(const Eigen::Vector3d& v3D); - Eigen::Vector2f project(const Eigen::Vector3f& v3D); - Eigen::Vector2f projectMat(const cv::Point3f& p3D); + cv::Point2f project(const cv::Point3f& p3D) const; + Eigen::Vector2d project(const Eigen::Vector3d& v3D) const; + Eigen::Vector2f project(const Eigen::Vector3f& v3D) const; + Eigen::Vector2f projectMat(const cv::Point3f& p3D) const; float uncertainty2(const Eigen::Matrix& p2D); - Eigen::Vector3f unprojectEig(const cv::Point2f& p2D); - cv::Point3f unproject(const cv::Point2f& p2D); + Eigen::Vector3f unprojectEig(const cv::Point2f& p2D) const; + cv::Point3f unproject(const cv::Point2f& p2D) const; Eigen::Matrix projectJac(const Eigen::Vector3d& v3D); @@ -79,10 +78,10 @@ class Pinhole : public GeometricCamera { std::vector& vP3D, std::vector& vbTriangulated); - cv::Mat toK(); - Eigen::Matrix3f toK_(); + cv::Mat toK() const; + Eigen::Matrix3f toK_()const; - bool epipolarConstrain(GeometricCamera* pCamera2, const cv::KeyPoint& kp1, + bool epipolarConstrain(const std::shared_ptr &pCamera2, const cv::KeyPoint& kp1, const cv::KeyPoint& kp2, const Eigen::Matrix3f& R12, const Eigen::Vector3f& t12, const float sigmaLevel, const float unc); @@ -98,15 +97,14 @@ class Pinhole : public GeometricCamera { friend std::ostream& operator<<(std::ostream& os, const Pinhole& ph); friend std::istream& operator>>(std::istream& os, Pinhole& ph); - bool IsEqual(GeometricCamera* pCam); + bool IsEqual(const std::shared_ptr &pCam); private: // Parameters vector corresponds to // [fx, fy, cx, cy] TwoViewReconstruction* tvr; }; -} // namespace ORB_SLAM3 +} // namespace MORB_SLAM // BOOST_CLASS_EXPORT_KEY(ORBSLAM2::Pinhole) -#endif // CAMERAMODELS_PINHOLE_H diff --git a/include/Converter.h b/include/MORB_SLAM/Converter.h similarity index 96% rename from include/Converter.h rename to include/MORB_SLAM/Converter.h index 466af230..2f162d47 100644 --- a/include/Converter.h +++ b/include/MORB_SLAM/Converter.h @@ -19,8 +19,7 @@ * ORB-SLAM3. If not, see . */ -#ifndef CONVERTER_H -#define CONVERTER_H +#pragma once #include #include @@ -30,7 +29,7 @@ #include "sophus/geometry.hpp" #include "sophus/sim3.hpp" -namespace ORB_SLAM3 { +namespace MORB_SLAM { class Converter { public: @@ -76,6 +75,4 @@ class Converter { static Sophus::Sim3f toSophus(const g2o::Sim3 &S); }; -} // namespace ORB_SLAM3 - -#endif // CONVERTER_H +} // namespace MORB_SLAM diff --git a/include/Frame.h b/include/MORB_SLAM/Frame.h similarity index 81% rename from include/Frame.h rename to include/MORB_SLAM/Frame.h index 4beb90c4..bcc5127e 100644 --- a/include/Frame.h +++ b/include/MORB_SLAM/Frame.h @@ -17,8 +17,7 @@ */ -#ifndef FRAME_H -#define FRAME_H +#pragma once #include @@ -39,7 +38,9 @@ #include "Eigen/Core" #include "sophus/se3.hpp" -namespace ORB_SLAM3 +#include "MORB_SLAM/Camera.hpp" + +namespace MORB_SLAM { #define FRAME_GRID_ROWS 48 #define FRAME_GRID_COLS 64 @@ -59,19 +60,19 @@ class Frame Frame(const Frame &frame); // Constructor for stereo cameras. - 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,Frame* pPrevF = static_cast(NULL), const IMU::Calib &ImuCalib = IMU::Calib()); + Frame(const Camera_ptr &cam, const cv::Mat &imLeft, const cv::Mat &imRight, const double &timeStamp, const std::shared_ptr &extractorLeft, const std::shared_ptr &extractorRight, ORBVocabulary* voc, cv::Mat &K, cv::Mat &distCoef, const float &bf, const float &thDepth, const std::shared_ptr &pCamera, const std::string &pNameFile, int pnNumDataset, Frame* pPrevF = nullptr, const IMU::Calib &ImuCalib = IMU::Calib()); // Constructor for RGB-D cameras. - Frame(const cv::Mat &imGray, const cv::Mat &imDepth, const double &timeStamp, ORBextractor* extractor,ORBVocabulary* voc, cv::Mat &K, cv::Mat &distCoef, const float &bf, const float &thDepth, GeometricCamera* pCamera,Frame* pPrevF = static_cast(NULL), const IMU::Calib &ImuCalib = IMU::Calib()); + Frame(const Camera_ptr &cam, const cv::Mat &imGray, const cv::Mat &imDepth, const double &timeStamp, const std::shared_ptr &extractor,ORBVocabulary* voc, cv::Mat &K, cv::Mat &distCoef, const float &bf, const float &thDepth, const std::shared_ptr &pCamera, const std::string &pNameFile, int pnNumDataset, Frame* pPrevF = nullptr, const IMU::Calib &ImuCalib = IMU::Calib()); // 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()); + Frame(const Camera_ptr &cam, const cv::Mat &imGray, const double &timeStamp, const std::shared_ptr &extractor,ORBVocabulary* voc, const std::shared_ptr &pCamera, cv::Mat &distCoef, const float &bf, const float &thDepth, const std::string &pNameFile, int pnNumDataset, Frame* pPrevF = nullptr, const IMU::Calib &ImuCalib = IMU::Calib()); // Destructor - // ~Frame(); + virtual ~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); + void ExtractORB(bool isLeft, const cv::Mat &im, const int x0, const int x1); // Compute Bag of Words representation. void ComputeBoW(); @@ -121,7 +122,7 @@ class Frame // Backprojects a keypoint (if stereo/depth info available) into 3D world coordinates. bool UnprojectStereo(const int &i, Eigen::Vector3f &x3D); - ConstraintPoseImu* mpcpi; + std::shared_ptr mpcpi; bool imuIsPreintegrated(); void setIntegrated(); @@ -193,7 +194,9 @@ class Frame ORBVocabulary* mpORBvocabulary; // Feature extractor. The right is used only in the stereo case. - ORBextractor* mpORBextractorLeft, *mpORBextractorRight; + std::shared_ptr mpORBextractorLeft; + std::shared_ptr mpORBextractorRight; + // Frame timestamp. double mTimeStamp; @@ -241,7 +244,7 @@ class Frame // ORB descriptor, each row associated to a keypoint. cv::Mat mDescriptors, mDescriptorsRight; - // MapPoints associated to keypoints, NULL pointer if no association. + // MapPoints associated to keypoints, nullptr pointer if no association. // Flag to identify outlier associations. std::vector mvbOutlier; int mnCloseMPs; @@ -320,10 +323,11 @@ class Frame bool mbImuPreintegrated; - std::mutex *mpMutexImu; + std::shared_ptr mpMutexImu; public: - GeometricCamera* mpCamera, *mpCamera2; + Camera_ptr camera; + std::shared_ptr mpCamera, mpCamera2; //Number of KeyPoints extracted in the left and right images int Nleft, Nright; @@ -343,7 +347,7 @@ class Frame //Grid for the right image std::vector mGridRight[FRAME_GRID_COLS][FRAME_GRID_ROWS]; - 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, Sophus::SE3f& Tlr,Frame* pPrevF = static_cast(NULL), const IMU::Calib &ImuCalib = IMU::Calib()); + Frame(const Camera_ptr &cam, const cv::Mat &imLeft, const cv::Mat &imRight, const double &timeStamp, const std::shared_ptr &extractorLeft, const std::shared_ptr &extractorRight, ORBVocabulary* voc, cv::Mat &K, cv::Mat &distCoef, const float &bf, const float &thDepth, const std::shared_ptr &pCamera, const std::shared_ptr &pCamera2, const std::string &pNameFile, int pnNumDataset, Sophus::SE3f& Tlr,Frame* pPrevF = nullptr, const IMU::Calib &ImuCalib = IMU::Calib()); //Stereo fisheye void ComputeStereoFishEyeMatches(); @@ -370,5 +374,3 @@ class Frame }; }// namespace ORB_SLAM - -#endif // FRAME_H diff --git a/include/FrameDrawer.h b/include/MORB_SLAM/FrameDrawer.h similarity index 95% rename from include/FrameDrawer.h rename to include/MORB_SLAM/FrameDrawer.h index d5954762..f420c5ac 100644 --- a/include/FrameDrawer.h +++ b/include/MORB_SLAM/FrameDrawer.h @@ -24,9 +24,9 @@ #include #include #include -#include "ImprovedTypes.hpp" +#include "MORB_SLAM/ImprovedTypes.hpp" -namespace ORB_SLAM3 { +namespace MORB_SLAM { class MapPoint; class Viewer; @@ -68,4 +68,4 @@ class FrameDrawer { std::vector> mvTracks; }; -} // namespace ORB_SLAM3 +} // namespace MORB_SLAM diff --git a/include/G2oTypes.h b/include/MORB_SLAM/G2oTypes.h similarity index 98% rename from include/G2oTypes.h rename to include/MORB_SLAM/G2oTypes.h index 60d6cdb0..36633587 100644 --- a/include/G2oTypes.h +++ b/include/MORB_SLAM/G2oTypes.h @@ -16,8 +16,7 @@ * If not, see . */ -#ifndef G2OTYPES_H -#define G2OTYPES_H +#pragma once #include "g2o/core/base_vertex.h" #include "g2o/core/base_binary_edge.h" @@ -31,13 +30,13 @@ #include #include -#include -#include +#include +#include -#include"Converter.h" +#include"MORB_SLAM/Converter.h" #include -namespace ORB_SLAM3 +namespace MORB_SLAM { class KeyFrame; @@ -100,7 +99,7 @@ class ImuCamPose std::vector Rcb, Rbc; std::vector tcb, tbc; double bf; - std::vector pCamera; + std::vector> pCamera; // For posegraph 4DoF Eigen::Matrix3d Rwb0; @@ -732,7 +731,7 @@ class EdgePriorPoseImu : public g2o::BaseMultiEdge<15,Vector15d> { public: - EdgePriorPoseImu(ConstraintPoseImu* c); + EdgePriorPoseImu(std::shared_ptr c); virtual bool read(std::istream& is){return false;} virtual bool write(std::ostream& os) const{return false;} @@ -843,4 +842,3 @@ class Edge4DoF : public g2o::BaseBinaryEdge<6,Vector6d,VertexPose4DoF,VertexPose } //namespace ORB_SLAM2 -#endif // G2OTYPES_H diff --git a/include/GeometricTools.h b/include/MORB_SLAM/GeometricTools.h similarity index 95% rename from include/GeometricTools.h rename to include/MORB_SLAM/GeometricTools.h index d17d24f9..c1d888d0 100644 --- a/include/GeometricTools.h +++ b/include/MORB_SLAM/GeometricTools.h @@ -19,14 +19,13 @@ * ORB-SLAM3. If not, see . */ -#ifndef GEOMETRIC_TOOLS_H -#define GEOMETRIC_TOOLS_H +#pragma once #include #include #include -namespace ORB_SLAM3 { +namespace MORB_SLAM { class KeyFrame; @@ -78,6 +77,5 @@ class GeometricTools { } }; -} // namespace ORB_SLAM3 +} // namespace MORB_SLAM -#endif // GEOMETRIC_TOOLS_H diff --git a/include/ImprovedTypes.hpp b/include/MORB_SLAM/ImprovedTypes.hpp similarity index 77% rename from include/ImprovedTypes.hpp rename to include/MORB_SLAM/ImprovedTypes.hpp index 24010ce4..a66a18a9 100644 --- a/include/ImprovedTypes.hpp +++ b/include/MORB_SLAM/ImprovedTypes.hpp @@ -4,7 +4,7 @@ #include #include -namespace ORB_SLAM3{ +namespace MORB_SLAM{ class System; class Atlas; @@ -44,5 +44,8 @@ template using umap = std::unordered_map. */ -#ifndef IMUTYPES_H -#define IMUTYPES_H +#pragma once #include #include @@ -33,9 +32,9 @@ #include #include -#include "SerializationUtils.h" +#include "MORB_SLAM/SerializationUtils.h" -namespace ORB_SLAM3 { +namespace MORB_SLAM { namespace IMU { @@ -270,6 +269,4 @@ Eigen::Matrix3f NormalizeRotation(const Eigen::Matrix3f &R); } // namespace IMU -} // namespace ORB_SLAM3 - -#endif // IMUTYPES_H +} // namespace MORB_SLAM diff --git a/include/KeyFrame.h b/include/MORB_SLAM/KeyFrame.h similarity index 95% rename from include/KeyFrame.h rename to include/MORB_SLAM/KeyFrame.h index 3f0a4ff2..86436ad1 100644 --- a/include/KeyFrame.h +++ b/include/MORB_SLAM/KeyFrame.h @@ -19,8 +19,7 @@ * ORB-SLAM3. If not, see . */ -#ifndef KEYFRAME_H -#define KEYFRAME_H +#pragma once #include #include @@ -29,16 +28,16 @@ #include "DBoW2/BowVector.h" #include "DBoW2/FeatureVector.h" -#include "Frame.h" -#include "GeometricCamera.h" -#include "ImuTypes.h" -#include "KeyFrameDatabase.h" -#include "MapPoint.h" -#include "ORBVocabulary.h" -#include "ORBextractor.h" -#include "SerializationUtils.h" +#include "MORB_SLAM/Frame.h" +#include "MORB_SLAM/CameraModels/GeometricCamera.h" +#include "MORB_SLAM/ImuTypes.h" +#include "MORB_SLAM/KeyFrameDatabase.h" +#include "MORB_SLAM/MapPoint.h" +#include "MORB_SLAM/ORBVocabulary.h" +#include "MORB_SLAM/ORBextractor.h" +#include "MORB_SLAM/SerializationUtils.h" -namespace ORB_SLAM3 { +namespace MORB_SLAM { class Map; class MapPoint; @@ -191,7 +190,7 @@ class KeyFrame { public: KeyFrame(); - KeyFrame(Frame& F, Map* pMap, KeyFrameDatabase* pKFDB); + KeyFrame(Frame& F, std::shared_ptr pMap, KeyFrameDatabase* pKFDB); // Pose functions void SetPose(const Sophus::SE3f& Tcw); @@ -279,8 +278,8 @@ class KeyFrame { return pKF1->mnId < pKF2->mnId; } - Map* GetMap(); - void UpdateMap(Map* pMap); + std::shared_ptr GetMap(); + void UpdateMap(std::shared_ptr pMap); void SetNewBias(const IMU::Bias& b); Eigen::Vector3f GetGyroBias(); @@ -294,10 +293,10 @@ class KeyFrame { float& v); void PreSave(set& spKF, set& spMP, - set& spCam); + set>& spCam); void PostLoad(map& mpKFid, map& mpMPid, - map& mpCamId); + map>& mpCamId); void SetORBVocabulary(ORBVocabulary* pORBVoc); void SetKeyFrameDatabase(KeyFrameDatabase* pKFDB); @@ -481,7 +480,7 @@ class KeyFrame { float mHalfBaseline; // Only for visualization - Map* mpMap; + std::shared_ptr mpMap; // Backup variables for inertial long long int mBackupPrevKFId; @@ -501,7 +500,7 @@ class KeyFrame { std::mutex mMutexMap; public: - GeometricCamera *mpCamera, *mpCamera2; + std::shared_ptr mpCamera, mpCamera2; // Indexes of stereo observations correspondences std::vector mvLeftToRightMatch, mvRightToLeftMatch; @@ -539,6 +538,5 @@ class KeyFrame { } }; -} // namespace ORB_SLAM3 +} // namespace MORB_SLAM -#endif // KEYFRAME_H diff --git a/include/KeyFrameDatabase.h b/include/MORB_SLAM/KeyFrameDatabase.h similarity index 91% rename from include/KeyFrameDatabase.h rename to include/MORB_SLAM/KeyFrameDatabase.h index d6a1ec98..e6892748 100644 --- a/include/KeyFrameDatabase.h +++ b/include/MORB_SLAM/KeyFrameDatabase.h @@ -19,8 +19,7 @@ * ORB-SLAM3. If not, see . */ -#ifndef KEYFRAMEDATABASE_H -#define KEYFRAMEDATABASE_H +#pragma once #include #include @@ -30,12 +29,12 @@ #include #include -#include "Frame.h" -#include "KeyFrame.h" -#include "Map.h" -#include "ORBVocabulary.h" +#include "MORB_SLAM/Frame.h" +#include "MORB_SLAM/KeyFrame.h" +#include "MORB_SLAM/Map.h" +#include "MORB_SLAM/ORBVocabulary.h" -namespace ORB_SLAM3 { +namespace MORB_SLAM { class KeyFrame; class Frame; @@ -60,7 +59,7 @@ class KeyFrameDatabase { void erase(KeyFrame* pKF); void clear(); - void clearMap(Map* pMap); + void clearMap(std::shared_ptr pMap); // Loop Detection(DEPRECATED) std::vector DetectLoopCandidates(KeyFrame* pKF, float minScore); @@ -76,7 +75,7 @@ class KeyFrameDatabase { int nNumCandidates); // Relocalization - std::vector DetectRelocalizationCandidates(Frame* F, Map* pMap); + std::vector DetectRelocalizationCandidates(Frame* F, std::shared_ptr pMap); void PreSave(); void PostLoad(map mpKFid); @@ -96,6 +95,5 @@ class KeyFrameDatabase { std::mutex mMutex; }; -} // namespace ORB_SLAM3 +} // namespace MORB_SLAM -#endif diff --git a/include/LocalMapping.h b/include/MORB_SLAM/LocalMapping.h similarity index 91% rename from include/LocalMapping.h rename to include/MORB_SLAM/LocalMapping.h index 06e7c37e..69ea75dd 100644 --- a/include/LocalMapping.h +++ b/include/MORB_SLAM/LocalMapping.h @@ -23,15 +23,15 @@ #include -#include "ImprovedTypes.hpp" -#include "Atlas.h" -#include "KeyFrame.h" -#include "KeyFrameDatabase.h" -#include "LoopClosing.h" -#include "Settings.h" -#include "Tracking.h" +#include "MORB_SLAM/ImprovedTypes.hpp" +#include "MORB_SLAM/Atlas.h" +#include "MORB_SLAM/KeyFrame.h" +#include "MORB_SLAM/KeyFrameDatabase.h" +#include "MORB_SLAM/LoopClosing.h" +#include "MORB_SLAM/Settings.h" +#include "MORB_SLAM/Tracking.h" -namespace ORB_SLAM3 { +namespace MORB_SLAM { class System; class Tracking; @@ -57,7 +57,7 @@ class LocalMapping { // Thread Synch void RequestStop(); void RequestReset(); - void RequestResetActiveMap(Map* pMap); + void RequestResetActiveMap(std::shared_ptr pMap); bool Stop(); void Release(); bool isStopped(); @@ -144,7 +144,7 @@ class LocalMapping { void ResetIfRequested(); bool mbResetRequested; bool mbResetRequestedActiveMap; - Map* mpMapToReset; + std::shared_ptr mpMapToReset; std::mutex mMutexReset; bool CheckFinish(); @@ -194,5 +194,5 @@ class LocalMapping { ofstream f_lm; }; -} // namespace ORB_SLAM3 +} // namespace MORB_SLAM diff --git a/include/LoopClosing.h b/include/MORB_SLAM/LoopClosing.h similarity index 93% rename from include/LoopClosing.h rename to include/MORB_SLAM/LoopClosing.h index 6d987403..64039497 100644 --- a/include/LoopClosing.h +++ b/include/MORB_SLAM/LoopClosing.h @@ -19,12 +19,12 @@ #pragma once -#include "ImprovedTypes.hpp" -#include "KeyFrame.h" -#include "LocalMapping.h" -#include "Atlas.h" -#include "ORBVocabulary.h" -#include "Tracking.h" +#include "MORB_SLAM/ImprovedTypes.hpp" +#include "MORB_SLAM/KeyFrame.h" +#include "MORB_SLAM/LocalMapping.h" +#include "MORB_SLAM/Atlas.h" +#include "MORB_SLAM/ORBVocabulary.h" +#include "MORB_SLAM/Tracking.h" #include "KeyFrameDatabase.h" @@ -33,7 +33,7 @@ #include #include "g2o/types/types_seven_dof_expmap.h" -namespace ORB_SLAM3 +namespace MORB_SLAM { class Tracking; @@ -64,10 +64,10 @@ class LoopClosing void InsertKeyFrame(KeyFrame *pKF); void RequestReset(); - void RequestResetActiveMap(Map* pMap); + void RequestResetActiveMap(std::shared_ptr pMap); // This function will run in a separate thread - void RunGlobalBundleAdjustment(Map* pActiveMap, unsigned long nLoopKF); + void RunGlobalBundleAdjustment(std::shared_ptr pActiveMap, unsigned long nLoopKF); bool isRunningGBA(){ unique_lock lock(mMutexGBA); @@ -145,7 +145,7 @@ class LoopClosing void ResetIfRequested(); bool mbResetRequested; bool mbResetActiveMapRequested; - Map* mpMapToReset; + std::shared_ptr mpMapToReset; std::mutex mMutexReset; bool CheckFinish(); @@ -182,7 +182,7 @@ class LoopClosing g2o::Sim3 mg2oScw; //------- - Map* mpLastMap; + std::shared_ptr mpLastMap; bool mbLoopDetected; int mnLoopNumCoincidences; diff --git a/include/MLPnPsolver.h b/include/MORB_SLAM/MLPnPsolver.h similarity index 97% rename from include/MLPnPsolver.h rename to include/MORB_SLAM/MLPnPsolver.h index bdeed215..f13d65b4 100644 --- a/include/MLPnPsolver.h +++ b/include/MORB_SLAM/MLPnPsolver.h @@ -49,16 +49,15 @@ * SUCH DAMAGE. * ******************************************************************************/ -#ifndef ORB_SLAM3_MLPNPSOLVER_H -#define ORB_SLAM3_MLPNPSOLVER_H +#pragma once #include #include -#include "Frame.h" -#include "MapPoint.h" +#include "MORB_SLAM/Frame.h" +#include "MORB_SLAM/MapPoint.h" -namespace ORB_SLAM3 { +namespace MORB_SLAM { class MLPnPsolver { public: @@ -238,9 +237,7 @@ class MLPnPsolver { // th*th*sigma(level)*sigma(level) vector mvMaxError; - GeometricCamera* mpCamera; + std::shared_ptr mpCamera; }; -} // namespace ORB_SLAM3 - -#endif // ORB_SLAM3_MLPNPSOLVER_H +} // namespace MORB_SLAM diff --git a/include/Map.h b/include/MORB_SLAM/Map.h similarity index 93% rename from include/Map.h rename to include/MORB_SLAM/Map.h index e1d20d4f..2431758c 100644 --- a/include/Map.h +++ b/include/MORB_SLAM/Map.h @@ -17,20 +17,20 @@ */ -#ifndef MAP_H -#define MAP_H +#pragma once -#include "MapPoint.h" -#include "KeyFrame.h" +#include "MORB_SLAM/MapPoint.h" +#include "MORB_SLAM/KeyFrame.h" #include #include #include - +#include #include +#include -namespace ORB_SLAM3 +namespace MORB_SLAM { class MapPoint; @@ -130,8 +130,8 @@ class Map unsigned int GetLowerKFID(); - void PreSave(std::set &spCams); - void PostLoad(KeyFrameDatabase* pKFDB, ORBVocabulary* pORBVoc/*, map& mpKeyFrameId*/, map &mpCams); + void PreSave(std::set> &spCams, std::shared_ptr sharedMap); + void PostLoad(KeyFrameDatabase* pKFDB, ORBVocabulary* pORBVoc/*, map& mpKeyFrameId*/, map> &mpCams, std::shared_ptr sharedMap); void printReprojectionError(list &lpLocalWindowKFs, KeyFrame* mpCurrentKF, string &name, string &name_folder); @@ -202,6 +202,4 @@ class Map }; -} //namespace ORB_SLAM3 - -#endif // MAP_H +} //namespace MORB_SLAM diff --git a/include/MapDrawer.h b/include/MORB_SLAM/MapDrawer.h similarity index 96% rename from include/MapDrawer.h rename to include/MORB_SLAM/MapDrawer.h index 03f78f77..16e3b5e9 100644 --- a/include/MapDrawer.h +++ b/include/MORB_SLAM/MapDrawer.h @@ -19,13 +19,13 @@ #pragma once -#include "ImprovedTypes.hpp" -#include "Settings.h" +#include "MORB_SLAM/ImprovedTypes.hpp" +#include "MORB_SLAM/Settings.h" #include #include -namespace ORB_SLAM3 +namespace MORB_SLAM { class Settings; diff --git a/include/MapPoint.h b/include/MORB_SLAM/MapPoint.h similarity index 92% rename from include/MapPoint.h rename to include/MORB_SLAM/MapPoint.h index 3e714088..ef7ba25f 100644 --- a/include/MapPoint.h +++ b/include/MORB_SLAM/MapPoint.h @@ -17,15 +17,14 @@ */ -#ifndef MAPPOINT_H -#define MAPPOINT_H +#pragma once -#include "KeyFrame.h" -#include "Frame.h" -#include "Map.h" -#include "Converter.h" +#include "MORB_SLAM/KeyFrame.h" +#include "MORB_SLAM/Frame.h" +#include "MORB_SLAM/Map.h" +#include "MORB_SLAM/Converter.h" -#include "SerializationUtils.h" +#include "MORB_SLAM/SerializationUtils.h" #include #include @@ -34,7 +33,7 @@ #include #include -namespace ORB_SLAM3 +namespace MORB_SLAM { class KeyFrame; @@ -107,9 +106,9 @@ class MapPoint MapPoint(); - MapPoint(const Eigen::Vector3f &Pos, KeyFrame* pRefKF, Map* pMap); - MapPoint(const double invDepth, cv::Point2f uv_init, KeyFrame* pRefKF, KeyFrame* pHostKF, Map* pMap); - MapPoint(const Eigen::Vector3f &Pos, Map* pMap, Frame* pFrame, const int &idxF); + MapPoint(const Eigen::Vector3f &Pos, KeyFrame* pRefKF, std::shared_ptr pMap); + MapPoint(const double invDepth, cv::Point2f uv_init, KeyFrame* pRefKF, KeyFrame* pHostKF, std::shared_ptr pMap); + MapPoint(const Eigen::Vector3f &Pos, std::shared_ptr pMap, Frame* pFrame, const int &idxF); void SetWorldPos(const Eigen::Vector3f &Pos); Eigen::Vector3f GetWorldPos(); @@ -152,8 +151,8 @@ class MapPoint int PredictScale(const float ¤tDist, KeyFrame*pKF); int PredictScale(const float ¤tDist, Frame* pF); - Map* GetMap(); - void UpdateMap(Map* pMap); + std::shared_ptr GetMap(); + void UpdateMap(std::shared_ptr pMap); void PrintObservations(); @@ -242,7 +241,7 @@ class MapPoint float mfMinDistance; float mfMaxDistance; - Map* mpMap; + std::shared_ptr mpMap; // Mutex std::mutex mMutexPos; @@ -252,5 +251,3 @@ class MapPoint }; } //namespace ORB_SLAM - -#endif // MAPPOINT_H diff --git a/include/ORBVocabulary.h b/include/MORB_SLAM/ORBVocabulary.h similarity index 89% rename from include/ORBVocabulary.h rename to include/MORB_SLAM/ORBVocabulary.h index cf48d179..0558266d 100644 --- a/include/ORBVocabulary.h +++ b/include/MORB_SLAM/ORBVocabulary.h @@ -19,17 +19,14 @@ * ORB-SLAM3. If not, see . */ -#ifndef ORBVOCABULARY_H -#define ORBVOCABULARY_H +#pragma once #include "DBoW2/FORB.h" #include "DBoW2/TemplatedVocabulary.h" -namespace ORB_SLAM3 { +namespace MORB_SLAM { typedef DBoW2::TemplatedVocabulary ORBVocabulary; -} // namespace ORB_SLAM3 - -#endif // ORBVOCABULARY_H +} // namespace MORB_SLAM diff --git a/include/ORBextractor.h b/include/MORB_SLAM/ORBextractor.h similarity index 91% rename from include/ORBextractor.h rename to include/MORB_SLAM/ORBextractor.h index aa08b9b0..fafa90a9 100644 --- a/include/ORBextractor.h +++ b/include/MORB_SLAM/ORBextractor.h @@ -19,18 +19,19 @@ * ORB-SLAM3. If not, see . */ -#ifndef ORBEXTRACTOR_H -#define ORBEXTRACTOR_H +#pragma once #include #include #include -namespace ORB_SLAM3 { +namespace MORB_SLAM { class ExtractorNode { public: ExtractorNode() : bNoMore(false) {} + ExtractorNode(const cv::Point2i &UL, const cv::Point2i &UR, const cv::Point2i &BL, const cv::Point2i &BR, int keySize) : + UL{UL}, UR{UR}, BL{BL}, BR{BR}, bNoMore(false) { vKeys.reserve(keySize); } void DivideNode(ExtractorNode &n1, ExtractorNode &n2, ExtractorNode &n3, ExtractorNode &n4); @@ -43,13 +44,9 @@ class ExtractorNode { class ORBextractor { public: - enum { HARRIS_SCORE = 0, FAST_SCORE = 1 }; - ORBextractor(int nfeatures, float scaleFactor, int nlevels, int iniThFAST, int minThFAST); - ~ORBextractor() {} - // Compute the ORB features and descriptors on an image. // ORB are dispersed on the image using an octree. // Mask is ignored in the current implementation. @@ -84,8 +81,6 @@ class ORBextractor { const int &maxX, const int &minY, const int &maxY, const int &nFeatures, const int &level); - void ComputeKeyPointsOld( - std::vector > &allKeypoints); std::vector pattern; int nfeatures; @@ -104,6 +99,4 @@ class ORBextractor { std::vector mvInvLevelSigma2; }; -} // namespace ORB_SLAM3 - -#endif +} // namespace MORB_SLAM diff --git a/include/ORBmatcher.h b/include/MORB_SLAM/ORBmatcher.h similarity index 96% rename from include/ORBmatcher.h rename to include/MORB_SLAM/ORBmatcher.h index 5c6f3db4..f9c69ab6 100644 --- a/include/ORBmatcher.h +++ b/include/MORB_SLAM/ORBmatcher.h @@ -19,19 +19,18 @@ * ORB-SLAM3. If not, see . */ -#ifndef ORBMATCHER_H -#define ORBMATCHER_H +#pragma once #include #include #include -#include "Frame.h" -#include "KeyFrame.h" -#include "MapPoint.h" +#include "MORB_SLAM/Frame.h" +#include "MORB_SLAM/KeyFrame.h" +#include "MORB_SLAM/MapPoint.h" #include "sophus/sim3.hpp" -namespace ORB_SLAM3 { +namespace MORB_SLAM { class ORBmatcher { public: @@ -128,6 +127,5 @@ class ORBmatcher { bool mbCheckOrientation; }; -} // namespace ORB_SLAM3 +} // namespace MORB_SLAM -#endif // ORBMATCHER_H \ No newline at end of file diff --git a/include/OptimizableTypes.h b/include/MORB_SLAM/OptimizableTypes.h similarity index 90% rename from include/OptimizableTypes.h rename to include/MORB_SLAM/OptimizableTypes.h index a2d3bcbf..1431c149 100644 --- a/include/OptimizableTypes.h +++ b/include/MORB_SLAM/OptimizableTypes.h @@ -19,10 +19,9 @@ * ORB-SLAM3. If not, see . */ -#ifndef ORB_SLAM3_OPTIMIZABLETYPES_H -#define ORB_SLAM3_OPTIMIZABLETYPES_H +#pragma once -#include +#include #include #include @@ -30,7 +29,7 @@ #include "g2o/core/base_unary_edge.h" -namespace ORB_SLAM3 { +namespace MORB_SLAM { class EdgeSE3ProjectXYZOnlyPose : public g2o::BaseUnaryEdge<2, Eigen::Vector2d, g2o::VertexSE3Expmap> { public: @@ -58,7 +57,7 @@ class EdgeSE3ProjectXYZOnlyPose virtual void linearizeOplus(); Eigen::Vector3d Xw; - GeometricCamera* pCamera; + std::shared_ptr pCamera; }; class EdgeSE3ProjectXYZOnlyPoseToBody @@ -88,7 +87,7 @@ class EdgeSE3ProjectXYZOnlyPoseToBody virtual void linearizeOplus(); Eigen::Vector3d Xw; - GeometricCamera* pCamera; + std::shared_ptr pCamera; g2o::SE3Quat mTrl; }; @@ -124,7 +123,7 @@ class EdgeSE3ProjectXYZ virtual void linearizeOplus(); - GeometricCamera* pCamera; + std::shared_ptr pCamera; }; class EdgeSE3ProjectXYZToBody @@ -159,7 +158,7 @@ class EdgeSE3ProjectXYZToBody virtual void linearizeOplus(); - GeometricCamera* pCamera; + std::shared_ptr pCamera; g2o::SE3Quat mTrl; }; @@ -181,14 +180,14 @@ class VertexSim3Expmap : public g2o::BaseVertex<7, g2o::Sim3> { setEstimate(s * estimate()); } - GeometricCamera *pCamera1, *pCamera2; + std::shared_ptr pCamera1, pCamera2; bool _fix_scale; }; class EdgeSim3ProjectXYZ : public g2o::BaseBinaryEdge<2, Eigen::Vector2d, g2o::VertexSBAPointXYZ, - ORB_SLAM3::VertexSim3Expmap> { + MORB_SLAM::VertexSim3Expmap> { public: EdgeSim3ProjectXYZ(); @@ -196,8 +195,8 @@ class EdgeSim3ProjectXYZ virtual bool write(std::ostream& os) const; void computeError() { - const ORB_SLAM3::VertexSim3Expmap* v1 = - static_cast(_vertices[1]); + const MORB_SLAM::VertexSim3Expmap* v1 = + static_cast(_vertices[1]); const g2o::VertexSBAPointXYZ* v2 = static_cast(_vertices[0]); @@ -218,8 +217,8 @@ class EdgeInverseSim3ProjectXYZ virtual bool write(std::ostream& os) const; void computeError() { - const ORB_SLAM3::VertexSim3Expmap* v1 = - static_cast(_vertices[1]); + const MORB_SLAM::VertexSim3Expmap* v1 = + static_cast(_vertices[1]); const g2o::VertexSBAPointXYZ* v2 = static_cast(_vertices[0]); @@ -231,6 +230,4 @@ class EdgeInverseSim3ProjectXYZ // virtual void linearizeOplus(); }; -} // namespace ORB_SLAM3 - -#endif // ORB_SLAM3_OPTIMIZABLETYPES_H +} // namespace MORB_SLAM diff --git a/include/Optimizer.h b/include/MORB_SLAM/Optimizer.h similarity index 81% rename from include/Optimizer.h rename to include/MORB_SLAM/Optimizer.h index 2ccd77c0..5bdcd6c3 100644 --- a/include/Optimizer.h +++ b/include/MORB_SLAM/Optimizer.h @@ -19,16 +19,15 @@ * ORB-SLAM3. If not, see . */ -#ifndef OPTIMIZER_H -#define OPTIMIZER_H +#pragma once #include -#include "Frame.h" -#include "KeyFrame.h" -#include "LoopClosing.h" -#include "Map.h" -#include "MapPoint.h" +#include "MORB_SLAM/Frame.h" +#include "MORB_SLAM/KeyFrame.h" +#include "MORB_SLAM/LoopClosing.h" +#include "MORB_SLAM/Map.h" +#include "MORB_SLAM/MapPoint.h" #include "g2o/core/block_solver.h" #include "g2o/core/optimization_algorithm_gauss_newton.h" #include "g2o/core/optimization_algorithm_levenberg.h" @@ -39,7 +38,7 @@ #include "g2o/types/types_seven_dof_expmap.h" #include "g2o/types/types_six_dof_expmap.h" -namespace ORB_SLAM3 { +namespace MORB_SLAM { class LoopClosing; @@ -47,21 +46,21 @@ class Optimizer { public: void static BundleAdjustment(const std::vector &vpKF, const std::vector &vpMP, - int nIterations = 5, bool *pbStopFlag = NULL, + int nIterations = 5, bool *pbStopFlag = nullptr, const unsigned long nLoopKF = 0, const bool bRobust = true); - void static GlobalBundleAdjustemnt(Map *pMap, int nIterations = 5, - bool *pbStopFlag = NULL, + void static GlobalBundleAdjustemnt(std::shared_ptr pMap, int nIterations = 5, + bool *pbStopFlag = nullptr, const unsigned long nLoopKF = 0, const bool bRobust = true); - void static FullInertialBA(Map *pMap, int its, const bool bFixLocal = false, + void static FullInertialBA(std::shared_ptr pMap, int its, const bool bFixLocal = false, const unsigned long nLoopKF = 0, - bool *pbStopFlag = NULL, bool bInit = false, + bool *pbStopFlag = nullptr, bool bInit = false, float priorG = 1e2, float priorA = 1e6, - Eigen::VectorXd *vSingVal = NULL, - bool *bHess = NULL); + Eigen::VectorXd *vSingVal = nullptr, + bool *bHess = nullptr); - void static LocalBundleAdjustment(KeyFrame *pKF, bool *pbStopFlag, Map *pMap, + void static LocalBundleAdjustment(KeyFrame *pKF, bool *pbStopFlag, std::shared_ptr pMap, int &num_fixedKF, int &num_OptKF, int &num_MPs, int &num_edges); @@ -74,7 +73,7 @@ class Optimizer { // if bFixScale is true, 6DoF optimization (stereo,rgbd), 7DoF otherwise // (mono) void static OptimizeEssentialGraph( - Map *pMap, KeyFrame *pLoopKF, KeyFrame *pCurKF, + std::shared_ptr pMap, KeyFrame *pLoopKF, KeyFrame *pCurKF, const LoopClosing::KeyFrameAndPose &NonCorrectedSim3, const LoopClosing::KeyFrameAndPose &CorrectedSim3, const map > &LoopConnections, @@ -87,7 +86,7 @@ class Optimizer { // For inertial loopclosing void static OptimizeEssentialGraph4DoF( - Map *pMap, KeyFrame *pLoopKF, KeyFrame *pCurKF, + std::shared_ptr pMap, KeyFrame *pLoopKF, KeyFrame *pCurKF, const LoopClosing::KeyFrameAndPose &NonCorrectedSim3, const LoopClosing::KeyFrameAndPose &CorrectedSim3, const map > &LoopConnections); @@ -103,12 +102,12 @@ class Optimizer { // For inertial systems - void static LocalInertialBA(KeyFrame *pKF, bool *pbStopFlag, Map *pMap, + void static LocalInertialBA(KeyFrame *pKF, bool *pbStopFlag, std::shared_ptr 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, + bool *pbStopFlag, std::shared_ptr pMap, LoopClosing::KeyFrameAndPose &corrPoses); // Local BA in welding area when two maps are merged @@ -123,21 +122,20 @@ class Optimizer { const int &end); // Inertial pose-graph - void static InertialOptimization(Map *pMap, Eigen::Matrix3d &Rwg, + void static InertialOptimization(std::shared_ptr pMap, Eigen::Matrix3d &Rwg, double &scale, Eigen::Vector3d &bg, Eigen::Vector3d &ba, bool bMono, Eigen::MatrixXd &covInertial, bool bFixedVel = false, bool bGauss = false, float priorG = 1e2, float priorA = 1e6); - void static InertialOptimization(Map *pMap, Eigen::Vector3d &bg, + void static InertialOptimization(std::shared_ptr pMap, Eigen::Vector3d &bg, Eigen::Vector3d &ba, float priorG = 1e2, float priorA = 1e6); - void static InertialOptimization(Map *pMap, Eigen::Matrix3d &Rwg, + void static InertialOptimization(std::shared_ptr pMap, Eigen::Matrix3d &Rwg, double &scale); ; }; -} // namespace ORB_SLAM3 +} // namespace MORB_SLAM -#endif // OPTIMIZER_H diff --git a/include/SerializationUtils.h b/include/MORB_SLAM/SerializationUtils.h similarity index 96% rename from include/SerializationUtils.h rename to include/MORB_SLAM/SerializationUtils.h index c43fcbcb..0f12eca3 100644 --- a/include/SerializationUtils.h +++ b/include/MORB_SLAM/SerializationUtils.h @@ -19,8 +19,7 @@ * ORB-SLAM3. If not, see . */ -#ifndef SERIALIZATION_UTILS_H -#define SERIALIZATION_UTILS_H +#pragma once #include #include @@ -30,7 +29,7 @@ #include #include -namespace ORB_SLAM3 { +namespace MORB_SLAM { template void serializeSophusSE3(Archive& ar, Sophus::SE3f& T, @@ -151,6 +150,5 @@ void serializeVectorKeyPoints(Archive& ar, const std::vector& vKP, } } -} // namespace ORB_SLAM3 +} // namespace MORB_SLAM -#endif // SERIALIZATION_UTILS_H diff --git a/include/Settings.h b/include/MORB_SLAM/Settings.h similarity index 94% rename from include/Settings.h rename to include/MORB_SLAM/Settings.h index 252c21d2..59672896 100644 --- a/include/Settings.h +++ b/include/MORB_SLAM/Settings.h @@ -19,8 +19,7 @@ * ORB-SLAM3. If not, see . */ -#ifndef ORB_SLAM3_SETTINGS_H -#define ORB_SLAM3_SETTINGS_H +#pragma once // Flag to activate the measurement of time in each process (track,localmap, // place recognition). @@ -31,10 +30,11 @@ #include #include +#include -#include "CameraModels/GeometricCamera.h" +#include "MORB_SLAM/CameraModels/GeometricCamera.h" -namespace ORB_SLAM3 { +namespace MORB_SLAM { class System; @@ -66,8 +66,8 @@ class Settings { * Getter methods */ CameraType cameraType() const { return cameraType_; } - GeometricCamera* camera1() { return calibration1_; } - GeometricCamera* camera2() { return calibration2_; } + std::shared_ptr camera1() { return calibration1_; } + std::shared_ptr camera2() { return calibration2_; } cv::Mat camera1DistortionCoef() { return cv::Mat(vPinHoleDistorsion1_.size(), 1, CV_32F, vPinHoleDistorsion1_.data()); @@ -169,7 +169,7 @@ class Settings { /* * Visual stuff */ - GeometricCamera *calibration1_, *calibration2_; // Camera calibration + std::shared_ptr calibration1_, calibration2_; // Camera calibration GeometricCamera *originalCalib1_, *originalCalib2_; std::vector vPinHoleDistorsion1_, vPinHoleDistorsion2_; @@ -235,6 +235,5 @@ class Settings { */ float thFarPoints_; }; -}; // namespace ORB_SLAM3 +} // namespace MORB_SLAM -#endif // ORB_SLAM3_SETTINGS_H diff --git a/include/Sim3Solver.h b/include/MORB_SLAM/Sim3Solver.h similarity index 92% rename from include/Sim3Solver.h rename to include/MORB_SLAM/Sim3Solver.h index 56331211..eba6c8dc 100644 --- a/include/Sim3Solver.h +++ b/include/MORB_SLAM/Sim3Solver.h @@ -19,15 +19,14 @@ * ORB-SLAM3. If not, see . */ -#ifndef SIM3SOLVER_H -#define SIM3SOLVER_H +#pragma once #include #include -#include "KeyFrame.h" +#include "MORB_SLAM/KeyFrame.h" -namespace ORB_SLAM3 { +namespace MORB_SLAM { class Sim3Solver { public: @@ -63,10 +62,10 @@ class Sim3Solver { void Project(const std::vector &vP3Dw, std::vector &vP2D, Eigen::Matrix4f Tcw, - GeometricCamera *pCamera); + const std::shared_ptr &pCamera); void FromCameraToImage(const std::vector &vP3Dc, std::vector &vP2D, - GeometricCamera *pCamera); + const std::shared_ptr &pCamera); protected: // KeyFrames and matches @@ -132,9 +131,7 @@ class Sim3Solver { // cv::Mat mK1; // cv::Mat mK2; - GeometricCamera *pCamera1, *pCamera2; + std::shared_ptr pCamera1, pCamera2; }; -} // namespace ORB_SLAM3 - -#endif // SIM3SOLVER_H +} // namespace MORB_SLAM diff --git a/include/System.h b/include/MORB_SLAM/System.h similarity index 93% rename from include/System.h rename to include/MORB_SLAM/System.h index 4afc922d..ef9516d6 100644 --- a/include/System.h +++ b/include/MORB_SLAM/System.h @@ -27,18 +27,19 @@ #include #include -#include "ImprovedTypes.hpp" -#include "Tracking.h" -#include "Atlas.h" -#include "LocalMapping.h" -#include "LoopClosing.h" -#include "KeyFrameDatabase.h" -#include "ORBVocabulary.h" -#include "ImuTypes.h" -#include "Settings.h" - - -namespace ORB_SLAM3 +#include "MORB_SLAM/ImprovedTypes.hpp" +#include "MORB_SLAM/Tracking.h" +#include "MORB_SLAM/Atlas.h" +#include "MORB_SLAM/LocalMapping.h" +#include "MORB_SLAM/LoopClosing.h" +#include "MORB_SLAM/KeyFrameDatabase.h" +#include "MORB_SLAM/ORBVocabulary.h" +#include "MORB_SLAM/ImuTypes.h" +#include "MORB_SLAM/Settings.h" +#include "MORB_SLAM/Camera.hpp" + + +namespace MORB_SLAM { class Verbose @@ -140,8 +141,8 @@ class System void SaveTrajectoryEuRoC(const string &filename); void SaveKeyFrameTrajectoryEuRoC(const string &filename); - void SaveTrajectoryEuRoC(const string &filename, Map* pMap); - void SaveKeyFrameTrajectoryEuRoC(const string &filename, Map* pMap); + void SaveTrajectoryEuRoC(const string &filename, std::shared_ptr pMap); + void SaveKeyFrameTrajectoryEuRoC(const string &filename, std::shared_ptr pMap); // Save data used for initialization debug void SaveDebugData(const int &iniIdx); @@ -187,6 +188,7 @@ class System // Input sensor CameraType::eSensor mSensor; + vector cameras; // ORB vocabulary used for place recognition and feature matching. ORBVocabulary* mpVocabulary; diff --git a/include/Tracking.h b/include/MORB_SLAM/Tracking.h similarity index 86% rename from include/Tracking.h rename to include/MORB_SLAM/Tracking.h index b427f7b7..e8ef1b7c 100644 --- a/include/Tracking.h +++ b/include/MORB_SLAM/Tracking.h @@ -26,20 +26,21 @@ #include #include -#include "Atlas.h" -#include "Frame.h" -#include "GeometricCamera.h" -#include "ImuTypes.h" -#include "KeyFrameDatabase.h" -#include "LocalMapping.h" -#include "LoopClosing.h" -#include "ORBVocabulary.h" -#include "ORBextractor.h" -#include "Settings.h" -#include "System.h" -#include "ImprovedTypes.hpp" - -namespace ORB_SLAM3 { +#include "MORB_SLAM/Atlas.h" +#include "MORB_SLAM/Frame.h" +#include "MORB_SLAM/CameraModels/GeometricCamera.h" +#include "MORB_SLAM/ImuTypes.h" +#include "MORB_SLAM/KeyFrameDatabase.h" +#include "MORB_SLAM/LocalMapping.h" +#include "MORB_SLAM/LoopClosing.h" +#include "MORB_SLAM/ORBVocabulary.h" +#include "MORB_SLAM/ORBextractor.h" +#include "MORB_SLAM/Settings.h" +#include "MORB_SLAM/System.h" +#include "MORB_SLAM/ImprovedTypes.hpp" +#include "MORB_SLAM/Camera.hpp" + +namespace MORB_SLAM { class Atlas; class LocalMapping; @@ -52,7 +53,7 @@ class Tracking { Tracking(System* pSys, ORBVocabulary* pVoc, const Atlas_ptr &pAtlas, KeyFrameDatabase* pKFDB, - const string& strSettingPath, const int sensor, Settings* settings, + const string& strSettingPath, const CameraType::eSensor sensor, Settings* settings, const string& _nameSeq = std::string()); ~Tracking(); @@ -66,11 +67,13 @@ class Tracking { // matching. Sophus::SE3f GrabImageStereo(const cv::Mat& imRectLeft, const cv::Mat& imRectRight, - const double& timestamp, string filename); + const double& timestamp, const string &filename, + const Camera_ptr &cam); Sophus::SE3f GrabImageRGBD(const cv::Mat& imRGB, const cv::Mat& imD, - const double& timestamp, string filename); + const double& timestamp, const string &filename, + const Camera_ptr &cam); Sophus::SE3f GrabImageMonocular(const cv::Mat& im, const double& timestamp, - string filename); + const string &filename, const Camera_ptr &cam); void GrabImuData(const IMU::Point& imuMeasurement); @@ -102,7 +105,7 @@ class Tracking { void SaveSubTrajectory(string strNameFile_frames, string strNameFile_kf, string strFolder = ""); void SaveSubTrajectory(string strNameFile_frames, string strNameFile_kf, - Map* pMap); + std::shared_ptr pMap); float GetImageScale(); @@ -119,7 +122,7 @@ class Tracking { Tracker::eTrackingState mLastProcessedState; // Input sensor - int mSensor; + CameraType::eSensor mSensor; // Current Frame Frame mCurrentFrame; @@ -230,7 +233,7 @@ class Tracking { std::mutex mMutexImuQueue; // Imu calibration parameters - IMU::Calib* mpImuCalib; + std::shared_ptr mpImuCalib; // Last Bias Estimation (at keyframe creation) IMU::Bias mLastBias; @@ -247,15 +250,16 @@ class Tracking { LoopClosing* mpLoopClosing; // ORB - ORBextractor *mpORBextractorLeft, *mpORBextractorRight; - ORBextractor* mpIniORBextractor; + std::shared_ptr mpORBextractorLeft; + std::shared_ptr mpORBextractorRight; + std::shared_ptr mpIniORBextractor; // BoW ORBVocabulary* mpORBVocabulary; KeyFrameDatabase* mpKeyFrameDB; // Initalization (only for monocular) - bool mbReadyToInitializate; + bool mbReadyToInitialize; bool mbSetInit; // Local Map @@ -334,7 +338,8 @@ class Tracking { double mTime_LocalMapTrack; double mTime_NewKF_Dec; - GeometricCamera *mpCamera, *mpCamera2; + std::shared_ptr mpCamera; + std::shared_ptr mpCamera2; int initID, lastID; @@ -355,5 +360,5 @@ class Tracking { cv::Mat mImRight; }; -} // namespace ORB_SLAM3 +} // namespace MORB_SLAM diff --git a/include/TwoViewReconstruction.h b/include/MORB_SLAM/TwoViewReconstruction.h similarity index 97% rename from include/TwoViewReconstruction.h rename to include/MORB_SLAM/TwoViewReconstruction.h index 98227e40..92bb39de 100644 --- a/include/TwoViewReconstruction.h +++ b/include/MORB_SLAM/TwoViewReconstruction.h @@ -16,8 +16,7 @@ * If not, see . */ -#ifndef TwoViewReconstruction_H -#define TwoViewReconstruction_H +#pragma once #include #include @@ -25,7 +24,7 @@ #include -namespace ORB_SLAM3 +namespace MORB_SLAM { class TwoViewReconstruction @@ -95,5 +94,3 @@ namespace ORB_SLAM3 }; } //namespace ORB_SLAM - -#endif // TwoViewReconstruction_H diff --git a/include/Viewer.h b/include/MORB_SLAM/Viewer.h similarity index 91% rename from include/Viewer.h rename to include/MORB_SLAM/Viewer.h index 37f5db36..c702ad28 100644 --- a/include/Viewer.h +++ b/include/MORB_SLAM/Viewer.h @@ -24,12 +24,12 @@ #include #include -#include "ImprovedTypes.hpp" -#include "FrameDrawer.h" -#include "MapDrawer.h" -#include "Settings.h" +#include "MORB_SLAM/ImprovedTypes.hpp" +#include "MORB_SLAM/FrameDrawer.h" +#include "MORB_SLAM/MapDrawer.h" +#include "MORB_SLAM/Settings.h" -namespace ORB_SLAM3 { +namespace MORB_SLAM { class Viewer { void newParameterLoader(const Settings& settings); @@ -74,4 +74,4 @@ class Viewer { bool mbClosed; }; -} // namespace ORB_SLAM3 +} // namespace MORB_SLAM diff --git a/output.txt b/output.txt new file mode 100644 index 00000000..1cf3a992 --- /dev/null +++ b/output.txt @@ -0,0 +1,1441 @@ +Using argument -j12 +Configuring and building Thirdparty/DBoW2 ... +-- Configuring done +-- Generating done +-- Build files have been written to: /orin_ssd/MORB_SLAM/Thirdparty/DBoW2/build +Consolidate compiler generated dependencies of target DBoW2 +[100%] Built target DBoW2 +Configuring and building Thirdparty/g2o ... +-- BUILD TYPE:Release +-- Compiling on Unix +-- Configuring done +-- Generating done +-- Build files have been written to: /orin_ssd/MORB_SLAM/Thirdparty/g2o/build +Consolidate compiler generated dependencies of target g2o +[100%] Built target g2o +Configuring and building Thirdparty/Sophus ... +-- Found installed version of Eigen: /usr/lib/cmake/eigen3 +-- Found required Ceres dependency: Eigen version 3.3.7 in /usr/include/eigen3 +-- Found required Ceres dependency: glog +-- Found installed version of gflags: /usr/lib/aarch64-linux-gnu/cmake/gflags +-- Detected gflags version: 2.2.2 +-- Found required Ceres dependency: gflags +-- Found Ceres version: 1.14.0 installed in: /usr with components: [EigenSparse, SparseLinearAlgebraLibrary, LAPACK, SuiteSparse, CXSparse, SchurSpecializations, OpenMP, Multithreading] +-- Found installed version of Eigen: /usr/lib/cmake/eigen3 +-- Found required Ceres dependency: Eigen version 3.3.7 in /usr/include/eigen3 +-- Found required Ceres dependency: glog +-- Found installed version of gflags: /usr/lib/aarch64-linux-gnu/cmake/gflags +-- Detected gflags version: 2.2.2 +-- Found required Ceres dependency: gflags +-- Found Ceres version: 1.14.0 installed in: /usr with components: [EigenSparse, SparseLinearAlgebraLibrary, LAPACK, SuiteSparse, CXSparse, SchurSpecializations, OpenMP, Multithreading] +-- CERES found +-- Found installed version of Eigen: /usr/lib/cmake/eigen3 +-- Found required Ceres dependency: Eigen version 3.3.7 in /usr/include/eigen3 +-- Found required Ceres dependency: glog +-- Found installed version of gflags: /usr/lib/aarch64-linux-gnu/cmake/gflags +-- Detected gflags version: 2.2.2 +-- Found required Ceres dependency: gflags +-- Found Ceres version: 1.14.0 installed in: /usr with components: [EigenSparse, SparseLinearAlgebraLibrary, LAPACK, SuiteSparse, CXSparse, SchurSpecializations, OpenMP, Multithreading] +-- Configuring done +-- Generating done +-- Build files have been written to: /orin_ssd/MORB_SLAM/Thirdparty/Sophus/build +Consolidate compiler generated dependencies of target test_common +Consolidate compiler generated dependencies of target test_rxso2 +Consolidate compiler generated dependencies of target test_so2 +Consolidate compiler generated dependencies of target test_so3 +Consolidate compiler generated dependencies of target test_rxso3 +Consolidate compiler generated dependencies of target test_geometry +Consolidate compiler generated dependencies of target test_sim2 +Consolidate compiler generated dependencies of target test_sim3 +Consolidate compiler generated dependencies of target test_velocities +Consolidate compiler generated dependencies of target test_se3 +Consolidate compiler generated dependencies of target test_se2 +Consolidate compiler generated dependencies of target test_ceres_se3.cpp +[ 15%] Built target test_common +[ 15%] Built target test_so2 +[ 23%] Built target test_sim2 +[ 30%] Built target test_sim3 +[ 46%] Built target test_velocities +[ 46%] Built target test_rxso2 +[ 53%] Built target test_rxso3 +[ 69%] Built target test_se3 +[ 69%] Built target test_geometry +[ 84%] Built target test_ceres_se3.cpp +[ 84%] Built target test_se2 +[ 92%] Built target test_so3 +Consolidate compiler generated dependencies of target HelloSO3 +[100%] Built target HelloSO3 +Configuring and building MORB_SLAM ... +-- Build type: RelWithDebInfo +-- OPENCV VERSION: +-- 4.5.4 +-- BUILD TYPE:RelWithDebInfo +-- Compiling on Unix +-- Configuring done +-- Generating done +-- Build files have been written to: /orin_ssd/MORB_SLAM/build +-- Build type: RelWithDebInfo +-- OPENCV VERSION: +-- 4.5.4 +-- BUILD TYPE:RelWithDebInfo +-- Compiling on Unix +-- Configuring done +-- Generating done +-- Build files have been written to: /orin_ssd/MORB_SLAM/build +[ 6%] Built target DBoW2 +[ 33%] Built target g2o +[ 34%] Building CXX object CMakeFiles/MORB_SLAM.dir/src/Tracking.cc.o +[ 34%] Building CXX object CMakeFiles/MORB_SLAM.dir/src/System.cc.o +[ 35%] Building CXX object CMakeFiles/MORB_SLAM.dir/src/LocalMapping.cc.o +[ 36%] Building CXX object CMakeFiles/MORB_SLAM.dir/src/LoopClosing.cc.o +[ 37%] Building CXX object CMakeFiles/MORB_SLAM.dir/src/ORBmatcher.cc.o +[ 39%] Building CXX object CMakeFiles/MORB_SLAM.dir/src/MapPoint.cc.o +[ 39%] Building CXX object CMakeFiles/MORB_SLAM.dir/src/FrameDrawer.cc.o +[ 41%] Building CXX object CMakeFiles/MORB_SLAM.dir/src/KeyFrame.cc.o +[ 41%] Building CXX object CMakeFiles/MORB_SLAM.dir/src/Atlas.cc.o +[ 42%] Building CXX object CMakeFiles/MORB_SLAM.dir/src/Map.cc.o +[ 43%] Building CXX object CMakeFiles/MORB_SLAM.dir/src/MapDrawer.cc.o +[ 44%] Building CXX object CMakeFiles/MORB_SLAM.dir/src/Optimizer.cc.o +/orin_ssd/MORB_SLAM/src/Map.cc: In member function ‘void MORB_SLAM::Map::PreSave(std::set >&, std::shared_ptr)’: +/orin_ssd/MORB_SLAM/src/Map.cc:339:32: error: no match for ‘operator!=’ (operand types are ‘std::shared_ptr’ and ‘std::__shared_ptr::element_type*’ {aka ‘MORB_SLAM::Map*’}) + 339 | if ((it->first->GetMap() != sharedMap.get() && this == sharedMap.get()) || it->first->isBad()) { + | ~~~~~~~~~~~~~~~~~~~ ^~ ~~~~~~~~~~~~~~~ + | | | + | | std::__shared_ptr::element_type* {aka MORB_SLAM::Map*} + | std::shared_ptr +In file included from /usr/include/c++/9/bits/stl_algobase.h:64, + from /usr/include/c++/9/bits/stl_tree.h:63, + from /usr/include/c++/9/map:60, + from /usr/include/boost/serialization/map.hpp:20, + from /orin_ssd/MORB_SLAM/include/KeyFrame.h:25, + from /orin_ssd/MORB_SLAM/include/MapPoint.h:22, + from /orin_ssd/MORB_SLAM/include/Map.h:22, + from /orin_ssd/MORB_SLAM/src/Map.cc:22: +/usr/include/c++/9/bits/stl_pair.h:461:5: note: candidate: ‘template constexpr bool std::operator!=(const std::pair<_T1, _T2>&, const std::pair<_T1, _T2>&)’ + 461 | operator!=(const pair<_T1, _T2>& __x, const pair<_T1, _T2>& __y) + | ^~~~~~~~ +/usr/include/c++/9/bits/stl_pair.h:461:5: note: template argument deduction/substitution failed: +/orin_ssd/MORB_SLAM/src/Map.cc:339:49: note: ‘std::shared_ptr’ is not derived from ‘const std::pair<_T1, _T2>’ + 339 | if ((it->first->GetMap() != sharedMap.get() && this == sharedMap.get()) || it->first->isBad()) { + | ^ +In file included from /usr/include/c++/9/bits/stl_algobase.h:67, + from /usr/include/c++/9/bits/stl_tree.h:63, + from /usr/include/c++/9/map:60, + from /usr/include/boost/serialization/map.hpp:20, + from /orin_ssd/MORB_SLAM/include/KeyFrame.h:25, + from /orin_ssd/MORB_SLAM/include/MapPoint.h:22, + from /orin_ssd/MORB_SLAM/include/Map.h:22, + from /orin_ssd/MORB_SLAM/src/Map.cc:22: +/usr/include/c++/9/bits/stl_iterator.h:337:5: note: candidate: ‘template constexpr bool std::operator!=(const std::reverse_iterator<_Iterator>&, const std::reverse_iterator<_Iterator>&)’ + 337 | operator!=(const reverse_iterator<_Iterator>& __x, + | ^~~~~~~~ +/usr/include/c++/9/bits/stl_iterator.h:337:5: note: template argument deduction/substitution failed: +/orin_ssd/MORB_SLAM/src/Map.cc:339:49: note: ‘std::shared_ptr’ is not derived from ‘const std::reverse_iterator<_Iterator>’ + 339 | if ((it->first->GetMap() != sharedMap.get() && this == sharedMap.get()) || it->first->isBad()) { + | ^ +In file included from /usr/include/c++/9/bits/stl_algobase.h:67, + from /usr/include/c++/9/bits/stl_tree.h:63, + from /usr/include/c++/9/map:60, + from /usr/include/boost/serialization/map.hpp:20, + from /orin_ssd/MORB_SLAM/include/KeyFrame.h:25, + from /orin_ssd/MORB_SLAM/include/MapPoint.h:22, + from /orin_ssd/MORB_SLAM/include/Map.h:22, + from /orin_ssd/MORB_SLAM/src/Map.cc:22: +/usr/include/c++/9/bits/stl_iterator.h:375:5: note: candidate: ‘template constexpr bool std::operator!=(const std::reverse_iterator<_Iterator>&, const std::reverse_iterator<_IteratorR>&)’ + 375 | operator!=(const reverse_iterator<_IteratorL>& __x, + | ^~~~~~~~ +/usr/include/c++/9/bits/stl_iterator.h:375:5: note: template argument deduction/substitution failed: +/orin_ssd/MORB_SLAM/src/Map.cc:339:49: note: ‘std::shared_ptr’ is not derived from ‘const std::reverse_iterator<_Iterator>’ + 339 | if ((it->first->GetMap() != sharedMap.get() && this == sharedMap.get()) || it->first->isBad()) { + | ^ +In file included from /usr/include/c++/9/bits/stl_algobase.h:67, + from /usr/include/c++/9/bits/stl_tree.h:63, + from /usr/include/c++/9/map:60, + from /usr/include/boost/serialization/map.hpp:20, + from /orin_ssd/MORB_SLAM/include/KeyFrame.h:25, + from /orin_ssd/MORB_SLAM/include/MapPoint.h:22, + from /orin_ssd/MORB_SLAM/include/Map.h:22, + from /orin_ssd/MORB_SLAM/src/Map.cc:22: +/usr/include/c++/9/bits/stl_iterator.h:1148:5: note: candidate: ‘template constexpr bool std::operator!=(const std::move_iterator<_IteratorL>&, const std::move_iterator<_IteratorR>&)’ + 1148 | operator!=(const move_iterator<_IteratorL>& __x, + | ^~~~~~~~ +/usr/include/c++/9/bits/stl_iterator.h:1148:5: note: template argument deduction/substitution failed: +/orin_ssd/MORB_SLAM/src/Map.cc:339:49: note: ‘std::shared_ptr’ is not derived from ‘const std::move_iterator<_IteratorL>’ + 339 | if ((it->first->GetMap() != sharedMap.get() && this == sharedMap.get()) || it->first->isBad()) { + | ^ +In file included from /usr/include/c++/9/bits/stl_algobase.h:67, + from /usr/include/c++/9/bits/stl_tree.h:63, + from /usr/include/c++/9/map:60, + from /usr/include/boost/serialization/map.hpp:20, + from /orin_ssd/MORB_SLAM/include/KeyFrame.h:25, + from /orin_ssd/MORB_SLAM/include/MapPoint.h:22, + from /orin_ssd/MORB_SLAM/include/Map.h:22, + from /orin_ssd/MORB_SLAM/src/Map.cc:22: +/usr/include/c++/9/bits/stl_iterator.h:1154:5: note: candidate: ‘template constexpr bool std::operator!=(const std::move_iterator<_IteratorL>&, const std::move_iterator<_IteratorL>&)’ + 1154 | operator!=(const move_iterator<_Iterator>& __x, + | ^~~~~~~~ +/usr/include/c++/9/bits/stl_iterator.h:1154:5: note: template argument deduction/substitution failed: +/orin_ssd/MORB_SLAM/src/Map.cc:339:49: note: ‘std::shared_ptr’ is not derived from ‘const std::move_iterator<_IteratorL>’ + 339 | if ((it->first->GetMap() != sharedMap.get() && this == sharedMap.get()) || it->first->isBad()) { + | ^ +In file included from /usr/include/c++/9/bits/stl_tree.h:64, + from /usr/include/c++/9/map:60, + from /usr/include/boost/serialization/map.hpp:20, + from /orin_ssd/MORB_SLAM/include/KeyFrame.h:25, + from /orin_ssd/MORB_SLAM/include/MapPoint.h:22, + from /orin_ssd/MORB_SLAM/include/Map.h:22, + from /orin_ssd/MORB_SLAM/src/Map.cc:22: +/usr/include/c++/9/bits/allocator.h:173:5: note: candidate: ‘template bool std::operator!=(const std::allocator<_Tp1>&, const std::allocator<_T2>&)’ + 173 | operator!=(const allocator<_T1>&, const allocator<_T2>&) + | ^~~~~~~~ +/usr/include/c++/9/bits/allocator.h:173:5: note: template argument deduction/substitution failed: +/orin_ssd/MORB_SLAM/src/Map.cc:339:49: note: ‘std::shared_ptr’ is not derived from ‘const std::allocator<_Tp1>’ + 339 | if ((it->first->GetMap() != sharedMap.get() && this == sharedMap.get()) || it->first->isBad()) { + | ^ +In file included from /usr/include/c++/9/bits/char_traits.h:40, + from /usr/include/c++/9/string:40, + from /usr/include/c++/9/stdexcept:39, + from /usr/include/c++/9/optional:38, + from /usr/include/c++/9/bits/node_handle.h:39, + from /usr/include/c++/9/bits/stl_tree.h:72, + from /usr/include/c++/9/map:60, + from /usr/include/boost/serialization/map.hpp:20, + from /orin_ssd/MORB_SLAM/include/KeyFrame.h:25, + from /orin_ssd/MORB_SLAM/include/MapPoint.h:22, + from /orin_ssd/MORB_SLAM/include/Map.h:22, + from /orin_ssd/MORB_SLAM/src/Map.cc:22: +/usr/include/c++/9/bits/postypes.h:227:5: note: candidate: ‘template bool std::operator!=(const std::fpos<_StateT>&, const std::fpos<_StateT>&)’ + 227 | operator!=(const fpos<_StateT>& __lhs, const fpos<_StateT>& __rhs) + | ^~~~~~~~ +/usr/include/c++/9/bits/postypes.h:227:5: note: template argument deduction/substitution failed: +/orin_ssd/MORB_SLAM/src/Map.cc:339:49: note: ‘std::shared_ptr’ is not derived from ‘const std::fpos<_StateT>’ + 339 | if ((it->first->GetMap() != sharedMap.get() && this == sharedMap.get()) || it->first->isBad()) { + | ^ +In file included from /usr/include/c++/9/bits/basic_string.h:48, + from /usr/include/c++/9/string:55, + from /usr/include/c++/9/stdexcept:39, + from /usr/include/c++/9/optional:38, + from /usr/include/c++/9/bits/node_handle.h:39, + from /usr/include/c++/9/bits/stl_tree.h:72, + from /usr/include/c++/9/map:60, + from /usr/include/boost/serialization/map.hpp:20, + from /orin_ssd/MORB_SLAM/include/KeyFrame.h:25, + from /orin_ssd/MORB_SLAM/include/MapPoint.h:22, + from /orin_ssd/MORB_SLAM/include/Map.h:22, + from /orin_ssd/MORB_SLAM/src/Map.cc:22: +/usr/include/c++/9/string_view:489:5: note: candidate: ‘template constexpr bool std::operator!=(std::basic_string_view<_CharT, _Traits>, std::basic_string_view<_CharT, _Traits>)’ + 489 | operator!=(basic_string_view<_CharT, _Traits> __x, + | ^~~~~~~~ +/usr/include/c++/9/string_view:489:5: note: template argument deduction/substitution failed: +/orin_ssd/MORB_SLAM/src/Map.cc:339:49: note: ‘std::shared_ptr’ is not derived from ‘std::basic_string_view<_CharT, _Traits>’ + 339 | if ((it->first->GetMap() != sharedMap.get() && this == sharedMap.get()) || it->first->isBad()) { + | ^ +In file included from /usr/include/c++/9/bits/basic_string.h:48, + from /usr/include/c++/9/string:55, + from /usr/include/c++/9/stdexcept:39, + from /usr/include/c++/9/optional:38, + from /usr/include/c++/9/bits/node_handle.h:39, + from /usr/include/c++/9/bits/stl_tree.h:72, + from /usr/include/c++/9/map:60, + from /usr/include/boost/serialization/map.hpp:20, + from /orin_ssd/MORB_SLAM/include/KeyFrame.h:25, + from /orin_ssd/MORB_SLAM/include/MapPoint.h:22, + from /orin_ssd/MORB_SLAM/include/Map.h:22, + from /orin_ssd/MORB_SLAM/src/Map.cc:22: +/usr/include/c++/9/string_view:495:5: note: candidate: ‘template constexpr bool std::operator!=(std::basic_string_view<_CharT, _Traits>, std::__detail::__idt >)’ + 495 | operator!=(basic_string_view<_CharT, _Traits> __x, + | ^~~~~~~~ +/usr/include/c++/9/string_view:495:5: note: template argument deduction/substitution failed: +/orin_ssd/MORB_SLAM/src/Map.cc:339:49: note: ‘std::shared_ptr’ is not derived from ‘std::basic_string_view<_CharT, _Traits>’ + 339 | if ((it->first->GetMap() != sharedMap.get() && this == sharedMap.get()) || it->first->isBad()) { + | ^ +In file included from /usr/include/c++/9/bits/basic_string.h:48, + from /usr/include/c++/9/string:55, + from /usr/include/c++/9/stdexcept:39, + from /usr/include/c++/9/optional:38, + from /usr/include/c++/9/bits/node_handle.h:39, + from /usr/include/c++/9/bits/stl_tree.h:72, + from /usr/include/c++/9/map:60, + from /usr/include/boost/serialization/map.hpp:20, + from /orin_ssd/MORB_SLAM/include/KeyFrame.h:25, + from /orin_ssd/MORB_SLAM/include/MapPoint.h:22, + from /orin_ssd/MORB_SLAM/include/Map.h:22, + from /orin_ssd/MORB_SLAM/src/Map.cc:22: +/usr/include/c++/9/string_view:501:5: note: candidate: ‘template constexpr bool std::operator!=(std::__detail::__idt >, std::basic_string_view<_CharT, _Traits>)’ + 501 | operator!=(__detail::__idt> __x, + | ^~~~~~~~ +/usr/include/c++/9/string_view:501:5: note: template argument deduction/substitution failed: +/orin_ssd/MORB_SLAM/src/Map.cc:339:49: note: mismatched types ‘std::basic_string_view<_CharT, _Traits>’ and ‘std::__shared_ptr::element_type*’ {aka ‘MORB_SLAM::Map*’} + 339 | if ((it->first->GetMap() != sharedMap.get() && this == sharedMap.get()) || it->first->isBad()) { + | ^ +In file included from /usr/include/c++/9/string:55, + from /usr/include/c++/9/stdexcept:39, + from /usr/include/c++/9/optional:38, + from /usr/include/c++/9/bits/node_handle.h:39, + from /usr/include/c++/9/bits/stl_tree.h:72, + from /usr/include/c++/9/map:60, + from /usr/include/boost/serialization/map.hpp:20, + from /orin_ssd/MORB_SLAM/include/KeyFrame.h:25, + from /orin_ssd/MORB_SLAM/include/MapPoint.h:22, + from /orin_ssd/MORB_SLAM/include/Map.h:22, + from /orin_ssd/MORB_SLAM/src/Map.cc:22: +/usr/include/c++/9/bits/basic_string.h:6191:5: note: candidate: ‘template bool std::operator!=(const std::__cxx11::basic_string<_CharT, _Traits, _Allocator>&, const std::__cxx11::basic_string<_CharT, _Traits, _Allocator>&)’ + 6191 | operator!=(const basic_string<_CharT, _Traits, _Alloc>& __lhs, + | ^~~~~~~~ +/usr/include/c++/9/bits/basic_string.h:6191:5: note: template argument deduction/substitution failed: +/orin_ssd/MORB_SLAM/src/Map.cc:339:49: note: ‘std::shared_ptr’ is not derived from ‘const std::__cxx11::basic_string<_CharT, _Traits, _Allocator>’ + 339 | if ((it->first->GetMap() != sharedMap.get() && this == sharedMap.get()) || it->first->isBad()) { + | ^ +In file included from /usr/include/c++/9/string:55, + from /usr/include/c++/9/stdexcept:39, + from /usr/include/c++/9/optional:38, + from /usr/include/c++/9/bits/node_handle.h:39, + from /usr/include/c++/9/bits/stl_tree.h:72, + from /usr/include/c++/9/map:60, + from /usr/include/boost/serialization/map.hpp:20, + from /orin_ssd/MORB_SLAM/include/KeyFrame.h:25, + from /orin_ssd/MORB_SLAM/include/MapPoint.h:22, + from /orin_ssd/MORB_SLAM/include/Map.h:22, + from /orin_ssd/MORB_SLAM/src/Map.cc:22: +/usr/include/c++/9/bits/basic_string.h:6204:5: note: candidate: ‘template bool std::operator!=(const _CharT*, const std::__cxx11::basic_string<_CharT, _Traits, _Allocator>&)’ + 6204 | operator!=(const _CharT* __lhs, + | ^~~~~~~~ +/usr/include/c++/9/bits/basic_string.h:6204:5: note: template argument deduction/substitution failed: +/orin_ssd/MORB_SLAM/src/Map.cc:339:49: note: mismatched types ‘const _CharT*’ and ‘std::shared_ptr’ + 339 | if ((it->first->GetMap() != sharedMap.get() && this == sharedMap.get()) || it->first->isBad()) { + | ^ +In file included from /usr/include/c++/9/string:55, + from /usr/include/c++/9/stdexcept:39, + from /usr/include/c++/9/optional:38, + from /usr/include/c++/9/bits/node_handle.h:39, + from /usr/include/c++/9/bits/stl_tree.h:72, + from /usr/include/c++/9/map:60, + from /usr/include/boost/serialization/map.hpp:20, + from /orin_ssd/MORB_SLAM/include/KeyFrame.h:25, + from /orin_ssd/MORB_SLAM/include/MapPoint.h:22, + from /orin_ssd/MORB_SLAM/include/Map.h:22, + from /orin_ssd/MORB_SLAM/src/Map.cc:22: +/usr/include/c++/9/bits/basic_string.h:6216:5: note: candidate: ‘template bool std::operator!=(const std::__cxx11::basic_string<_CharT, _Traits, _Allocator>&, const _CharT*)’ + 6216 | operator!=(const basic_string<_CharT, _Traits, _Alloc>& __lhs, + | ^~~~~~~~ +/usr/include/c++/9/bits/basic_string.h:6216:5: note: template argument deduction/substitution failed: +/orin_ssd/MORB_SLAM/src/Map.cc:339:49: note: ‘std::shared_ptr’ is not derived from ‘const std::__cxx11::basic_string<_CharT, _Traits, _Allocator>’ + 339 | if ((it->first->GetMap() != sharedMap.get() && this == sharedMap.get()) || it->first->isBad()) { + | ^ +In file included from /usr/include/c++/9/bits/node_handle.h:39, + from /usr/include/c++/9/bits/stl_tree.h:72, + from /usr/include/c++/9/map:60, + from /usr/include/boost/serialization/map.hpp:20, + from /orin_ssd/MORB_SLAM/include/KeyFrame.h:25, + from /orin_ssd/MORB_SLAM/include/MapPoint.h:22, + from /orin_ssd/MORB_SLAM/include/Map.h:22, + from /orin_ssd/MORB_SLAM/src/Map.cc:22: +/usr/include/c++/9/optional:992:5: note: candidate: ‘template constexpr std::__optional_relop_t() != declval<_Up>()))> std::operator!=(const std::optional<_Tp>&, const std::optional<_Up>&)’ + 992 | operator!=(const optional<_Tp>& __lhs, const optional<_Up>& __rhs) + | ^~~~~~~~ +/usr/include/c++/9/optional:992:5: note: template argument deduction/substitution failed: +/orin_ssd/MORB_SLAM/src/Map.cc:339:49: note: ‘std::shared_ptr’ is not derived from ‘const std::optional<_Tp>’ + 339 | if ((it->first->GetMap() != sharedMap.get() && this == sharedMap.get()) || it->first->isBad()) { + | ^ +In file included from /usr/include/c++/9/bits/node_handle.h:39, + from /usr/include/c++/9/bits/stl_tree.h:72, + from /usr/include/c++/9/map:60, + from /usr/include/boost/serialization/map.hpp:20, + from /orin_ssd/MORB_SLAM/include/KeyFrame.h:25, + from /orin_ssd/MORB_SLAM/include/MapPoint.h:22, + from /orin_ssd/MORB_SLAM/include/Map.h:22, + from /orin_ssd/MORB_SLAM/src/Map.cc:22: +/usr/include/c++/9/optional:1044:5: note: candidate: ‘template constexpr bool std::operator!=(const std::optional<_Tp>&, std::nullopt_t)’ + 1044 | operator!=(const optional<_Tp>& __lhs, nullopt_t) noexcept + | ^~~~~~~~ +/usr/include/c++/9/optional:1044:5: note: template argument deduction/substitution failed: +/orin_ssd/MORB_SLAM/src/Map.cc:339:49: note: ‘std::shared_ptr’ is not derived from ‘const std::optional<_Tp>’ + 339 | if ((it->first->GetMap() != sharedMap.get() && this == sharedMap.get()) || it->first->isBad()) { + | ^ +In file included from /usr/include/c++/9/bits/node_handle.h:39, + from /usr/include/c++/9/bits/stl_tree.h:72, + from /usr/include/c++/9/map:60, + from /usr/include/boost/serialization/map.hpp:20, + from /orin_ssd/MORB_SLAM/include/KeyFrame.h:25, + from /orin_ssd/MORB_SLAM/include/MapPoint.h:22, + from /orin_ssd/MORB_SLAM/include/Map.h:22, + from /orin_ssd/MORB_SLAM/src/Map.cc:22: +/usr/include/c++/9/optional:1049:5: note: candidate: ‘template constexpr bool std::operator!=(std::nullopt_t, const std::optional<_Tp>&)’ + 1049 | operator!=(nullopt_t, const optional<_Tp>& __rhs) noexcept + | ^~~~~~~~ +/usr/include/c++/9/optional:1049:5: note: template argument deduction/substitution failed: +/orin_ssd/MORB_SLAM/src/Map.cc:339:49: note: mismatched types ‘const std::optional<_Tp>’ and ‘std::__shared_ptr::element_type*’ {aka ‘MORB_SLAM::Map*’} + 339 | if ((it->first->GetMap() != sharedMap.get() && this == sharedMap.get()) || it->first->isBad()) { + | ^ +In file included from /usr/include/c++/9/bits/node_handle.h:39, + from /usr/include/c++/9/bits/stl_tree.h:72, + from /usr/include/c++/9/map:60, + from /usr/include/boost/serialization/map.hpp:20, + from /orin_ssd/MORB_SLAM/include/KeyFrame.h:25, + from /orin_ssd/MORB_SLAM/include/MapPoint.h:22, + from /orin_ssd/MORB_SLAM/include/Map.h:22, + from /orin_ssd/MORB_SLAM/src/Map.cc:22: +/usr/include/c++/9/optional:1107:5: note: candidate: ‘template constexpr std::__optional_relop_t() != declval<_Up>()))> std::operator!=(const std::optional<_Tp>&, const _Up&)’ + 1107 | operator!=(const optional<_Tp>& __lhs, const _Up& __rhs) + | ^~~~~~~~ +/usr/include/c++/9/optional:1107:5: note: template argument deduction/substitution failed: +/orin_ssd/MORB_SLAM/src/Map.cc:339:49: note: ‘std::shared_ptr’ is not derived from ‘const std::optional<_Tp>’ + 339 | if ((it->first->GetMap() != sharedMap.get() && this == sharedMap.get()) || it->first->isBad()) { + | ^ +In file included from /usr/include/c++/9/bits/node_handle.h:39, + from /usr/include/c++/9/bits/stl_tree.h:72, + from /usr/include/c++/9/map:60, + from /usr/include/boost/serialization/map.hpp:20, + from /orin_ssd/MORB_SLAM/include/KeyFrame.h:25, + from /orin_ssd/MORB_SLAM/include/MapPoint.h:22, + from /orin_ssd/MORB_SLAM/include/Map.h:22, + from /orin_ssd/MORB_SLAM/src/Map.cc:22: +/usr/include/c++/9/optional:1113:5: note: candidate: ‘template constexpr std::__optional_relop_t() != declval<_Tp>()))> std::operator!=(const _Up&, const std::optional<_Tp>&)’ + 1113 | operator!=(const _Up& __lhs, const optional<_Tp>& __rhs) + | ^~~~~~~~ +/usr/include/c++/9/optional:1113:5: note: template argument deduction/substitution failed: +/orin_ssd/MORB_SLAM/src/Map.cc:339:49: note: mismatched types ‘const std::optional<_Tp>’ and ‘std::__shared_ptr::element_type*’ {aka ‘MORB_SLAM::Map*’} + 339 | if ((it->first->GetMap() != sharedMap.get() && this == sharedMap.get()) || it->first->isBad()) { + | ^ +In file included from /usr/include/c++/9/tuple:39, + from /usr/include/c++/9/bits/stl_map.h:63, + from /usr/include/c++/9/map:61, + from /usr/include/boost/serialization/map.hpp:20, + from /orin_ssd/MORB_SLAM/include/KeyFrame.h:25, + from /orin_ssd/MORB_SLAM/include/MapPoint.h:22, + from /orin_ssd/MORB_SLAM/include/Map.h:22, + from /orin_ssd/MORB_SLAM/src/Map.cc:22: +/usr/include/c++/9/array:257:5: note: candidate: ‘template bool std::operator!=(const std::array<_Tp, _Nm>&, const std::array<_Tp, _Nm>&)’ + 257 | operator!=(const array<_Tp, _Nm>& __one, const array<_Tp, _Nm>& __two) + | ^~~~~~~~ +/usr/include/c++/9/array:257:5: note: template argument deduction/substitution failed: +/orin_ssd/MORB_SLAM/src/Map.cc:339:49: note: ‘std::shared_ptr’ is not derived from ‘const std::array<_Tp, _Nm>’ + 339 | if ((it->first->GetMap() != sharedMap.get() && this == sharedMap.get()) || it->first->isBad()) { + | ^ +In file included from /usr/include/c++/9/bits/stl_map.h:63, + from /usr/include/c++/9/map:61, + from /usr/include/boost/serialization/map.hpp:20, + from /orin_ssd/MORB_SLAM/include/KeyFrame.h:25, + from /orin_ssd/MORB_SLAM/include/MapPoint.h:22, + from /orin_ssd/MORB_SLAM/include/Map.h:22, + from /orin_ssd/MORB_SLAM/src/Map.cc:22: +/usr/include/c++/9/tuple:1445:5: note: candidate: ‘template constexpr bool std::operator!=(const std::tuple<_Tps ...>&, const std::tuple<_Args2 ...>&)’ + 1445 | operator!=(const tuple<_TElements...>& __t, + | ^~~~~~~~ +/usr/include/c++/9/tuple:1445:5: note: template argument deduction/substitution failed: +/orin_ssd/MORB_SLAM/src/Map.cc:339:49: note: ‘std::shared_ptr’ is not derived from ‘const std::tuple<_Tps ...>’ + 339 | if ((it->first->GetMap() != sharedMap.get() && this == sharedMap.get()) || it->first->isBad()) { + | ^ +In file included from /usr/include/c++/9/map:61, + from /usr/include/boost/serialization/map.hpp:20, + from /orin_ssd/MORB_SLAM/include/KeyFrame.h:25, + from /orin_ssd/MORB_SLAM/include/MapPoint.h:22, + from /orin_ssd/MORB_SLAM/include/Map.h:22, + from /orin_ssd/MORB_SLAM/src/Map.cc:22: +/usr/include/c++/9/bits/stl_map.h:1479:5: note: candidate: ‘template bool std::operator!=(const std::map<_Key, _Tp, _Compare, _Allocator>&, const std::map<_Key, _Tp, _Compare, _Allocator>&)’ + 1479 | operator!=(const map<_Key, _Tp, _Compare, _Alloc>& __x, + | ^~~~~~~~ +/usr/include/c++/9/bits/stl_map.h:1479:5: note: template argument deduction/substitution failed: +/orin_ssd/MORB_SLAM/src/Map.cc:339:49: note: ‘std::shared_ptr’ is not derived from ‘const std::map<_Key, _Tp, _Compare, _Allocator>’ + 339 | if ((it->first->GetMap() != sharedMap.get() && this == sharedMap.get()) || it->first->isBad()) { + | ^ +In file included from /usr/include/c++/9/map:62, + from /usr/include/boost/serialization/map.hpp:20, + from /orin_ssd/MORB_SLAM/include/KeyFrame.h:25, + from /orin_ssd/MORB_SLAM/include/MapPoint.h:22, + from /orin_ssd/MORB_SLAM/include/Map.h:22, + from /orin_ssd/MORB_SLAM/src/Map.cc:22: +/usr/include/c++/9/bits/stl_multimap.h:1143:5: note: candidate: ‘template bool std::operator!=(const std::multimap<_Key, _Tp, _Compare, _Allocator>&, const std::multimap<_Key, _Tp, _Compare, _Allocator>&)’ + 1143 | operator!=(const multimap<_Key, _Tp, _Compare, _Alloc>& __x, + | ^~~~~~~~ +/usr/include/c++/9/bits/stl_multimap.h:1143:5: note: template argument deduction/substitution failed: +/orin_ssd/MORB_SLAM/src/Map.cc:339:49: note: ‘std::shared_ptr’ is not derived from ‘const std::multimap<_Key, _Tp, _Compare, _Allocator>’ + 339 | if ((it->first->GetMap() != sharedMap.get() && this == sharedMap.get()) || it->first->isBad()) { + | ^ +In file included from /usr/include/c++/9/memory:80, + from /usr/include/boost/smart_ptr/scoped_ptr.hpp:22, + from /usr/include/boost/scoped_ptr.hpp:13, + from /usr/include/boost/archive/detail/basic_iarchive.hpp:23, + from /usr/include/boost/serialization/map.hpp:24, + from /orin_ssd/MORB_SLAM/include/KeyFrame.h:25, + from /orin_ssd/MORB_SLAM/include/MapPoint.h:22, + from /orin_ssd/MORB_SLAM/include/Map.h:22, + from /orin_ssd/MORB_SLAM/src/Map.cc:22: +/usr/include/c++/9/bits/unique_ptr.h:732:5: note: candidate: ‘template bool std::operator!=(const std::unique_ptr<_Tp, _Dp>&, const std::unique_ptr<_Up, _Ep>&)’ + 732 | operator!=(const unique_ptr<_Tp, _Dp>& __x, + | ^~~~~~~~ +/usr/include/c++/9/bits/unique_ptr.h:732:5: note: template argument deduction/substitution failed: +/orin_ssd/MORB_SLAM/src/Map.cc:339:49: note: ‘std::shared_ptr’ is not derived from ‘const std::unique_ptr<_Tp, _Dp>’ + 339 | if ((it->first->GetMap() != sharedMap.get() && this == sharedMap.get()) || it->first->isBad()) { + | ^ +In file included from /usr/include/c++/9/memory:80, + from /usr/include/boost/smart_ptr/scoped_ptr.hpp:22, + from /usr/include/boost/scoped_ptr.hpp:13, + from /usr/include/boost/archive/detail/basic_iarchive.hpp:23, + from /usr/include/boost/serialization/map.hpp:24, + from /orin_ssd/MORB_SLAM/include/KeyFrame.h:25, + from /orin_ssd/MORB_SLAM/include/MapPoint.h:22, + from /orin_ssd/MORB_SLAM/include/Map.h:22, + from /orin_ssd/MORB_SLAM/src/Map.cc:22: +/usr/include/c++/9/bits/unique_ptr.h:738:5: note: candidate: ‘template bool std::operator!=(const std::unique_ptr<_Tp, _Dp>&, std::nullptr_t)’ + 738 | operator!=(const unique_ptr<_Tp, _Dp>& __x, nullptr_t) noexcept + | ^~~~~~~~ +/usr/include/c++/9/bits/unique_ptr.h:738:5: note: template argument deduction/substitution failed: +/orin_ssd/MORB_SLAM/src/Map.cc:339:49: note: ‘std::shared_ptr’ is not derived from ‘const std::unique_ptr<_Tp, _Dp>’ + 339 | if ((it->first->GetMap() != sharedMap.get() && this == sharedMap.get()) || it->first->isBad()) { + | ^ +In file included from /usr/include/c++/9/memory:80, + from /usr/include/boost/smart_ptr/scoped_ptr.hpp:22, + from /usr/include/boost/scoped_ptr.hpp:13, + from /usr/include/boost/archive/detail/basic_iarchive.hpp:23, + from /usr/include/boost/serialization/map.hpp:24, + from /orin_ssd/MORB_SLAM/include/KeyFrame.h:25, + from /orin_ssd/MORB_SLAM/include/MapPoint.h:22, + from /orin_ssd/MORB_SLAM/include/Map.h:22, + from /orin_ssd/MORB_SLAM/src/Map.cc:22: +/usr/include/c++/9/bits/unique_ptr.h:743:5: note: candidate: ‘template bool std::operator!=(std::nullptr_t, const std::unique_ptr<_Tp, _Dp>&)’ + 743 | operator!=(nullptr_t, const unique_ptr<_Tp, _Dp>& __x) noexcept + | ^~~~~~~~ +/usr/include/c++/9/bits/unique_ptr.h:743:5: note: template argument deduction/substitution failed: +/orin_ssd/MORB_SLAM/src/Map.cc:339:49: note: mismatched types ‘const std::unique_ptr<_Tp, _Dp>’ and ‘std::__shared_ptr::element_type*’ {aka ‘MORB_SLAM::Map*’} + 339 | if ((it->first->GetMap() != sharedMap.get() && this == sharedMap.get()) || it->first->isBad()) { + | ^ +In file included from /usr/include/c++/9/bits/shared_ptr.h:52, + from /usr/include/c++/9/memory:81, + from /usr/include/boost/smart_ptr/scoped_ptr.hpp:22, + from /usr/include/boost/scoped_ptr.hpp:13, + from /usr/include/boost/archive/detail/basic_iarchive.hpp:23, + from /usr/include/boost/serialization/map.hpp:24, + from /orin_ssd/MORB_SLAM/include/KeyFrame.h:25, + from /orin_ssd/MORB_SLAM/include/MapPoint.h:22, + from /orin_ssd/MORB_SLAM/include/Map.h:22, + from /orin_ssd/MORB_SLAM/src/Map.cc:22: +/usr/include/c++/9/bits/shared_ptr_base.h:1428:5: note: candidate: ‘template bool std::operator!=(const std::__shared_ptr<_Tp1, _Lp>&, const std::__shared_ptr<_Tp2, _Lp>&)’ + 1428 | operator!=(const __shared_ptr<_Tp1, _Lp>& __a, + | ^~~~~~~~ +/usr/include/c++/9/bits/shared_ptr_base.h:1428:5: note: template argument deduction/substitution failed: +/orin_ssd/MORB_SLAM/src/Map.cc:339:49: note: mismatched types ‘const std::__shared_ptr<_Tp2, _Lp>’ and ‘std::__shared_ptr::element_type*’ {aka ‘MORB_SLAM::Map*’} + 339 | if ((it->first->GetMap() != sharedMap.get() && this == sharedMap.get()) || it->first->isBad()) { + | ^ +In file included from /usr/include/c++/9/bits/shared_ptr.h:52, + from /usr/include/c++/9/memory:81, + from /usr/include/boost/smart_ptr/scoped_ptr.hpp:22, + from /usr/include/boost/scoped_ptr.hpp:13, + from /usr/include/boost/archive/detail/basic_iarchive.hpp:23, + from /usr/include/boost/serialization/map.hpp:24, + from /orin_ssd/MORB_SLAM/include/KeyFrame.h:25, + from /orin_ssd/MORB_SLAM/include/MapPoint.h:22, + from /orin_ssd/MORB_SLAM/include/Map.h:22, + from /orin_ssd/MORB_SLAM/src/Map.cc:22: +/usr/include/c++/9/bits/shared_ptr_base.h:1434:5: note: candidate: ‘template bool std::operator!=(const std::__shared_ptr<_Tp, _Lp>&, std::nullptr_t)’ + 1434 | operator!=(const __shared_ptr<_Tp, _Lp>& __a, nullptr_t) noexcept + | ^~~~~~~~ +/usr/include/c++/9/bits/shared_ptr_base.h:1434:5: note: template argument deduction/substitution failed: +/orin_ssd/MORB_SLAM/src/Map.cc:339:48: note: cannot convert ‘sharedMap.std::shared_ptr::.std::__shared_ptr::get()’ (type ‘std::__shared_ptr::element_type*’ {aka ‘MORB_SLAM::Map*’}) to type ‘std::nullptr_t’ + 339 | if ((it->first->GetMap() != sharedMap.get() && this == sharedMap.get()) || it->first->isBad()) { + | ~~~~~~~~~~~~~^~ +In file included from /usr/include/c++/9/bits/shared_ptr.h:52, + from /usr/include/c++/9/memory:81, + from /usr/include/boost/smart_ptr/scoped_ptr.hpp:22, + from /usr/include/boost/scoped_ptr.hpp:13, + from /usr/include/boost/archive/detail/basic_iarchive.hpp:23, + from /usr/include/boost/serialization/map.hpp:24, + from /orin_ssd/MORB_SLAM/include/KeyFrame.h:25, + from /orin_ssd/MORB_SLAM/include/MapPoint.h:22, + from /orin_ssd/MORB_SLAM/include/Map.h:22, + from /orin_ssd/MORB_SLAM/src/Map.cc:22: +/usr/include/c++/9/bits/shared_ptr_base.h:1439:5: note: candidate: ‘template bool std::operator!=(std::nullptr_t, const std::__shared_ptr<_Tp, _Lp>&)’ + 1439 | operator!=(nullptr_t, const __shared_ptr<_Tp, _Lp>& __a) noexcept + | ^~~~~~~~ +/usr/include/c++/9/bits/shared_ptr_base.h:1439:5: note: template argument deduction/substitution failed: +/orin_ssd/MORB_SLAM/src/Map.cc:339:49: note: mismatched types ‘const std::__shared_ptr<_Tp, _Lp>’ and ‘std::__shared_ptr::element_type*’ {aka ‘MORB_SLAM::Map*’} + 339 | if ((it->first->GetMap() != sharedMap.get() && this == sharedMap.get()) || it->first->isBad()) { + | ^ +In file included from /usr/include/c++/9/memory:81, + from /usr/include/boost/smart_ptr/scoped_ptr.hpp:22, + from /usr/include/boost/scoped_ptr.hpp:13, + from /usr/include/boost/archive/detail/basic_iarchive.hpp:23, + from /usr/include/boost/serialization/map.hpp:24, + from /orin_ssd/MORB_SLAM/include/KeyFrame.h:25, + from /orin_ssd/MORB_SLAM/include/MapPoint.h:22, + from /orin_ssd/MORB_SLAM/include/Map.h:22, + from /orin_ssd/MORB_SLAM/src/Map.cc:22: +/usr/include/c++/9/bits/shared_ptr.h:398:5: note: candidate: ‘template bool std::operator!=(const std::shared_ptr<_Tp>&, const std::shared_ptr<_Tp>&)’ + 398 | operator!=(const shared_ptr<_Tp>& __a, const shared_ptr<_Up>& __b) noexcept + | ^~~~~~~~ +/usr/include/c++/9/bits/shared_ptr.h:398:5: note: template argument deduction/substitution failed: +/orin_ssd/MORB_SLAM/src/Map.cc:339:49: note: mismatched types ‘const std::shared_ptr<_Tp>’ and ‘std::__shared_ptr::element_type*’ {aka ‘MORB_SLAM::Map*’} + 339 | if ((it->first->GetMap() != sharedMap.get() && this == sharedMap.get()) || it->first->isBad()) { + | ^ +In file included from /usr/include/c++/9/memory:81, + from /usr/include/boost/smart_ptr/scoped_ptr.hpp:22, + from /usr/include/boost/scoped_ptr.hpp:13, + from /usr/include/boost/archive/detail/basic_iarchive.hpp:23, + from /usr/include/boost/serialization/map.hpp:24, + from /orin_ssd/MORB_SLAM/include/KeyFrame.h:25, + from /orin_ssd/MORB_SLAM/include/MapPoint.h:22, + from /orin_ssd/MORB_SLAM/include/Map.h:22, + from /orin_ssd/MORB_SLAM/src/Map.cc:22: +/usr/include/c++/9/bits/shared_ptr.h:403:5: note: candidate: ‘template bool std::operator!=(const std::shared_ptr<_Tp>&, std::nullptr_t)’ + 403 | operator!=(const shared_ptr<_Tp>& __a, nullptr_t) noexcept + | ^~~~~~~~ +/usr/include/c++/9/bits/shared_ptr.h:403:5: note: template argument deduction/substitution failed: +/orin_ssd/MORB_SLAM/src/Map.cc:339:48: note: cannot convert ‘sharedMap.std::shared_ptr::.std::__shared_ptr::get()’ (type ‘std::__shared_ptr::element_type*’ {aka ‘MORB_SLAM::Map*’}) to type ‘std::nullptr_t’ + 339 | if ((it->first->GetMap() != sharedMap.get() && this == sharedMap.get()) || it->first->isBad()) { + | ~~~~~~~~~~~~~^~ +In file included from /usr/include/c++/9/memory:81, + from /usr/include/boost/smart_ptr/scoped_ptr.hpp:22, + from /usr/include/boost/scoped_ptr.hpp:13, + from /usr/include/boost/archive/detail/basic_iarchive.hpp:23, + from /usr/include/boost/serialization/map.hpp:24, + from /orin_ssd/MORB_SLAM/include/KeyFrame.h:25, + from /orin_ssd/MORB_SLAM/include/MapPoint.h:22, + from /orin_ssd/MORB_SLAM/include/Map.h:22, + from /orin_ssd/MORB_SLAM/src/Map.cc:22: +/usr/include/c++/9/bits/shared_ptr.h:408:5: note: candidate: ‘template bool std::operator!=(std::nullptr_t, const std::shared_ptr<_Tp>&)’ + 408 | operator!=(nullptr_t, const shared_ptr<_Tp>& __a) noexcept + | ^~~~~~~~ +/usr/include/c++/9/bits/shared_ptr.h:408:5: note: template argument deduction/substitution failed: +/orin_ssd/MORB_SLAM/src/Map.cc:339:49: note: mismatched types ‘const std::shared_ptr<_Tp>’ and ‘std::__shared_ptr::element_type*’ {aka ‘MORB_SLAM::Map*’} + 339 | if ((it->first->GetMap() != sharedMap.get() && this == sharedMap.get()) || it->first->isBad()) { + | ^ +In file included from /usr/include/c++/9/vector:67, + from /usr/include/boost/archive/detail/helper_collection.hpp:20, + from /usr/include/boost/archive/detail/basic_iarchive.hpp:28, + from /usr/include/boost/serialization/map.hpp:24, + from /orin_ssd/MORB_SLAM/include/KeyFrame.h:25, + from /orin_ssd/MORB_SLAM/include/MapPoint.h:22, + from /orin_ssd/MORB_SLAM/include/Map.h:22, + from /orin_ssd/MORB_SLAM/src/Map.cc:22: +/usr/include/c++/9/bits/stl_vector.h:1912:5: note: candidate: ‘template bool std::operator!=(const std::vector<_Tp, _Alloc>&, const std::vector<_Tp, _Alloc>&)’ + 1912 | operator!=(const vector<_Tp, _Alloc>& __x, const vector<_Tp, _Alloc>& __y) + | ^~~~~~~~ +/usr/include/c++/9/bits/stl_vector.h:1912:5: note: template argument deduction/substitution failed: +/orin_ssd/MORB_SLAM/src/Map.cc:339:49: note: ‘std::shared_ptr’ is not derived from ‘const std::vector<_Tp, _Alloc>’ + 339 | if ((it->first->GetMap() != sharedMap.get() && this == sharedMap.get()) || it->first->isBad()) { + | ^ +In file included from /usr/include/c++/9/functional:59, + from /usr/include/c++/9/pstl/glue_algorithm_defs.h:13, + from /usr/include/c++/9/algorithm:71, + from /usr/include/boost/archive/detail/helper_collection.hpp:23, + from /usr/include/boost/archive/detail/basic_iarchive.hpp:28, + from /usr/include/boost/serialization/map.hpp:24, + from /orin_ssd/MORB_SLAM/include/KeyFrame.h:25, + from /orin_ssd/MORB_SLAM/include/MapPoint.h:22, + from /orin_ssd/MORB_SLAM/include/Map.h:22, + from /orin_ssd/MORB_SLAM/src/Map.cc:22: +/usr/include/c++/9/bits/std_function.h:764:5: note: candidate: ‘template bool std::operator!=(const std::function<_Res(_ArgTypes ...)>&, std::nullptr_t)’ + 764 | operator!=(const function<_Res(_Args...)>& __f, nullptr_t) noexcept + | ^~~~~~~~ +/usr/include/c++/9/bits/std_function.h:764:5: note: template argument deduction/substitution failed: +/orin_ssd/MORB_SLAM/src/Map.cc:339:49: note: ‘std::shared_ptr’ is not derived from ‘const std::function<_Res(_ArgTypes ...)>’ + 339 | if ((it->first->GetMap() != sharedMap.get() && this == sharedMap.get()) || it->first->isBad()) { + | ^ +In file included from /usr/include/c++/9/functional:59, + from /usr/include/c++/9/pstl/glue_algorithm_defs.h:13, + from /usr/include/c++/9/algorithm:71, + from /usr/include/boost/archive/detail/helper_collection.hpp:23, + from /usr/include/boost/archive/detail/basic_iarchive.hpp:28, + from /usr/include/boost/serialization/map.hpp:24, + from /orin_ssd/MORB_SLAM/include/KeyFrame.h:25, + from /orin_ssd/MORB_SLAM/include/MapPoint.h:22, + from /orin_ssd/MORB_SLAM/include/Map.h:22, + from /orin_ssd/MORB_SLAM/src/Map.cc:22: +/usr/include/c++/9/bits/std_function.h:770:5: note: candidate: ‘template bool std::operator!=(std::nullptr_t, const std::function<_Res(_ArgTypes ...)>&)’ + 770 | operator!=(nullptr_t, const function<_Res(_Args...)>& __f) noexcept + | ^~~~~~~~ +/usr/include/c++/9/bits/std_function.h:770:5: note: template argument deduction/substitution failed: +/orin_ssd/MORB_SLAM/src/Map.cc:339:49: note: mismatched types ‘const std::function<_Res(_ArgTypes ...)>’ and ‘std::__shared_ptr::element_type*’ {aka ‘MORB_SLAM::Map*’} + 339 | if ((it->first->GetMap() != sharedMap.get() && this == sharedMap.get()) || it->first->isBad()) { + | ^ +In file included from /usr/include/c++/9/unordered_map:47, + from /usr/include/c++/9/functional:61, + from /usr/include/c++/9/pstl/glue_algorithm_defs.h:13, + from /usr/include/c++/9/algorithm:71, + from /usr/include/boost/archive/detail/helper_collection.hpp:23, + from /usr/include/boost/archive/detail/basic_iarchive.hpp:28, + from /usr/include/boost/serialization/map.hpp:24, + from /orin_ssd/MORB_SLAM/include/KeyFrame.h:25, + from /orin_ssd/MORB_SLAM/include/MapPoint.h:22, + from /orin_ssd/MORB_SLAM/include/Map.h:22, + from /orin_ssd/MORB_SLAM/src/Map.cc:22: +/usr/include/c++/9/bits/unordered_map.h:2099:5: note: candidate: ‘template bool std::operator!=(const std::unordered_map<_Key1, _Tp1, _Hash1, _Pred1, _Alloc1>&, const std::unordered_map<_Key1, _Tp1, _Hash1, _Pred1, _Alloc1>&)’ + 2099 | operator!=(const unordered_map<_Key, _Tp, _Hash, _Pred, _Alloc>& __x, + | ^~~~~~~~ +/usr/include/c++/9/bits/unordered_map.h:2099:5: note: template argument deduction/substitution failed: +/orin_ssd/MORB_SLAM/src/Map.cc:339:49: note: ‘std::shared_ptr’ is not derived from ‘const std::unordered_map<_Key1, _Tp1, _Hash1, _Pred1, _Alloc1>’ + 339 | if ((it->first->GetMap() != sharedMap.get() && this == sharedMap.get()) || it->first->isBad()) { + | ^ +In file included from /usr/include/c++/9/unordered_map:47, + from /usr/include/c++/9/functional:61, + from /usr/include/c++/9/pstl/glue_algorithm_defs.h:13, + from /usr/include/c++/9/algorithm:71, + from /usr/include/boost/archive/detail/helper_collection.hpp:23, + from /usr/include/boost/archive/detail/basic_iarchive.hpp:28, + from /usr/include/boost/serialization/map.hpp:24, + from /orin_ssd/MORB_SLAM/include/KeyFrame.h:25, + from /orin_ssd/MORB_SLAM/include/MapPoint.h:22, + from /orin_ssd/MORB_SLAM/include/Map.h:22, + from /orin_ssd/MORB_SLAM/src/Map.cc:22: +/usr/include/c++/9/bits/unordered_map.h:2111:5: note: candidate: ‘template bool std::operator!=(const std::unordered_multimap<_Key1, _Tp1, _Hash1, _Pred1, _Alloc1>&, const std::unordered_multimap<_Key1, _Tp1, _Hash1, _Pred1, _Alloc1>&)’ + 2111 | operator!=(const unordered_multimap<_Key, _Tp, _Hash, _Pred, _Alloc>& __x, + | ^~~~~~~~ +/usr/include/c++/9/bits/unordered_map.h:2111:5: note: template argument deduction/substitution failed: +/orin_ssd/MORB_SLAM/src/Map.cc:339:49: note: ‘std::shared_ptr’ is not derived from ‘const std::unordered_multimap<_Key1, _Tp1, _Hash1, _Pred1, _Alloc1>’ + 339 | if ((it->first->GetMap() != sharedMap.get() && this == sharedMap.get()) || it->first->isBad()) { + | ^ +In file included from /usr/include/c++/9/bits/ios_base.h:46, + from /usr/include/c++/9/ios:42, + from /usr/include/c++/9/ostream:38, + from /usr/include/c++/9/iterator:64, + from /usr/include/boost/operators.hpp:98, + from /usr/include/boost/serialization/strong_typedef.hpp:27, + from /usr/include/boost/serialization/collection_size_type.hpp:10, + from /usr/include/boost/serialization/map.hpp:27, + from /orin_ssd/MORB_SLAM/include/KeyFrame.h:25, + from /orin_ssd/MORB_SLAM/include/MapPoint.h:22, + from /orin_ssd/MORB_SLAM/include/Map.h:22, + from /orin_ssd/MORB_SLAM/src/Map.cc:22: +/usr/include/c++/9/system_error:319:3: note: candidate: ‘bool std::operator!=(const std::error_code&, const std::error_code&)’ + 319 | operator!=(const error_code& __lhs, const error_code& __rhs) noexcept + | ^~~~~~~~ +/usr/include/c++/9/system_error:319:32: note: no known conversion for argument 1 from ‘std::shared_ptr’ to ‘const std::error_code&’ + 319 | operator!=(const error_code& __lhs, const error_code& __rhs) noexcept + | ~~~~~~~~~~~~~~~~~~^~~~~ +/usr/include/c++/9/system_error:323:3: note: candidate: ‘bool std::operator!=(const std::error_code&, const std::error_condition&)’ + 323 | operator!=(const error_code& __lhs, const error_condition& __rhs) noexcept + | ^~~~~~~~ +/usr/include/c++/9/system_error:323:32: note: no known conversion for argument 1 from ‘std::shared_ptr’ to ‘const std::error_code&’ + 323 | operator!=(const error_code& __lhs, const error_condition& __rhs) noexcept + | ~~~~~~~~~~~~~~~~~~^~~~~ +/usr/include/c++/9/system_error:327:3: note: candidate: ‘bool std::operator!=(const std::error_condition&, const std::error_code&)’ + 327 | operator!=(const error_condition& __lhs, const error_code& __rhs) noexcept + | ^~~~~~~~ +/usr/include/c++/9/system_error:327:37: note: no known conversion for argument 1 from ‘std::shared_ptr’ to ‘const std::error_condition&’ + 327 | operator!=(const error_condition& __lhs, const error_code& __rhs) noexcept + | ~~~~~~~~~~~~~~~~~~~~~~~^~~~~ +/usr/include/c++/9/system_error:331:3: note: candidate: ‘bool std::operator!=(const std::error_condition&, const std::error_condition&)’ + 331 | operator!=(const error_condition& __lhs, + | ^~~~~~~~ +/usr/include/c++/9/system_error:331:37: note: no known conversion for argument 1 from ‘std::shared_ptr’ to ‘const std::error_condition&’ + 331 | operator!=(const error_condition& __lhs, + | ~~~~~~~~~~~~~~~~~~~~~~~^~~~~ +In file included from /usr/include/c++/9/bits/locale_facets.h:48, + from /usr/include/c++/9/bits/basic_ios.h:37, + from /usr/include/c++/9/ios:44, + from /usr/include/c++/9/ostream:38, + from /usr/include/c++/9/iterator:64, + from /usr/include/boost/operators.hpp:98, + from /usr/include/boost/serialization/strong_typedef.hpp:27, + from /usr/include/boost/serialization/collection_size_type.hpp:10, + from /usr/include/boost/serialization/map.hpp:27, + from /orin_ssd/MORB_SLAM/include/KeyFrame.h:25, + from /orin_ssd/MORB_SLAM/include/MapPoint.h:22, + from /orin_ssd/MORB_SLAM/include/Map.h:22, + from /orin_ssd/MORB_SLAM/src/Map.cc:22: +/usr/include/c++/9/bits/streambuf_iterator.h:214:5: note: candidate: ‘template bool std::operator!=(const std::istreambuf_iterator<_CharT, _Traits>&, const std::istreambuf_iterator<_CharT, _Traits>&)’ + 214 | operator!=(const istreambuf_iterator<_CharT, _Traits>& __a, + | ^~~~~~~~ +/usr/include/c++/9/bits/streambuf_iterator.h:214:5: note: template argument deduction/substitution failed: +/orin_ssd/MORB_SLAM/src/Map.cc:339:49: note: ‘std::shared_ptr’ is not derived from ‘const std::istreambuf_iterator<_CharT, _Traits>’ + 339 | if ((it->first->GetMap() != sharedMap.get() && this == sharedMap.get()) || it->first->isBad()) { + | ^ +In file included from /usr/include/c++/9/iterator:66, + from /usr/include/boost/operators.hpp:98, + from /usr/include/boost/serialization/strong_typedef.hpp:27, + from /usr/include/boost/serialization/collection_size_type.hpp:10, + from /usr/include/boost/serialization/map.hpp:27, + from /orin_ssd/MORB_SLAM/include/KeyFrame.h:25, + from /orin_ssd/MORB_SLAM/include/MapPoint.h:22, + from /orin_ssd/MORB_SLAM/include/Map.h:22, + from /orin_ssd/MORB_SLAM/src/Map.cc:22: +/usr/include/c++/9/bits/stream_iterator.h:141:5: note: candidate: ‘template bool std::operator!=(const std::istream_iterator<_Tp, _CharT, _Traits, _Dist>&, const std::istream_iterator<_Tp, _CharT, _Traits, _Dist>&)’ + 141 | operator!=(const istream_iterator<_Tp, _CharT, _Traits, _Dist>& __x, + | ^~~~~~~~ +/usr/include/c++/9/bits/stream_iterator.h:141:5: note: template argument deduction/substitution failed: +/orin_ssd/MORB_SLAM/src/Map.cc:339:49: note: ‘std::shared_ptr’ is not derived from ‘const std::istream_iterator<_Tp, _CharT, _Traits, _Dist>’ + 339 | if ((it->first->GetMap() != sharedMap.get() && this == sharedMap.get()) || it->first->isBad()) { + | ^ +In file included from /orin_ssd/MORB_SLAM/Thirdparty/Sophus/sophus/so2.hpp:7, + from /orin_ssd/MORB_SLAM/Thirdparty/Sophus/sophus/se2.hpp:7, + from /orin_ssd/MORB_SLAM/Thirdparty/Sophus/sophus/geometry.hpp:7, + from /orin_ssd/MORB_SLAM/include/Frame.h:27, + from /orin_ssd/MORB_SLAM/include/KeyFrame.h:31, + from /orin_ssd/MORB_SLAM/include/MapPoint.h:22, + from /orin_ssd/MORB_SLAM/include/Map.h:22, + from /orin_ssd/MORB_SLAM/src/Map.cc:22: +/usr/include/c++/9/complex:481:5: note: candidate: ‘template constexpr bool std::operator!=(const std::complex<_Tp>&, const std::complex<_Tp>&)’ + 481 | operator!=(const complex<_Tp>& __x, const complex<_Tp>& __y) + | ^~~~~~~~ +/usr/include/c++/9/complex:481:5: note: template argument deduction/substitution failed: +/orin_ssd/MORB_SLAM/src/Map.cc:339:49: note: ‘std::shared_ptr’ is not derived from ‘const std::complex<_Tp>’ + 339 | if ((it->first->GetMap() != sharedMap.get() && this == sharedMap.get()) || it->first->isBad()) { + | ^ +In file included from /orin_ssd/MORB_SLAM/Thirdparty/Sophus/sophus/so2.hpp:7, + from /orin_ssd/MORB_SLAM/Thirdparty/Sophus/sophus/se2.hpp:7, + from /orin_ssd/MORB_SLAM/Thirdparty/Sophus/sophus/geometry.hpp:7, + from /orin_ssd/MORB_SLAM/include/Frame.h:27, + from /orin_ssd/MORB_SLAM/include/KeyFrame.h:31, + from /orin_ssd/MORB_SLAM/include/MapPoint.h:22, + from /orin_ssd/MORB_SLAM/include/Map.h:22, + from /orin_ssd/MORB_SLAM/src/Map.cc:22: +/usr/include/c++/9/complex:486:5: note: candidate: ‘template constexpr bool std::operator!=(const std::complex<_Tp>&, const _Tp&)’ + 486 | operator!=(const complex<_Tp>& __x, const _Tp& __y) + | ^~~~~~~~ +/usr/include/c++/9/complex:486:5: note: template argument deduction/substitution failed: +/orin_ssd/MORB_SLAM/src/Map.cc:339:49: note: ‘std::shared_ptr’ is not derived from ‘const std::complex<_Tp>’ + 339 | if ((it->first->GetMap() != sharedMap.get() && this == sharedMap.get()) || it->first->isBad()) { + | ^ +In file included from /orin_ssd/MORB_SLAM/Thirdparty/Sophus/sophus/so2.hpp:7, + from /orin_ssd/MORB_SLAM/Thirdparty/Sophus/sophus/se2.hpp:7, + from /orin_ssd/MORB_SLAM/Thirdparty/Sophus/sophus/geometry.hpp:7, + from /orin_ssd/MORB_SLAM/include/Frame.h:27, + from /orin_ssd/MORB_SLAM/include/KeyFrame.h:31, + from /orin_ssd/MORB_SLAM/include/MapPoint.h:22, + from /orin_ssd/MORB_SLAM/include/Map.h:22, + from /orin_ssd/MORB_SLAM/src/Map.cc:22: +/usr/include/c++/9/complex:491:5: note: candidate: ‘template constexpr bool std::operator!=(const _Tp&, const std::complex<_Tp>&)’ + 491 | operator!=(const _Tp& __x, const complex<_Tp>& __y) + | ^~~~~~~~ +/usr/include/c++/9/complex:491:5: note: template argument deduction/substitution failed: +/orin_ssd/MORB_SLAM/src/Map.cc:339:49: note: mismatched types ‘const std::complex<_Tp>’ and ‘std::__shared_ptr::element_type*’ {aka ‘MORB_SLAM::Map*’} + 339 | if ((it->first->GetMap() != sharedMap.get() && this == sharedMap.get()) || it->first->isBad()) { + | ^ +In file included from /usr/include/c++/9/random:49, + from /orin_ssd/MORB_SLAM/Thirdparty/Sophus/sophus/common.hpp:10, + from /orin_ssd/MORB_SLAM/Thirdparty/Sophus/sophus/types.hpp:8, + from /orin_ssd/MORB_SLAM/Thirdparty/Sophus/sophus/rotation_matrix.hpp:10, + from /orin_ssd/MORB_SLAM/Thirdparty/Sophus/sophus/so2.hpp:14, + from /orin_ssd/MORB_SLAM/Thirdparty/Sophus/sophus/se2.hpp:7, + from /orin_ssd/MORB_SLAM/Thirdparty/Sophus/sophus/geometry.hpp:7, + from /orin_ssd/MORB_SLAM/include/Frame.h:27, + from /orin_ssd/MORB_SLAM/include/KeyFrame.h:31, + from /orin_ssd/MORB_SLAM/include/MapPoint.h:22, + from /orin_ssd/MORB_SLAM/include/Map.h:22, + from /orin_ssd/MORB_SLAM/src/Map.cc:22: +/usr/include/c++/9/bits/random.h:421:5: note: candidate: ‘template bool std::operator!=(const std::linear_congruential_engine<_UIntType, __a, __c, __m>&, const std::linear_congruential_engine<_UIntType, __a, __c, __m>&)’ + 421 | operator!=(const std::linear_congruential_engine<_UIntType, __a, + | ^~~~~~~~ +/usr/include/c++/9/bits/random.h:421:5: note: template argument deduction/substitution failed: +/orin_ssd/MORB_SLAM/src/Map.cc:339:49: note: ‘std::shared_ptr’ is not derived from ‘const std::linear_congruential_engine<_UIntType, __a, __c, __m>’ + 339 | if ((it->first->GetMap() != sharedMap.get() && this == sharedMap.get()) || it->first->isBad()) { + | ^ +In file included from /usr/include/c++/9/random:49, + from /orin_ssd/MORB_SLAM/Thirdparty/Sophus/sophus/common.hpp:10, + from /orin_ssd/MORB_SLAM/Thirdparty/Sophus/sophus/types.hpp:8, + from /orin_ssd/MORB_SLAM/Thirdparty/Sophus/sophus/rotation_matrix.hpp:10, + from /orin_ssd/MORB_SLAM/Thirdparty/Sophus/sophus/so2.hpp:14, + from /orin_ssd/MORB_SLAM/Thirdparty/Sophus/sophus/se2.hpp:7, + from /orin_ssd/MORB_SLAM/Thirdparty/Sophus/sophus/geometry.hpp:7, + from /orin_ssd/MORB_SLAM/include/Frame.h:27, + from /orin_ssd/MORB_SLAM/include/KeyFrame.h:31, + from /orin_ssd/MORB_SLAM/include/MapPoint.h:22, + from /orin_ssd/MORB_SLAM/include/Map.h:22, + from /orin_ssd/MORB_SLAM/src/Map.cc:22: +/usr/include/c++/9/bits/random.h:658:5: note: candidate: ‘template bool std::operator!=(const std::mersenne_twister_engine<_UIntType, __w, __n, __m, __r, __a, __u, __d, __s, __b, __t, __c, __l, __f>&, const std::mersenne_twister_engine<_UIntType, __w, __n, __m, __r, __a, __u, __d, __s, __b, __t, __c, __l, __f>&)’ + 658 | operator!=(const std::mersenne_twister_engine<_UIntType, __w, __n, __m, + | ^~~~~~~~ +/usr/include/c++/9/bits/random.h:658:5: note: template argument deduction/substitution failed: +/orin_ssd/MORB_SLAM/src/Map.cc:339:49: note: ‘std::shared_ptr’ is not derived from ‘const std::mersenne_twister_engine<_UIntType, __w, __n, __m, __r, __a, __u, __d, __s, __b, __t, __c, __l, __f>’ + 339 | if ((it->first->GetMap() != sharedMap.get() && this == sharedMap.get()) || it->first->isBad()) { + | ^ +In file included from /usr/include/c++/9/random:49, + from /orin_ssd/MORB_SLAM/Thirdparty/Sophus/sophus/common.hpp:10, + from /orin_ssd/MORB_SLAM/Thirdparty/Sophus/sophus/types.hpp:8, + from /orin_ssd/MORB_SLAM/Thirdparty/Sophus/sophus/rotation_matrix.hpp:10, + from /orin_ssd/MORB_SLAM/Thirdparty/Sophus/sophus/so2.hpp:14, + from /orin_ssd/MORB_SLAM/Thirdparty/Sophus/sophus/se2.hpp:7, + from /orin_ssd/MORB_SLAM/Thirdparty/Sophus/sophus/geometry.hpp:7, + from /orin_ssd/MORB_SLAM/include/Frame.h:27, + from /orin_ssd/MORB_SLAM/include/KeyFrame.h:31, + from /orin_ssd/MORB_SLAM/include/MapPoint.h:22, + from /orin_ssd/MORB_SLAM/include/Map.h:22, + from /orin_ssd/MORB_SLAM/src/Map.cc:22: +/usr/include/c++/9/bits/random.h:859:5: note: candidate: ‘template bool std::operator!=(const std::subtract_with_carry_engine<_UIntType, __w, __s, __r>&, const std::subtract_with_carry_engine<_UIntType, __w, __s, __r>&)’ + 859 | operator!=(const std::subtract_with_carry_engine<_UIntType, __w, + | ^~~~~~~~ +/usr/include/c++/9/bits/random.h:859:5: note: template argument deduction/substitution failed: +/orin_ssd/MORB_SLAM/src/Map.cc:339:49: note: ‘std::shared_ptr’ is not derived from ‘const std::subtract_with_carry_engine<_UIntType, __w, __s, __r>’ + 339 | if ((it->first->GetMap() != sharedMap.get() && this == sharedMap.get()) || it->first->isBad()) { + | ^ +In file included from /usr/include/c++/9/random:49, + from /orin_ssd/MORB_SLAM/Thirdparty/Sophus/sophus/common.hpp:10, + from /orin_ssd/MORB_SLAM/Thirdparty/Sophus/sophus/types.hpp:8, + from /orin_ssd/MORB_SLAM/Thirdparty/Sophus/sophus/rotation_matrix.hpp:10, + from /orin_ssd/MORB_SLAM/Thirdparty/Sophus/sophus/so2.hpp:14, + from /orin_ssd/MORB_SLAM/Thirdparty/Sophus/sophus/se2.hpp:7, + from /orin_ssd/MORB_SLAM/Thirdparty/Sophus/sophus/geometry.hpp:7, + from /orin_ssd/MORB_SLAM/include/Frame.h:27, + from /orin_ssd/MORB_SLAM/include/KeyFrame.h:31, + from /orin_ssd/MORB_SLAM/include/MapPoint.h:22, + from /orin_ssd/MORB_SLAM/include/Map.h:22, + from /orin_ssd/MORB_SLAM/src/Map.cc:22: +/usr/include/c++/9/bits/random.h:1082:5: note: candidate: ‘template bool std::operator!=(const std::discard_block_engine<_RandomNumberEngine, __p, __r>&, const std::discard_block_engine<_RandomNumberEngine, __p, __r>&)’ + 1082 | operator!=(const std::discard_block_engine<_RandomNumberEngine, __p, + | ^~~~~~~~ +/usr/include/c++/9/bits/random.h:1082:5: note: template argument deduction/substitution failed: +/orin_ssd/MORB_SLAM/src/Map.cc:339:49: note: ‘std::shared_ptr’ is not derived from ‘const std::discard_block_engine<_RandomNumberEngine, __p, __r>’ + 339 | if ((it->first->GetMap() != sharedMap.get() && this == sharedMap.get()) || it->first->isBad()) { + | ^ +In file included from /usr/include/c++/9/random:49, + from /orin_ssd/MORB_SLAM/Thirdparty/Sophus/sophus/common.hpp:10, + from /orin_ssd/MORB_SLAM/Thirdparty/Sophus/sophus/types.hpp:8, + from /orin_ssd/MORB_SLAM/Thirdparty/Sophus/sophus/rotation_matrix.hpp:10, + from /orin_ssd/MORB_SLAM/Thirdparty/Sophus/sophus/so2.hpp:14, + from /orin_ssd/MORB_SLAM/Thirdparty/Sophus/sophus/se2.hpp:7, + from /orin_ssd/MORB_SLAM/Thirdparty/Sophus/sophus/geometry.hpp:7, + from /orin_ssd/MORB_SLAM/include/Frame.h:27, + from /orin_ssd/MORB_SLAM/include/KeyFrame.h:31, + from /orin_ssd/MORB_SLAM/include/MapPoint.h:22, + from /orin_ssd/MORB_SLAM/include/Map.h:22, + from /orin_ssd/MORB_SLAM/src/Map.cc:22: +/usr/include/c++/9/bits/random.h:1279:5: note: candidate: ‘template bool std::operator!=(const std::independent_bits_engine<_RandomNumberEngine, __w, _UIntType>&, const std::independent_bits_engine<_RandomNumberEngine, __w, _UIntType>&)’ + 1279 | operator!=(const std::independent_bits_engine<_RandomNumberEngine, __w, + | ^~~~~~~~ +/usr/include/c++/9/bits/random.h:1279:5: note: template argument deduction/substitution failed: +/orin_ssd/MORB_SLAM/src/Map.cc:339:49: note: ‘std::shared_ptr’ is not derived from ‘const std::independent_bits_engine<_RandomNumberEngine, __w, _UIntType>’ + 339 | if ((it->first->GetMap() != sharedMap.get() && this == sharedMap.get()) || it->first->isBad()) { + | ^ +In file included from /usr/include/c++/9/random:49, + from /orin_ssd/MORB_SLAM/Thirdparty/Sophus/sophus/common.hpp:10, + from /orin_ssd/MORB_SLAM/Thirdparty/Sophus/sophus/types.hpp:8, + from /orin_ssd/MORB_SLAM/Thirdparty/Sophus/sophus/rotation_matrix.hpp:10, + from /orin_ssd/MORB_SLAM/Thirdparty/Sophus/sophus/so2.hpp:14, + from /orin_ssd/MORB_SLAM/Thirdparty/Sophus/sophus/se2.hpp:7, + from /orin_ssd/MORB_SLAM/Thirdparty/Sophus/sophus/geometry.hpp:7, + from /orin_ssd/MORB_SLAM/include/Frame.h:27, + from /orin_ssd/MORB_SLAM/include/KeyFrame.h:31, + from /orin_ssd/MORB_SLAM/include/MapPoint.h:22, + from /orin_ssd/MORB_SLAM/include/Map.h:22, + from /orin_ssd/MORB_SLAM/src/Map.cc:22: +/usr/include/c++/9/bits/random.h:1532:5: note: candidate: ‘template bool std::operator!=(const std::shuffle_order_engine<_RandomNumberEngine, __k>&, const std::shuffle_order_engine<_RandomNumberEngine, __k>&)’ + 1532 | operator!=(const std::shuffle_order_engine<_RandomNumberEngine, + | ^~~~~~~~ +/usr/include/c++/9/bits/random.h:1532:5: note: template argument deduction/substitution failed: +/orin_ssd/MORB_SLAM/src/Map.cc:339:49: note: ‘std::shared_ptr’ is not derived from ‘const std::shuffle_order_engine<_RandomNumberEngine, __k>’ + 339 | if ((it->first->GetMap() != sharedMap.get() && this == sharedMap.get()) || it->first->isBad()) { + | ^ +In file included from /usr/include/c++/9/random:49, + from /orin_ssd/MORB_SLAM/Thirdparty/Sophus/sophus/common.hpp:10, + from /orin_ssd/MORB_SLAM/Thirdparty/Sophus/sophus/types.hpp:8, + from /orin_ssd/MORB_SLAM/Thirdparty/Sophus/sophus/rotation_matrix.hpp:10, + from /orin_ssd/MORB_SLAM/Thirdparty/Sophus/sophus/so2.hpp:14, + from /orin_ssd/MORB_SLAM/Thirdparty/Sophus/sophus/se2.hpp:7, + from /orin_ssd/MORB_SLAM/Thirdparty/Sophus/sophus/geometry.hpp:7, + from /orin_ssd/MORB_SLAM/include/Frame.h:27, + from /orin_ssd/MORB_SLAM/include/KeyFrame.h:31, + from /orin_ssd/MORB_SLAM/include/MapPoint.h:22, + from /orin_ssd/MORB_SLAM/include/Map.h:22, + from /orin_ssd/MORB_SLAM/src/Map.cc:22: +/usr/include/c++/9/bits/random.h:1692:5: note: candidate: ‘template bool std::operator!=(const std::uniform_int_distribution<_IntType>&, const std::uniform_int_distribution<_IntType>&)’ + 1692 | operator!=(const std::uniform_int_distribution<_IntType>& __d1, + | ^~~~~~~~ +/usr/include/c++/9/bits/random.h:1692:5: note: template argument deduction/substitution failed: +/orin_ssd/MORB_SLAM/src/Map.cc:339:49: note: ‘std::shared_ptr’ is not derived from ‘const std::uniform_int_distribution<_IntType>’ + 339 | if ((it->first->GetMap() != sharedMap.get() && this == sharedMap.get()) || it->first->isBad()) { + | ^ +In file included from /usr/include/c++/9/random:49, + from /orin_ssd/MORB_SLAM/Thirdparty/Sophus/sophus/common.hpp:10, + from /orin_ssd/MORB_SLAM/Thirdparty/Sophus/sophus/types.hpp:8, + from /orin_ssd/MORB_SLAM/Thirdparty/Sophus/sophus/rotation_matrix.hpp:10, + from /orin_ssd/MORB_SLAM/Thirdparty/Sophus/sophus/so2.hpp:14, + from /orin_ssd/MORB_SLAM/Thirdparty/Sophus/sophus/se2.hpp:7, + from /orin_ssd/MORB_SLAM/Thirdparty/Sophus/sophus/geometry.hpp:7, + from /orin_ssd/MORB_SLAM/include/Frame.h:27, + from /orin_ssd/MORB_SLAM/include/KeyFrame.h:31, + from /orin_ssd/MORB_SLAM/include/MapPoint.h:22, + from /orin_ssd/MORB_SLAM/include/Map.h:22, + from /orin_ssd/MORB_SLAM/src/Map.cc:22: +/usr/include/c++/9/bits/random.h:1913:5: note: candidate: ‘template bool std::operator!=(const std::uniform_real_distribution<_IntType>&, const std::uniform_real_distribution<_IntType>&)’ + 1913 | operator!=(const std::uniform_real_distribution<_IntType>& __d1, + | ^~~~~~~~ +/usr/include/c++/9/bits/random.h:1913:5: note: template argument deduction/substitution failed: +/orin_ssd/MORB_SLAM/src/Map.cc:339:49: note: ‘std::shared_ptr’ is not derived from ‘const std::uniform_real_distribution<_IntType>’ + 339 | if ((it->first->GetMap() != sharedMap.get() && this == sharedMap.get()) || it->first->isBad()) { + | ^ +In file included from /usr/include/c++/9/random:49, + from /orin_ssd/MORB_SLAM/Thirdparty/Sophus/sophus/common.hpp:10, + from /orin_ssd/MORB_SLAM/Thirdparty/Sophus/sophus/types.hpp:8, + from /orin_ssd/MORB_SLAM/Thirdparty/Sophus/sophus/rotation_matrix.hpp:10, + from /orin_ssd/MORB_SLAM/Thirdparty/Sophus/sophus/so2.hpp:14, + from /orin_ssd/MORB_SLAM/Thirdparty/Sophus/sophus/se2.hpp:7, + from /orin_ssd/MORB_SLAM/Thirdparty/Sophus/sophus/geometry.hpp:7, + from /orin_ssd/MORB_SLAM/include/Frame.h:27, + from /orin_ssd/MORB_SLAM/include/KeyFrame.h:31, + from /orin_ssd/MORB_SLAM/include/MapPoint.h:22, + from /orin_ssd/MORB_SLAM/include/Map.h:22, + from /orin_ssd/MORB_SLAM/src/Map.cc:22: +/usr/include/c++/9/bits/random.h:2170:5: note: candidate: ‘template bool std::operator!=(const std::normal_distribution<_RealType>&, const std::normal_distribution<_RealType>&)’ + 2170 | operator!=(const std::normal_distribution<_RealType>& __d1, + | ^~~~~~~~ +/usr/include/c++/9/bits/random.h:2170:5: note: template argument deduction/substitution failed: +/orin_ssd/MORB_SLAM/src/Map.cc:339:49: note: ‘std::shared_ptr’ is not derived from ‘const std::normal_distribution<_RealType>’ + 339 | if ((it->first->GetMap() != sharedMap.get() && this == sharedMap.get()) || it->first->isBad()) { + | ^ +In file included from /usr/include/c++/9/random:49, + from /orin_ssd/MORB_SLAM/Thirdparty/Sophus/sophus/common.hpp:10, + from /orin_ssd/MORB_SLAM/Thirdparty/Sophus/sophus/types.hpp:8, + from /orin_ssd/MORB_SLAM/Thirdparty/Sophus/sophus/rotation_matrix.hpp:10, + from /orin_ssd/MORB_SLAM/Thirdparty/Sophus/sophus/so2.hpp:14, + from /orin_ssd/MORB_SLAM/Thirdparty/Sophus/sophus/se2.hpp:7, + from /orin_ssd/MORB_SLAM/Thirdparty/Sophus/sophus/geometry.hpp:7, + from /orin_ssd/MORB_SLAM/include/Frame.h:27, + from /orin_ssd/MORB_SLAM/include/KeyFrame.h:31, + from /orin_ssd/MORB_SLAM/include/MapPoint.h:22, + from /orin_ssd/MORB_SLAM/include/Map.h:22, + from /orin_ssd/MORB_SLAM/src/Map.cc:22: +/usr/include/c++/9/bits/random.h:2381:5: note: candidate: ‘template bool std::operator!=(const std::lognormal_distribution<_RealType>&, const std::lognormal_distribution<_RealType>&)’ + 2381 | operator!=(const std::lognormal_distribution<_RealType>& __d1, + | ^~~~~~~~ +/usr/include/c++/9/bits/random.h:2381:5: note: template argument deduction/substitution failed: +/orin_ssd/MORB_SLAM/src/Map.cc:339:49: note: ‘std::shared_ptr’ is not derived from ‘const std::lognormal_distribution<_RealType>’ + 339 | if ((it->first->GetMap() != sharedMap.get() && this == sharedMap.get()) || it->first->isBad()) { + | ^ +In file included from /usr/include/c++/9/random:49, + from /orin_ssd/MORB_SLAM/Thirdparty/Sophus/sophus/common.hpp:10, + from /orin_ssd/MORB_SLAM/Thirdparty/Sophus/sophus/types.hpp:8, + from /orin_ssd/MORB_SLAM/Thirdparty/Sophus/sophus/rotation_matrix.hpp:10, + from /orin_ssd/MORB_SLAM/Thirdparty/Sophus/sophus/so2.hpp:14, + from /orin_ssd/MORB_SLAM/Thirdparty/Sophus/sophus/se2.hpp:7, + from /orin_ssd/MORB_SLAM/Thirdparty/Sophus/sophus/geometry.hpp:7, + from /orin_ssd/MORB_SLAM/include/Frame.h:27, + from /orin_ssd/MORB_SLAM/include/KeyFrame.h:31, + from /orin_ssd/MORB_SLAM/include/MapPoint.h:22, + from /orin_ssd/MORB_SLAM/include/Map.h:22, + from /orin_ssd/MORB_SLAM/src/Map.cc:22: +/usr/include/c++/9/bits/random.h:2612:6: note: candidate: ‘template bool std::operator!=(const std::gamma_distribution<_RealType>&, const std::gamma_distribution<_RealType>&)’ + 2612 | operator!=(const std::gamma_distribution<_RealType>& __d1, + | ^~~~~~~~ +/usr/include/c++/9/bits/random.h:2612:6: note: template argument deduction/substitution failed: +/orin_ssd/MORB_SLAM/src/Map.cc:339:49: note: ‘std::shared_ptr’ is not derived from ‘const std::gamma_distribution<_RealType>’ + 339 | if ((it->first->GetMap() != sharedMap.get() && this == sharedMap.get()) || it->first->isBad()) { + | ^ +In file included from /usr/include/c++/9/random:49, + from /orin_ssd/MORB_SLAM/Thirdparty/Sophus/sophus/common.hpp:10, + from /orin_ssd/MORB_SLAM/Thirdparty/Sophus/sophus/types.hpp:8, + from /orin_ssd/MORB_SLAM/Thirdparty/Sophus/sophus/rotation_matrix.hpp:10, + from /orin_ssd/MORB_SLAM/Thirdparty/Sophus/sophus/so2.hpp:14, + from /orin_ssd/MORB_SLAM/Thirdparty/Sophus/sophus/se2.hpp:7, + from /orin_ssd/MORB_SLAM/Thirdparty/Sophus/sophus/geometry.hpp:7, + from /orin_ssd/MORB_SLAM/include/Frame.h:27, + from /orin_ssd/MORB_SLAM/include/KeyFrame.h:31, + from /orin_ssd/MORB_SLAM/include/MapPoint.h:22, + from /orin_ssd/MORB_SLAM/include/Map.h:22, + from /orin_ssd/MORB_SLAM/src/Map.cc:22: +/usr/include/c++/9/bits/random.h:2836:5: note: candidate: ‘template bool std::operator!=(const std::chi_squared_distribution<_RealType>&, const std::chi_squared_distribution<_RealType>&)’ + 2836 | operator!=(const std::chi_squared_distribution<_RealType>& __d1, + | ^~~~~~~~ +/usr/include/c++/9/bits/random.h:2836:5: note: template argument deduction/substitution failed: +/orin_ssd/MORB_SLAM/src/Map.cc:339:49: note: ‘std::shared_ptr’ is not derived from ‘const std::chi_squared_distribution<_RealType>’ + 339 | if ((it->first->GetMap() != sharedMap.get() && this == sharedMap.get()) || it->first->isBad()) { + | ^ +In file included from /usr/include/c++/9/random:49, + from /orin_ssd/MORB_SLAM/Thirdparty/Sophus/sophus/common.hpp:10, + from /orin_ssd/MORB_SLAM/Thirdparty/Sophus/sophus/types.hpp:8, + from /orin_ssd/MORB_SLAM/Thirdparty/Sophus/sophus/rotation_matrix.hpp:10, + from /orin_ssd/MORB_SLAM/Thirdparty/Sophus/sophus/so2.hpp:14, + from /orin_ssd/MORB_SLAM/Thirdparty/Sophus/sophus/se2.hpp:7, + from /orin_ssd/MORB_SLAM/Thirdparty/Sophus/sophus/geometry.hpp:7, + from /orin_ssd/MORB_SLAM/include/Frame.h:27, + from /orin_ssd/MORB_SLAM/include/KeyFrame.h:31, + from /orin_ssd/MORB_SLAM/include/MapPoint.h:22, + from /orin_ssd/MORB_SLAM/include/Map.h:22, + from /orin_ssd/MORB_SLAM/src/Map.cc:22: +/usr/include/c++/9/bits/random.h:3010:5: note: candidate: ‘template bool std::operator!=(const std::cauchy_distribution<_RealType>&, const std::cauchy_distribution<_RealType>&)’ + 3010 | operator!=(const std::cauchy_distribution<_RealType>& __d1, + | ^~~~~~~~ +/usr/include/c++/9/bits/random.h:3010:5: note: template argument deduction/substitution failed: +/orin_ssd/MORB_SLAM/src/Map.cc:339:49: note: ‘std::shared_ptr’ is not derived from ‘const std::cauchy_distribution<_RealType>’ + 339 | if ((it->first->GetMap() != sharedMap.get() && this == sharedMap.get()) || it->first->isBad()) { + | ^ +In file included from /usr/include/c++/9/random:49, + from /orin_ssd/MORB_SLAM/Thirdparty/Sophus/sophus/common.hpp:10, + from /orin_ssd/MORB_SLAM/Thirdparty/Sophus/sophus/types.hpp:8, + from /orin_ssd/MORB_SLAM/Thirdparty/Sophus/sophus/rotation_matrix.hpp:10, + from /orin_ssd/MORB_SLAM/Thirdparty/Sophus/sophus/so2.hpp:14, + from /orin_ssd/MORB_SLAM/Thirdparty/Sophus/sophus/se2.hpp:7, + from /orin_ssd/MORB_SLAM/Thirdparty/Sophus/sophus/geometry.hpp:7, + from /orin_ssd/MORB_SLAM/include/Frame.h:27, + from /orin_ssd/MORB_SLAM/include/KeyFrame.h:31, + from /orin_ssd/MORB_SLAM/include/MapPoint.h:22, + from /orin_ssd/MORB_SLAM/include/Map.h:22, + from /orin_ssd/MORB_SLAM/src/Map.cc:22: +/usr/include/c++/9/bits/random.h:3274:5: note: candidate: ‘template bool std::operator!=(const std::fisher_f_distribution<_RealType>&, const std::fisher_f_distribution<_RealType>&)’ + 3274 | operator!=(const std::fisher_f_distribution<_RealType>& __d1, + | ^~~~~~~~ +/usr/include/c++/9/bits/random.h:3274:5: note: template argument deduction/substitution failed: +/orin_ssd/MORB_SLAM/src/Map.cc:339:49: note: ‘std::shared_ptr’ is not derived from ‘const std::fisher_f_distribution<_RealType>’ + 339 | if ((it->first->GetMap() != sharedMap.get() && this == sharedMap.get()) || it->first->isBad()) { + | ^ +In file included from /usr/include/c++/9/random:49, + from /orin_ssd/MORB_SLAM/Thirdparty/Sophus/sophus/common.hpp:10, + from /orin_ssd/MORB_SLAM/Thirdparty/Sophus/sophus/types.hpp:8, + from /orin_ssd/MORB_SLAM/Thirdparty/Sophus/sophus/rotation_matrix.hpp:10, + from /orin_ssd/MORB_SLAM/Thirdparty/Sophus/sophus/so2.hpp:14, + from /orin_ssd/MORB_SLAM/Thirdparty/Sophus/sophus/se2.hpp:7, + from /orin_ssd/MORB_SLAM/Thirdparty/Sophus/sophus/geometry.hpp:7, + from /orin_ssd/MORB_SLAM/include/Frame.h:27, + from /orin_ssd/MORB_SLAM/include/KeyFrame.h:31, + from /orin_ssd/MORB_SLAM/include/MapPoint.h:22, + from /orin_ssd/MORB_SLAM/include/Map.h:22, + from /orin_ssd/MORB_SLAM/src/Map.cc:22: +/usr/include/c++/9/bits/random.h:3496:5: note: candidate: ‘template bool std::operator!=(const std::student_t_distribution<_RealType>&, const std::student_t_distribution<_RealType>&)’ + 3496 | operator!=(const std::student_t_distribution<_RealType>& __d1, + | ^~~~~~~~ +/usr/include/c++/9/bits/random.h:3496:5: note: template argument deduction/substitution failed: +/orin_ssd/MORB_SLAM/src/Map.cc:339:49: note: ‘std::shared_ptr’ is not derived from ‘const std::student_t_distribution<_RealType>’ + 339 | if ((it->first->GetMap() != sharedMap.get() && this == sharedMap.get()) || it->first->isBad()) { + | ^ +In file included from /usr/include/c++/9/random:49, + from /orin_ssd/MORB_SLAM/Thirdparty/Sophus/sophus/common.hpp:10, + from /orin_ssd/MORB_SLAM/Thirdparty/Sophus/sophus/types.hpp:8, + from /orin_ssd/MORB_SLAM/Thirdparty/Sophus/sophus/rotation_matrix.hpp:10, + from /orin_ssd/MORB_SLAM/Thirdparty/Sophus/sophus/so2.hpp:14, + from /orin_ssd/MORB_SLAM/Thirdparty/Sophus/sophus/se2.hpp:7, + from /orin_ssd/MORB_SLAM/Thirdparty/Sophus/sophus/geometry.hpp:7, + from /orin_ssd/MORB_SLAM/include/Frame.h:27, + from /orin_ssd/MORB_SLAM/include/KeyFrame.h:31, + from /orin_ssd/MORB_SLAM/include/MapPoint.h:22, + from /orin_ssd/MORB_SLAM/include/Map.h:22, + from /orin_ssd/MORB_SLAM/src/Map.cc:22: +/usr/include/c++/9/bits/random.h:3684:3: note: candidate: ‘bool std::operator!=(const std::bernoulli_distribution&, const std::bernoulli_distribution&)’ + 3684 | operator!=(const std::bernoulli_distribution& __d1, + | ^~~~~~~~ +/usr/include/c++/9/bits/random.h:3684:49: note: no known conversion for argument 1 from ‘std::shared_ptr’ to ‘const std::bernoulli_distribution&’ + 3684 | operator!=(const std::bernoulli_distribution& __d1, + | ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~^~~~ +/usr/include/c++/9/bits/random.h:3959:5: note: candidate: ‘template bool std::operator!=(const std::binomial_distribution<_IntType>&, const std::binomial_distribution<_IntType>&)’ + 3959 | operator!=(const std::binomial_distribution<_IntType>& __d1, + | ^~~~~~~~ +/usr/include/c++/9/bits/random.h:3959:5: note: template argument deduction/substitution failed: +/orin_ssd/MORB_SLAM/src/Map.cc:339:49: note: ‘std::shared_ptr’ is not derived from ‘const std::binomial_distribution<_IntType>’ + 339 | if ((it->first->GetMap() != sharedMap.get() && this == sharedMap.get()) || it->first->isBad()) { + | ^ +In file included from /usr/include/c++/9/random:49, + from /orin_ssd/MORB_SLAM/Thirdparty/Sophus/sophus/common.hpp:10, + from /orin_ssd/MORB_SLAM/Thirdparty/Sophus/sophus/types.hpp:8, + from /orin_ssd/MORB_SLAM/Thirdparty/Sophus/sophus/rotation_matrix.hpp:10, + from /orin_ssd/MORB_SLAM/Thirdparty/Sophus/sophus/so2.hpp:14, + from /orin_ssd/MORB_SLAM/Thirdparty/Sophus/sophus/se2.hpp:7, + from /orin_ssd/MORB_SLAM/Thirdparty/Sophus/sophus/geometry.hpp:7, + from /orin_ssd/MORB_SLAM/include/Frame.h:27, + from /orin_ssd/MORB_SLAM/include/KeyFrame.h:31, + from /orin_ssd/MORB_SLAM/include/MapPoint.h:22, + from /orin_ssd/MORB_SLAM/include/Map.h:22, + from /orin_ssd/MORB_SLAM/src/Map.cc:22: +/usr/include/c++/9/bits/random.h:4138:5: note: candidate: ‘template bool std::operator!=(const std::geometric_distribution<_IntType>&, const std::geometric_distribution<_IntType>&)’ + 4138 | operator!=(const std::geometric_distribution<_IntType>& __d1, + | ^~~~~~~~ +/usr/include/c++/9/bits/random.h:4138:5: note: template argument deduction/substitution failed: +/orin_ssd/MORB_SLAM/src/Map.cc:339:49: note: ‘std::shared_ptr’ is not derived from ‘const std::geometric_distribution<_IntType>’ + 339 | if ((it->first->GetMap() != sharedMap.get() && this == sharedMap.get()) || it->first->isBad()) { + | ^ +In file included from /usr/include/c++/9/random:49, + from /orin_ssd/MORB_SLAM/Thirdparty/Sophus/sophus/common.hpp:10, + from /orin_ssd/MORB_SLAM/Thirdparty/Sophus/sophus/types.hpp:8, + from /orin_ssd/MORB_SLAM/Thirdparty/Sophus/sophus/rotation_matrix.hpp:10, + from /orin_ssd/MORB_SLAM/Thirdparty/Sophus/sophus/so2.hpp:14, + from /orin_ssd/MORB_SLAM/Thirdparty/Sophus/sophus/se2.hpp:7, + from /orin_ssd/MORB_SLAM/Thirdparty/Sophus/sophus/geometry.hpp:7, + from /orin_ssd/MORB_SLAM/include/Frame.h:27, + from /orin_ssd/MORB_SLAM/include/KeyFrame.h:31, + from /orin_ssd/MORB_SLAM/include/MapPoint.h:22, + from /orin_ssd/MORB_SLAM/include/Map.h:22, + from /orin_ssd/MORB_SLAM/src/Map.cc:22: +/usr/include/c++/9/bits/random.h:4392:5: note: candidate: ‘template bool std::operator!=(const std::negative_binomial_distribution<_IntType>&, const std::negative_binomial_distribution<_IntType>&)’ + 4392 | operator!=(const std::negative_binomial_distribution<_IntType>& __d1, + | ^~~~~~~~ +/usr/include/c++/9/bits/random.h:4392:5: note: template argument deduction/substitution failed: +/orin_ssd/MORB_SLAM/src/Map.cc:339:49: note: ‘std::shared_ptr’ is not derived from ‘const std::negative_binomial_distribution<_IntType>’ + 339 | if ((it->first->GetMap() != sharedMap.get() && this == sharedMap.get()) || it->first->isBad()) { + | ^ +In file included from /usr/include/c++/9/random:49, + from /orin_ssd/MORB_SLAM/Thirdparty/Sophus/sophus/common.hpp:10, + from /orin_ssd/MORB_SLAM/Thirdparty/Sophus/sophus/types.hpp:8, + from /orin_ssd/MORB_SLAM/Thirdparty/Sophus/sophus/rotation_matrix.hpp:10, + from /orin_ssd/MORB_SLAM/Thirdparty/Sophus/sophus/so2.hpp:14, + from /orin_ssd/MORB_SLAM/Thirdparty/Sophus/sophus/se2.hpp:7, + from /orin_ssd/MORB_SLAM/Thirdparty/Sophus/sophus/geometry.hpp:7, + from /orin_ssd/MORB_SLAM/include/Frame.h:27, + from /orin_ssd/MORB_SLAM/include/KeyFrame.h:31, + from /orin_ssd/MORB_SLAM/include/MapPoint.h:22, + from /orin_ssd/MORB_SLAM/include/Map.h:22, + from /orin_ssd/MORB_SLAM/src/Map.cc:22: +/usr/include/c++/9/bits/random.h:4618:5: note: candidate: ‘template bool std::operator!=(const std::poisson_distribution<_IntType>&, const std::poisson_distribution<_IntType>&)’ + 4618 | operator!=(const std::poisson_distribution<_IntType>& __d1, + | ^~~~~~~~ +/usr/include/c++/9/bits/random.h:4618:5: note: template argument deduction/substitution failed: +/orin_ssd/MORB_SLAM/src/Map.cc:339:49: note: ‘std::shared_ptr’ is not derived from ‘const std::poisson_distribution<_IntType>’ + 339 | if ((it->first->GetMap() != sharedMap.get() && this == sharedMap.get()) || it->first->isBad()) { + | ^ +In file included from /usr/include/c++/9/random:49, + from /orin_ssd/MORB_SLAM/Thirdparty/Sophus/sophus/common.hpp:10, + from /orin_ssd/MORB_SLAM/Thirdparty/Sophus/sophus/types.hpp:8, + from /orin_ssd/MORB_SLAM/Thirdparty/Sophus/sophus/rotation_matrix.hpp:10, + from /orin_ssd/MORB_SLAM/Thirdparty/Sophus/sophus/so2.hpp:14, + from /orin_ssd/MORB_SLAM/Thirdparty/Sophus/sophus/se2.hpp:7, + from /orin_ssd/MORB_SLAM/Thirdparty/Sophus/sophus/geometry.hpp:7, + from /orin_ssd/MORB_SLAM/include/Frame.h:27, + from /orin_ssd/MORB_SLAM/include/KeyFrame.h:31, + from /orin_ssd/MORB_SLAM/include/MapPoint.h:22, + from /orin_ssd/MORB_SLAM/include/Map.h:22, + from /orin_ssd/MORB_SLAM/src/Map.cc:22: +/usr/include/c++/9/bits/random.h:4809:5: note: candidate: ‘template bool std::operator!=(const std::exponential_distribution<_RealType>&, const std::exponential_distribution<_RealType>&)’ + 4809 | operator!=(const std::exponential_distribution<_RealType>& __d1, + | ^~~~~~~~ +/usr/include/c++/9/bits/random.h:4809:5: note: template argument deduction/substitution failed: +/orin_ssd/MORB_SLAM/src/Map.cc:339:49: note: ‘std::shared_ptr’ is not derived from ‘const std::exponential_distribution<_RealType>’ + 339 | if ((it->first->GetMap() != sharedMap.get() && this == sharedMap.get()) || it->first->isBad()) { + | ^ +In file included from /usr/include/c++/9/random:49, + from /orin_ssd/MORB_SLAM/Thirdparty/Sophus/sophus/common.hpp:10, + from /orin_ssd/MORB_SLAM/Thirdparty/Sophus/sophus/types.hpp:8, + from /orin_ssd/MORB_SLAM/Thirdparty/Sophus/sophus/rotation_matrix.hpp:10, + from /orin_ssd/MORB_SLAM/Thirdparty/Sophus/sophus/so2.hpp:14, + from /orin_ssd/MORB_SLAM/Thirdparty/Sophus/sophus/se2.hpp:7, + from /orin_ssd/MORB_SLAM/Thirdparty/Sophus/sophus/geometry.hpp:7, + from /orin_ssd/MORB_SLAM/include/Frame.h:27, + from /orin_ssd/MORB_SLAM/include/KeyFrame.h:31, + from /orin_ssd/MORB_SLAM/include/MapPoint.h:22, + from /orin_ssd/MORB_SLAM/include/Map.h:22, + from /orin_ssd/MORB_SLAM/src/Map.cc:22: +/usr/include/c++/9/bits/random.h:5019:5: note: candidate: ‘template bool std::operator!=(const std::weibull_distribution<_RealType>&, const std::weibull_distribution<_RealType>&)’ + 5019 | operator!=(const std::weibull_distribution<_RealType>& __d1, + | ^~~~~~~~ +/usr/include/c++/9/bits/random.h:5019:5: note: template argument deduction/substitution failed: +/orin_ssd/MORB_SLAM/src/Map.cc:339:49: note: ‘std::shared_ptr’ is not derived from ‘const std::weibull_distribution<_RealType>’ + 339 | if ((it->first->GetMap() != sharedMap.get() && this == sharedMap.get()) || it->first->isBad()) { + | ^ +In file included from /usr/include/c++/9/random:49, + from /orin_ssd/MORB_SLAM/Thirdparty/Sophus/sophus/common.hpp:10, + from /orin_ssd/MORB_SLAM/Thirdparty/Sophus/sophus/types.hpp:8, + from /orin_ssd/MORB_SLAM/Thirdparty/Sophus/sophus/rotation_matrix.hpp:10, + from /orin_ssd/MORB_SLAM/Thirdparty/Sophus/sophus/so2.hpp:14, + from /orin_ssd/MORB_SLAM/Thirdparty/Sophus/sophus/se2.hpp:7, + from /orin_ssd/MORB_SLAM/Thirdparty/Sophus/sophus/geometry.hpp:7, + from /orin_ssd/MORB_SLAM/include/Frame.h:27, + from /orin_ssd/MORB_SLAM/include/KeyFrame.h:31, + from /orin_ssd/MORB_SLAM/include/MapPoint.h:22, + from /orin_ssd/MORB_SLAM/include/Map.h:22, + from /orin_ssd/MORB_SLAM/src/Map.cc:22: +/usr/include/c++/9/bits/random.h:5229:5: note: candidate: ‘template bool std::operator!=(const std::extreme_value_distribution<_RealType>&, const std::extreme_value_distribution<_RealType>&)’ + 5229 | operator!=(const std::extreme_value_distribution<_RealType>& __d1, + | ^~~~~~~~ +/usr/include/c++/9/bits/random.h:5229:5: note: template argument deduction/substitution failed: +/orin_ssd/MORB_SLAM/src/Map.cc:339:49: note: ‘std::shared_ptr’ is not derived from ‘const std::extreme_value_distribution<_RealType>’ + 339 | if ((it->first->GetMap() != sharedMap.get() && this == sharedMap.get()) || it->first->isBad()) { + | ^ +In file included from /usr/include/c++/9/random:49, + from /orin_ssd/MORB_SLAM/Thirdparty/Sophus/sophus/common.hpp:10, + from /orin_ssd/MORB_SLAM/Thirdparty/Sophus/sophus/types.hpp:8, + from /orin_ssd/MORB_SLAM/Thirdparty/Sophus/sophus/rotation_matrix.hpp:10, + from /orin_ssd/MORB_SLAM/Thirdparty/Sophus/sophus/so2.hpp:14, + from /orin_ssd/MORB_SLAM/Thirdparty/Sophus/sophus/se2.hpp:7, + from /orin_ssd/MORB_SLAM/Thirdparty/Sophus/sophus/geometry.hpp:7, + from /orin_ssd/MORB_SLAM/include/Frame.h:27, + from /orin_ssd/MORB_SLAM/include/KeyFrame.h:31, + from /orin_ssd/MORB_SLAM/include/MapPoint.h:22, + from /orin_ssd/MORB_SLAM/include/Map.h:22, + from /orin_ssd/MORB_SLAM/src/Map.cc:22: +/usr/include/c++/9/bits/random.h:5494:5: note: candidate: ‘template bool std::operator!=(const std::discrete_distribution<_IntType>&, const std::discrete_distribution<_IntType>&)’ + 5494 | operator!=(const std::discrete_distribution<_IntType>& __d1, + | ^~~~~~~~ +/usr/include/c++/9/bits/random.h:5494:5: note: template argument deduction/substitution failed: +/orin_ssd/MORB_SLAM/src/Map.cc:339:49: note: ‘std::shared_ptr’ is not derived from ‘const std::discrete_distribution<_IntType>’ + 339 | if ((it->first->GetMap() != sharedMap.get() && this == sharedMap.get()) || it->first->isBad()) { + | ^ +In file included from /usr/include/c++/9/random:49, + from /orin_ssd/MORB_SLAM/Thirdparty/Sophus/sophus/common.hpp:10, + from /orin_ssd/MORB_SLAM/Thirdparty/Sophus/sophus/types.hpp:8, + from /orin_ssd/MORB_SLAM/Thirdparty/Sophus/sophus/rotation_matrix.hpp:10, + from /orin_ssd/MORB_SLAM/Thirdparty/Sophus/sophus/so2.hpp:14, + from /orin_ssd/MORB_SLAM/Thirdparty/Sophus/sophus/se2.hpp:7, + from /orin_ssd/MORB_SLAM/Thirdparty/Sophus/sophus/geometry.hpp:7, + from /orin_ssd/MORB_SLAM/include/Frame.h:27, + from /orin_ssd/MORB_SLAM/include/KeyFrame.h:31, + from /orin_ssd/MORB_SLAM/include/MapPoint.h:22, + from /orin_ssd/MORB_SLAM/include/Map.h:22, + from /orin_ssd/MORB_SLAM/src/Map.cc:22: +/usr/include/c++/9/bits/random.h:5765:5: note: candidate: ‘template bool std::operator!=(const std::piecewise_constant_distribution<_RealType>&, const std::piecewise_constant_distribution<_RealType>&)’ + 5765 | operator!=(const std::piecewise_constant_distribution<_RealType>& __d1, + | ^~~~~~~~ +/usr/include/c++/9/bits/random.h:5765:5: note: template argument deduction/substitution failed: +/orin_ssd/MORB_SLAM/src/Map.cc:339:49: note: ‘std::shared_ptr’ is not derived from ‘const std::piecewise_constant_distribution<_RealType>’ + 339 | if ((it->first->GetMap() != sharedMap.get() && this == sharedMap.get()) || it->first->isBad()) { + | ^ +In file included from /usr/include/c++/9/random:49, + from /orin_ssd/MORB_SLAM/Thirdparty/Sophus/sophus/common.hpp:10, + from /orin_ssd/MORB_SLAM/Thirdparty/Sophus/sophus/types.hpp:8, + from /orin_ssd/MORB_SLAM/Thirdparty/Sophus/sophus/rotation_matrix.hpp:10, + from /orin_ssd/MORB_SLAM/Thirdparty/Sophus/sophus/so2.hpp:14, + from /orin_ssd/MORB_SLAM/Thirdparty/Sophus/sophus/se2.hpp:7, + from /orin_ssd/MORB_SLAM/Thirdparty/Sophus/sophus/geometry.hpp:7, + from /orin_ssd/MORB_SLAM/include/Frame.h:27, + from /orin_ssd/MORB_SLAM/include/KeyFrame.h:31, + from /orin_ssd/MORB_SLAM/include/MapPoint.h:22, + from /orin_ssd/MORB_SLAM/include/Map.h:22, + from /orin_ssd/MORB_SLAM/src/Map.cc:22: +/usr/include/c++/9/bits/random.h:6038:5: note: candidate: ‘template bool std::operator!=(const std::piecewise_linear_distribution<_RealType>&, const std::piecewise_linear_distribution<_RealType>&)’ + 6038 | operator!=(const std::piecewise_linear_distribution<_RealType>& __d1, + | ^~~~~~~~ +/usr/include/c++/9/bits/random.h:6038:5: note: template argument deduction/substitution failed: +/orin_ssd/MORB_SLAM/src/Map.cc:339:49: note: ‘std::shared_ptr’ is not derived from ‘const std::piecewise_linear_distribution<_RealType>’ + 339 | if ((it->first->GetMap() != sharedMap.get() && this == sharedMap.get()) || it->first->isBad()) { + | ^ +In file included from /usr/include/c++/9/set:61, + from /orin_ssd/MORB_SLAM/Thirdparty/g2o/g2o/types/../core/optimizable_graph.h:30, + from /orin_ssd/MORB_SLAM/Thirdparty/g2o/g2o/types/../core/base_vertex.h:30, + from /orin_ssd/MORB_SLAM/Thirdparty/g2o/g2o/types/types_seven_dof_expmap.h:34, + from /orin_ssd/MORB_SLAM/include/Converter.h:27, + from /orin_ssd/MORB_SLAM/include/Frame.h:32, + from /orin_ssd/MORB_SLAM/include/KeyFrame.h:31, + from /orin_ssd/MORB_SLAM/include/MapPoint.h:22, + from /orin_ssd/MORB_SLAM/include/Map.h:22, + from /orin_ssd/MORB_SLAM/src/Map.cc:22: +/usr/include/c++/9/bits/stl_set.h:1003:5: note: candidate: ‘template bool std::operator!=(const std::set<_Key, _Compare, _Allocator>&, const std::set<_Key, _Compare, _Allocator>&)’ + 1003 | operator!=(const set<_Key, _Compare, _Alloc>& __x, + | ^~~~~~~~ +/usr/include/c++/9/bits/stl_set.h:1003:5: note: template argument deduction/substitution failed: +/orin_ssd/MORB_SLAM/src/Map.cc:339:49: note: ‘std::shared_ptr’ is not derived from ‘const std::set<_Key, _Compare, _Allocator>’ + 339 | if ((it->first->GetMap() != sharedMap.get() && this == sharedMap.get()) || it->first->isBad()) { + | ^ +In file included from /usr/include/c++/9/set:62, + from /orin_ssd/MORB_SLAM/Thirdparty/g2o/g2o/types/../core/optimizable_graph.h:30, + from /orin_ssd/MORB_SLAM/Thirdparty/g2o/g2o/types/../core/base_vertex.h:30, + from /orin_ssd/MORB_SLAM/Thirdparty/g2o/g2o/types/types_seven_dof_expmap.h:34, + from /orin_ssd/MORB_SLAM/include/Converter.h:27, + from /orin_ssd/MORB_SLAM/include/Frame.h:32, + from /orin_ssd/MORB_SLAM/include/KeyFrame.h:31, + from /orin_ssd/MORB_SLAM/include/MapPoint.h:22, + from /orin_ssd/MORB_SLAM/include/Map.h:22, + from /orin_ssd/MORB_SLAM/src/Map.cc:22: +/usr/include/c++/9/bits/stl_multiset.h:988:5: note: candidate: ‘template bool std::operator!=(const std::multiset<_Key, _Compare, _Allocator>&, const std::multiset<_Key, _Compare, _Allocator>&)’ + 988 | operator!=(const multiset<_Key, _Compare, _Alloc>& __x, + | ^~~~~~~~ +/usr/include/c++/9/bits/stl_multiset.h:988:5: note: template argument deduction/substitution failed: +/orin_ssd/MORB_SLAM/src/Map.cc:339:49: note: ‘std::shared_ptr’ is not derived from ‘const std::multiset<_Key, _Compare, _Allocator>’ + 339 | if ((it->first->GetMap() != sharedMap.get() && this == sharedMap.get()) || it->first->isBad()) { + | ^ +In file included from /usr/include/c++/9/list:63, + from /orin_ssd/MORB_SLAM/Thirdparty/g2o/g2o/types/../core/optimizable_graph.h:32, + from /orin_ssd/MORB_SLAM/Thirdparty/g2o/g2o/types/../core/base_vertex.h:30, + from /orin_ssd/MORB_SLAM/Thirdparty/g2o/g2o/types/types_seven_dof_expmap.h:34, + from /orin_ssd/MORB_SLAM/include/Converter.h:27, + from /orin_ssd/MORB_SLAM/include/Frame.h:32, + from /orin_ssd/MORB_SLAM/include/KeyFrame.h:31, + from /orin_ssd/MORB_SLAM/include/MapPoint.h:22, + from /orin_ssd/MORB_SLAM/include/Map.h:22, + from /orin_ssd/MORB_SLAM/src/Map.cc:22: +/usr/include/c++/9/bits/stl_list.h:2032:5: note: candidate: ‘template bool std::operator!=(const std::__cxx11::list<_Tp, _Alloc>&, const std::__cxx11::list<_Tp, _Alloc>&)’ + 2032 | operator!=(const list<_Tp, _Alloc>& __x, const list<_Tp, _Alloc>& __y) + | ^~~~~~~~ +/usr/include/c++/9/bits/stl_list.h:2032:5: note: template argument deduction/substitution failed: +/orin_ssd/MORB_SLAM/src/Map.cc:339:49: note: ‘std::shared_ptr’ is not derived from ‘const std::__cxx11::list<_Tp, _Alloc>’ + 339 | if ((it->first->GetMap() != sharedMap.get() && this == sharedMap.get()) || it->first->isBad()) { + | ^ +In file included from /usr/include/c++/9/deque:67, + from /usr/include/c++/9/stack:60, + from /orin_ssd/MORB_SLAM/Thirdparty/g2o/g2o/types/../core/base_vertex.h:38, + from /orin_ssd/MORB_SLAM/Thirdparty/g2o/g2o/types/types_seven_dof_expmap.h:34, + from /orin_ssd/MORB_SLAM/include/Converter.h:27, + from /orin_ssd/MORB_SLAM/include/Frame.h:32, + from /orin_ssd/MORB_SLAM/include/KeyFrame.h:31, + from /orin_ssd/MORB_SLAM/include/MapPoint.h:22, + from /orin_ssd/MORB_SLAM/include/Map.h:22, + from /orin_ssd/MORB_SLAM/src/Map.cc:22: +/usr/include/c++/9/bits/stl_deque.h:299:5: note: candidate: ‘template bool std::operator!=(const std::_Deque_iterator<_Tp, _Ref, _Ptr>&, const std::_Deque_iterator<_Tp, _Ref, _Ptr>&)’ + 299 | operator!=(const _Deque_iterator<_Tp, _Ref, _Ptr>& __x, + | ^~~~~~~~ +/usr/include/c++/9/bits/stl_deque.h:299:5: note: template argument deduction/substitution failed: +/orin_ssd/MORB_SLAM/src/Map.cc:339:49: note: ‘std::shared_ptr’ is not derived from ‘const std::_Deque_iterator<_Tp, _Ref, _Ptr>’ + 339 | if ((it->first->GetMap() != sharedMap.get() && this == sharedMap.get()) || it->first->isBad()) { + | ^ +In file included from /usr/include/c++/9/deque:67, + from /usr/include/c++/9/stack:60, + from /orin_ssd/MORB_SLAM/Thirdparty/g2o/g2o/types/../core/base_vertex.h:38, + from /orin_ssd/MORB_SLAM/Thirdparty/g2o/g2o/types/types_seven_dof_expmap.h:34, + from /orin_ssd/MORB_SLAM/include/Converter.h:27, + from /orin_ssd/MORB_SLAM/include/Frame.h:32, + from /orin_ssd/MORB_SLAM/include/KeyFrame.h:31, + from /orin_ssd/MORB_SLAM/include/MapPoint.h:22, + from /orin_ssd/MORB_SLAM/include/Map.h:22, + from /orin_ssd/MORB_SLAM/src/Map.cc:22: +/usr/include/c++/9/bits/stl_deque.h:306:5: note: candidate: ‘template bool std::operator!=(const std::_Deque_iterator<_Tp, _Ref, _Ptr>&, const std::_Deque_iterator<_Tp, _RefR, _PtrR>&)’ + 306 | operator!=(const _Deque_iterator<_Tp, _RefL, _PtrL>& __x, + | ^~~~~~~~ +/usr/include/c++/9/bits/stl_deque.h:306:5: note: template argument deduction/substitution failed: +/orin_ssd/MORB_SLAM/src/Map.cc:339:49: note: ‘std::shared_ptr’ is not derived from ‘const std::_Deque_iterator<_Tp, _Ref, _Ptr>’ + 339 | if ((it->first->GetMap() != sharedMap.get() && this == sharedMap.get()) || it->first->isBad()) { + | ^ +In file included from /usr/include/c++/9/deque:67, + from /usr/include/c++/9/stack:60, + from /orin_ssd/MORB_SLAM/Thirdparty/g2o/g2o/types/../core/base_vertex.h:38, + from /orin_ssd/MORB_SLAM/Thirdparty/g2o/g2o/types/types_seven_dof_expmap.h:34, + from /orin_ssd/MORB_SLAM/include/Converter.h:27, + from /orin_ssd/MORB_SLAM/include/Frame.h:32, + from /orin_ssd/MORB_SLAM/include/KeyFrame.h:31, + from /orin_ssd/MORB_SLAM/include/MapPoint.h:22, + from /orin_ssd/MORB_SLAM/include/Map.h:22, + from /orin_ssd/MORB_SLAM/src/Map.cc:22: +/usr/include/c++/9/bits/stl_deque.h:2338:5: note: candidate: ‘template bool std::operator!=(const std::deque<_Tp, _Alloc>&, const std::deque<_Tp, _Alloc>&)’ + 2338 | operator!=(const deque<_Tp, _Alloc>& __x, + | ^~~~~~~~ +/usr/include/c++/9/bits/stl_deque.h:2338:5: note: template argument deduction/substitution failed: +/orin_ssd/MORB_SLAM/src/Map.cc:339:49: note: ‘std::shared_ptr’ is not derived from ‘const std::deque<_Tp, _Alloc>’ + 339 | if ((it->first->GetMap() != sharedMap.get() && this == sharedMap.get()) || it->first->isBad()) { + | ^ +In file included from /usr/include/c++/9/stack:61, + from /orin_ssd/MORB_SLAM/Thirdparty/g2o/g2o/types/../core/base_vertex.h:38, + from /orin_ssd/MORB_SLAM/Thirdparty/g2o/g2o/types/types_seven_dof_expmap.h:34, + from /orin_ssd/MORB_SLAM/include/Converter.h:27, + from /orin_ssd/MORB_SLAM/include/Frame.h:32, + from /orin_ssd/MORB_SLAM/include/KeyFrame.h:31, + from /orin_ssd/MORB_SLAM/include/MapPoint.h:22, + from /orin_ssd/MORB_SLAM/include/Map.h:22, + from /orin_ssd/MORB_SLAM/src/Map.cc:22: +/usr/include/c++/9/bits/stl_stack.h:337:5: note: candidate: ‘template bool std::operator!=(const std::stack<_Tp, _Seq>&, const std::stack<_Tp, _Seq>&)’ + 337 | operator!=(const stack<_Tp, _Seq>& __x, const stack<_Tp, _Seq>& __y) + | ^~~~~~~~ +/usr/include/c++/9/bits/stl_stack.h:337:5: note: template argument deduction/substitution failed: +/orin_ssd/MORB_SLAM/src/Map.cc:339:49: note: ‘std::shared_ptr’ is not derived from ‘const std::stack<_Tp, _Seq>’ + 339 | if ((it->first->GetMap() != sharedMap.get() && this == sharedMap.get()) || it->first->isBad()) { + | ^ +In file included from /usr/include/c++/9/queue:64, + from /usr/local/include/opencv4/opencv2/stitching/detail/util_inl.hpp:46, + from /usr/local/include/opencv4/opencv2/stitching/detail/util.hpp:119, + from /usr/local/include/opencv4/opencv2/stitching/detail/motion_estimators.hpp:48, + from /usr/local/include/opencv4/opencv2/stitching.hpp:50, + from /usr/local/include/opencv4/opencv2/opencv.hpp:86, + from /orin_ssd/MORB_SLAM/include/Frame.h:36, + from /orin_ssd/MORB_SLAM/include/KeyFrame.h:31, + from /orin_ssd/MORB_SLAM/include/MapPoint.h:22, + from /orin_ssd/MORB_SLAM/include/Map.h:22, + from /orin_ssd/MORB_SLAM/src/Map.cc:22: +/usr/include/c++/9/bits/stl_queue.h:362:5: note: candidate: ‘template bool std::operator!=(const std::queue<_Tp, _Seq>&, const std::queue<_Tp, _Seq>&)’ + 362 | operator!=(const queue<_Tp, _Seq>& __x, const queue<_Tp, _Seq>& __y) + | ^~~~~~~~ +/usr/include/c++/9/bits/stl_queue.h:362:5: note: template argument deduction/substitution failed: +/orin_ssd/MORB_SLAM/src/Map.cc:339:49: note: ‘std::shared_ptr’ is not derived from ‘const std::queue<_Tp, _Seq>’ + 339 | if ((it->first->GetMap() != sharedMap.get() && this == sharedMap.get()) || it->first->isBad()) { + | ^ +In file included from /orin_ssd/MORB_SLAM/include/Camera.hpp:2, + from /orin_ssd/MORB_SLAM/include/Frame.h:41, + from /orin_ssd/MORB_SLAM/include/KeyFrame.h:31, + from /orin_ssd/MORB_SLAM/include/MapPoint.h:22, + from /orin_ssd/MORB_SLAM/include/Map.h:22, + from /orin_ssd/MORB_SLAM/src/Map.cc:22: +/usr/include/c++/9/thread:286:3: note: candidate: ‘bool std::operator!=(std::thread::id, std::thread::id)’ + 286 | operator!=(thread::id __x, thread::id __y) noexcept + | ^~~~~~~~ +/usr/include/c++/9/thread:286:25: note: no known conversion for argument 1 from ‘std::shared_ptr’ to ‘std::thread::id’ + 286 | operator!=(thread::id __x, thread::id __y) noexcept + | ~~~~~~~~~~~^~~ +make[2]: *** [CMakeFiles/MORB_SLAM.dir/build.make:244: CMakeFiles/MORB_SLAM.dir/src/Map.cc.o] Error 1 +make[2]: *** Waiting for unfinished jobs.... +make[2]: *** [CMakeFiles/MORB_SLAM.dir/build.make:76: CMakeFiles/MORB_SLAM.dir/src/System.cc.o] Interrupt +make[2]: *** [CMakeFiles/MORB_SLAM.dir/build.make:104: CMakeFiles/MORB_SLAM.dir/src/Tracking.cc.o] Interrupt +make[2]: *** [CMakeFiles/MORB_SLAM.dir/build.make:272: CMakeFiles/MORB_SLAM.dir/src/Optimizer.cc.o] Interrupt +make[2]: *** [CMakeFiles/MORB_SLAM.dir/build.make:118: CMakeFiles/MORB_SLAM.dir/src/LocalMapping.cc.o] Interrupt +make[2]: *** wait: No child processes. Stop. +make[1]: *** [CMakeFiles/Makefile2:166: CMakeFiles/MORB_SLAM.dir/all] Error 2 +make: *** [Makefile:136: all] Interrupt diff --git a/src/Atlas.cc b/src/Atlas.cc index cad1631a..acbf0df4 100644 --- a/src/Atlas.cc +++ b/src/Atlas.cc @@ -19,36 +19,22 @@ * ORB-SLAM3. If not, see . */ -#include "Atlas.h" +#include "MORB_SLAM/Atlas.h" -#include "GeometricCamera.h" -#include "KannalaBrandt8.h" -#include "Pinhole.h" +#include "MORB_SLAM/CameraModels/GeometricCamera.h" +#include "MORB_SLAM/CameraModels/KannalaBrandt8.h" +#include "MORB_SLAM/CameraModels/Pinhole.h" -namespace ORB_SLAM3 { +namespace MORB_SLAM { -Atlas::Atlas() { mpCurrentMap = static_cast(NULL); } +Atlas::Atlas() { mpCurrentMap = nullptr; } Atlas::Atlas(int initKFid) : mnLastInitKFidMap(initKFid) { - mpCurrentMap = static_cast(NULL); + mpCurrentMap = nullptr; CreateNewMap(); } -Atlas::~Atlas() { - std::cout << "deleting atlas" << std::endl; - for (std::set::iterator it = mspMaps.begin(), end = mspMaps.end(); - it != end;) { - Map* pMi = *it; - - if (pMi) { - delete pMi; - pMi = static_cast(NULL); - - it = mspMaps.erase(it); - } else - ++it; - } -} +Atlas::~Atlas() {} void Atlas::CreateNewMap() { unique_lock lock(mMutexAtlas); @@ -63,12 +49,12 @@ void Atlas::CreateNewMap() { } cout << "Creation of new map with last KF id: " << mnLastInitKFidMap << endl; - mpCurrentMap = new Map(mnLastInitKFidMap); + mpCurrentMap = std::make_shared(mnLastInitKFidMap); mpCurrentMap->SetCurrentMap(); mspMaps.insert(mpCurrentMap); } -void Atlas::ChangeMap(Map* pMap) { +void Atlas::ChangeMap(std::shared_ptr pMap) { unique_lock lock(mMutexAtlas); cout << "Change to map with id: " << pMap->GetId() << endl; if (mpCurrentMap) { @@ -85,32 +71,32 @@ unsigned long int Atlas::GetLastInitKFid() { } void Atlas::AddKeyFrame(KeyFrame* pKF) { - Map* pMapKF = pKF->GetMap(); + std::shared_ptr pMapKF = pKF->GetMap(); pMapKF->AddKeyFrame(pKF); } void Atlas::AddMapPoint(MapPoint* pMP) { - Map* pMapMP = pMP->GetMap(); + std::shared_ptr pMapMP = pMP->GetMap(); pMapMP->AddMapPoint(pMP); } -GeometricCamera* Atlas::AddCamera(GeometricCamera* pCam) { +std::shared_ptr Atlas::AddCamera(const std::shared_ptr &pCam) { // Check if the camera already exists bool bAlreadyInMap = false; int index_cam = -1; for (size_t i = 0; i < mvpCameras.size(); ++i) { - GeometricCamera* pCam_i = mvpCameras[i]; + std::shared_ptr pCam_i = mvpCameras[i]; if (!pCam) std::cout << "Not pCam" << std::endl; if (!pCam_i) std::cout << "Not pCam_i" << std::endl; if (pCam->GetType() != pCam_i->GetType()) continue; if (pCam->GetType() == GeometricCamera::CAM_PINHOLE) { - if (((Pinhole*)pCam_i)->IsEqual(pCam)) { + if (reinterpret_pointer_cast(pCam_i)->IsEqual(pCam)) { bAlreadyInMap = true; index_cam = i; } } else if (pCam->GetType() == GeometricCamera::CAM_FISHEYE) { - if (((KannalaBrandt8*)pCam_i)->IsEqual(pCam)) { + if (reinterpret_pointer_cast(pCam_i)->IsEqual(pCam)) { bAlreadyInMap = true; index_cam = i; } @@ -125,7 +111,7 @@ GeometricCamera* Atlas::AddCamera(GeometricCamera* pCam) { } } -std::vector Atlas::GetAllCameras() { return mvpCameras; } +std::vector> Atlas::GetAllCameras() { return mvpCameras; } void Atlas::SetReferenceMapPoints(const std::vector& vpMPs) { unique_lock lock(mMutexAtlas); @@ -167,14 +153,14 @@ std::vector Atlas::GetReferenceMapPoints() { return mpCurrentMap->GetReferenceMapPoints(); } -vector Atlas::GetAllMaps() { +vector> Atlas::GetAllMaps() { unique_lock lock(mMutexAtlas); struct compFunctor { - inline bool operator()(Map* elem1, Map* elem2) { + inline bool operator()(std::shared_ptr elem1, std::shared_ptr elem2) { return elem1->GetId() < elem2->GetId(); } }; - vector vMaps(mspMaps.begin(), mspMaps.end()); + vector> vMaps(mspMaps.begin(), mspMaps.end()); sort(vMaps.begin(), vMaps.end(), compFunctor()); return vMaps; } @@ -198,11 +184,11 @@ void Atlas::clearAtlas() { delete *it; }*/ mspMaps.clear(); - mpCurrentMap = static_cast(NULL); + mpCurrentMap = nullptr; mnLastInitKFidMap = 0; } -Map* Atlas::GetCurrentMap(System* sys) { +std::shared_ptr Atlas::GetCurrentMap(System* sys) { unique_lock lock(mMutexAtlas); if (!mpCurrentMap) CreateNewMap(); while (mpCurrentMap->IsBad()) { @@ -212,7 +198,7 @@ Map* Atlas::GetCurrentMap(System* sys) { return mpCurrentMap; } -void Atlas::SetMapBad(Map* pMap) { +void Atlas::SetMapBad(std::shared_ptr pMap) { // mspMaps.erase(pMap); pMap->SetBad(); @@ -223,7 +209,7 @@ void Atlas::RemoveBadMaps() { /*for(Map* pMap : mspBadMaps) { delete pMap; - pMap = static_cast(NULL); + pMap = nullptr; }*/ mspBadMaps.clear(); } @@ -256,15 +242,15 @@ void Atlas::PreSave() { } struct compFunctor { - inline bool operator()(Map* elem1, Map* elem2) { + inline bool operator()(std::shared_ptr elem1, std::shared_ptr 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()); + std::set> spCams(mvpCameras.begin(), mvpCameras.end()); - for (Map* pMi : mvpBackupMaps) { + for (std::shared_ptr pMi : mvpBackupMaps) { if (!pMi || pMi->IsBad()) continue; if (pMi->GetAllKeyFrames().size() == 0) { @@ -272,22 +258,22 @@ void Atlas::PreSave() { SetMapBad(pMi); continue; } - pMi->PreSave(spCams); + pMi->PreSave(spCams, pMi); } RemoveBadMaps(); } void Atlas::PostLoad() { - map mpCams; - for (GeometricCamera* pCam : mvpCameras) { + map> mpCams; + for (std::shared_ptr pCam : mvpCameras) { mpCams[pCam->GetId()] = pCam; } mspMaps.clear(); unsigned long int numKF = 0, numMP = 0; - for (Map* pMi : mvpBackupMaps) { + for (std::shared_ptr pMi : mvpBackupMaps) { mspMaps.insert(pMi); - pMi->PostLoad(mpKeyFrameDB, mpORBVocabulary, mpCams); + pMi->PostLoad(mpKeyFrameDB, mpORBVocabulary, mpCams, pMi); numKF += pMi->GetAllKeyFrames().size(); numMP += pMi->GetAllMapPoints().size(); } @@ -309,7 +295,7 @@ ORBVocabulary* Atlas::GetORBVocabulary() { return mpORBVocabulary; } long unsigned int Atlas::GetNumLivedKF() { unique_lock lock(mMutexAtlas); long unsigned int num = 0; - for (Map* pMap_i : mspMaps) { + for (std::shared_ptr pMap_i : mspMaps) { num += pMap_i->GetAllKeyFrames().size(); } @@ -319,7 +305,7 @@ long unsigned int Atlas::GetNumLivedKF() { long unsigned int Atlas::GetNumLivedMP() { unique_lock lock(mMutexAtlas); long unsigned int num = 0; - for (Map* pMap_i : mspMaps) { + for (std::shared_ptr pMap_i : mspMaps) { num += pMap_i->GetAllMapPoints().size(); } @@ -328,7 +314,7 @@ long unsigned int Atlas::GetNumLivedMP() { map Atlas::GetAtlasKeyframes() { map mpIdKFs; - for (Map* pMap_i : mvpBackupMaps) { + for (std::shared_ptr pMap_i : mvpBackupMaps) { vector vpKFs_Mi = pMap_i->GetAllKeyFrames(); for (KeyFrame* pKF_j_Mi : vpKFs_Mi) { @@ -339,4 +325,4 @@ map Atlas::GetAtlasKeyframes() { return mpIdKFs; } -} // namespace ORB_SLAM3 +} // namespace MORB_SLAM diff --git a/src/Camera.cpp b/src/Camera.cpp new file mode 100644 index 00000000..42c2de0b --- /dev/null +++ b/src/Camera.cpp @@ -0,0 +1,69 @@ +#include "MORB_SLAM/Camera.hpp" + +namespace MORB_SLAM{ + +Camera::Camera(CameraType::eSensor type, const std::string &name): ljobs{}, rjobs{}, name{name}, type{type}, + shouldStop{false}, lthread{&Camera::threadExec, this, &ljobs}, rthread{&Camera::threadExec, this, &rjobs} { + +} + +Camera::~Camera(){ + { + std::scoped_lock lock(camMutex); + shouldStop = true; + camCV.notify_all(); + } + + if(lthread.joinable()) lthread.join(); + if(rthread.joinable()) rthread.join(); +} + +void Camera::threadExec(std::deque, std::function>> *jobs){ + while(true){ + ManagedPromise promise; + std::function func; + { + std::unique_lock lock(camMutex); + if(shouldStop) break; + if(jobs->empty()){ + camCV.wait(lock); + if(shouldStop) break; + if(jobs->empty()) continue; + } + promise = jobs->front().first; + func = jobs->front().second; + jobs->pop_front(); + } + + try{ + func(); + }catch(const std::exception &e){ + promise.set_exception(std::current_exception()); + } + promise.set_value(true); + } +} + +ManagedFuture Camera::queue(std::function func, bool isLeft){ + ManagedPromise promise; + std::deque, std::function>> &jobs = isLeft ? ljobs : rjobs; + { + std::scoped_lock lock(camMutex); + + if(!jobs.empty()){ // only the newest! // TODO add a policy variable to enable or disable this! + jobs.front().first.set_value(false); + jobs.clear(); + } + if(!shouldStop) jobs.emplace_back(promise,func); + camCV.notify_all(); + } + + return promise; +} +ManagedFuture Camera::queueLeft(const std::function &func){ return queue(func, true); } +ManagedFuture Camera::queueRight(const std::function &func){ return queue(func, false); } + +CameraType::eSensor Camera::getType() const { return type; } +const std::string &Camera::getName() const { return name; } + +} // namespace MORB_SLAM \ No newline at end of file diff --git a/src/CameraModels/KannalaBrandt8.cpp b/src/CameraModels/KannalaBrandt8.cpp index 739f4774..71de141c 100644 --- a/src/CameraModels/KannalaBrandt8.cpp +++ b/src/CameraModels/KannalaBrandt8.cpp @@ -19,16 +19,16 @@ * ORB-SLAM3. If not, see . */ -#include "KannalaBrandt8.h" +#include "MORB_SLAM/CameraModels/KannalaBrandt8.h" #include -// BOOST_CLASS_EXPORT_IMPLEMENT(ORB_SLAM3::KannalaBrandt8) +// BOOST_CLASS_EXPORT_IMPLEMENT(MORB_SLAM::KannalaBrandt8) -namespace ORB_SLAM3 { +namespace MORB_SLAM { // BOOST_CLASS_EXPORT_GUID(KannalaBrandt8, "KannalaBrandt8") -cv::Point2f KannalaBrandt8::project(const cv::Point3f &p3D) { +cv::Point2f KannalaBrandt8::project(const cv::Point3f &p3D) const { const float x2_plus_y2 = p3D.x * p3D.x + p3D.y * p3D.y; const float theta = atan2f(sqrtf(x2_plus_y2), p3D.z); const float psi = atan2f(p3D.y, p3D.x); @@ -45,7 +45,7 @@ cv::Point2f KannalaBrandt8::project(const cv::Point3f &p3D) { mvParameters[1] * r * sin(psi) + mvParameters[3]); } -Eigen::Vector2d KannalaBrandt8::project(const Eigen::Vector3d &v3D) { +Eigen::Vector2d KannalaBrandt8::project(const Eigen::Vector3d &v3D) const { const double x2_plus_y2 = v3D[0] * v3D[0] + v3D[1] * v3D[1]; const double theta = atan2f(sqrtf(x2_plus_y2), v3D[2]); const double psi = atan2f(v3D[1], v3D[0]); @@ -65,7 +65,7 @@ Eigen::Vector2d KannalaBrandt8::project(const Eigen::Vector3d &v3D) { return res; } -Eigen::Vector2f KannalaBrandt8::project(const Eigen::Vector3f &v3D) { +Eigen::Vector2f KannalaBrandt8::project(const Eigen::Vector3f &v3D) const { const float x2_plus_y2 = v3D[0] * v3D[0] + v3D[1] * v3D[1]; const float theta = atan2f(sqrtf(x2_plus_y2), v3D[2]); const float psi = atan2f(v3D[1], v3D[0]); @@ -93,7 +93,7 @@ Eigen::Vector2f KannalaBrandt8::project(const Eigen::Vector3f &v3D) { return res;*/ } -Eigen::Vector2f KannalaBrandt8::projectMat(const cv::Point3f &p3D) { +Eigen::Vector2f KannalaBrandt8::projectMat(const cv::Point3f &p3D) const { cv::Point2f point = this->project(p3D); return Eigen::Vector2f(point.x, point.y); } @@ -108,12 +108,12 @@ float KannalaBrandt8::uncertainty2(const Eigen::Matrix &p2D) { return 1.f; } -Eigen::Vector3f KannalaBrandt8::unprojectEig(const cv::Point2f &p2D) { +Eigen::Vector3f KannalaBrandt8::unprojectEig(const cv::Point2f &p2D) const { cv::Point3f ray = this->unproject(p2D); return Eigen::Vector3f(ray.x, ray.y, ray.z); } -cv::Point3f KannalaBrandt8::unproject(const cv::Point2f &p2D) { +cv::Point3f KannalaBrandt8::unproject(const cv::Point2f &p2D) const { // 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]); @@ -214,12 +214,12 @@ bool KannalaBrandt8::ReconstructWithTwoViews( vbTriangulated); } -cv::Mat KannalaBrandt8::toK() { +cv::Mat KannalaBrandt8::toK() const { cv::Mat K = (cv::Mat_(3, 3) << mvParameters[0], 0.f, mvParameters[2], 0.f, mvParameters[1], mvParameters[3], 0.f, 0.f, 1.f); return K; } -Eigen::Matrix3f KannalaBrandt8::toK_() { +Eigen::Matrix3f KannalaBrandt8::toK_() const { Eigen::Matrix3f K; K << mvParameters[0], 0.f, mvParameters[2], 0.f, mvParameters[1], mvParameters[3], 0.f, 0.f, 1.f; @@ -227,7 +227,7 @@ Eigen::Matrix3f KannalaBrandt8::toK_() { } bool KannalaBrandt8::epipolarConstrain( - GeometricCamera *pCamera2, const cv::KeyPoint &kp1, const cv::KeyPoint &kp2, + const std::shared_ptr &pCamera2, const cv::KeyPoint &kp1, const cv::KeyPoint &kp2, const Eigen::Matrix3f &R12, const Eigen::Vector3f &t12, const float sigmaLevel, const float unc) { Eigen::Vector3f p3D; @@ -321,7 +321,7 @@ bool KannalaBrandt8::matchAndtriangulate( } float KannalaBrandt8::TriangulateMatches( - GeometricCamera *pCamera2, const cv::KeyPoint &kp1, const cv::KeyPoint &kp2, + const std::shared_ptr &pCamera2, const cv::KeyPoint &kp1, const cv::KeyPoint &kp2, const Eigen::Matrix3f &R12, const Eigen::Vector3f &t12, const float sigmaLevel, const float unc, Eigen::Vector3f &p3D) { Eigen::Vector3f r1 = this->unprojectEig(kp1.pt); @@ -427,10 +427,10 @@ void KannalaBrandt8::Triangulate(const cv::Point2f &p1, const cv::Point2f &p2, x3D = x3Dh.head(3) / x3Dh(3); } -bool KannalaBrandt8::IsEqual(GeometricCamera *pCam) { +bool KannalaBrandt8::IsEqual(const std::shared_ptr &pCam) { if (pCam->GetType() != GeometricCamera::CAM_FISHEYE) return false; - KannalaBrandt8 *pKBCam = (KannalaBrandt8 *)pCam; + std::shared_ptr pKBCam = std::static_pointer_cast(pCam); if (abs(precision - pKBCam->GetPrecision()) > 1e-6) return false; @@ -446,4 +446,4 @@ bool KannalaBrandt8::IsEqual(GeometricCamera *pCam) { return is_same_camera; } -} // namespace ORB_SLAM3 +} // namespace MORB_SLAM diff --git a/src/CameraModels/Pinhole.cpp b/src/CameraModels/Pinhole.cpp index 04a75d68..6df3f00f 100644 --- a/src/CameraModels/Pinhole.cpp +++ b/src/CameraModels/Pinhole.cpp @@ -19,23 +19,23 @@ * ORB-SLAM3. If not, see . */ -#include "Pinhole.h" +#include "MORB_SLAM/CameraModels/Pinhole.h" #include -// BOOST_CLASS_EXPORT_IMPLEMENT(ORB_SLAM3::Pinhole) +// BOOST_CLASS_EXPORT_IMPLEMENT(MORB_SLAM::Pinhole) -namespace ORB_SLAM3 { +namespace MORB_SLAM { // BOOST_CLASS_EXPORT_GUID(Pinhole, "Pinhole") long unsigned int GeometricCamera::nNextId = 0; -cv::Point2f Pinhole::project(const cv::Point3f &p3D) { +cv::Point2f Pinhole::project(const cv::Point3f &p3D) const { return cv::Point2f(mvParameters[0] * p3D.x / p3D.z + mvParameters[2], mvParameters[1] * p3D.y / p3D.z + mvParameters[3]); } -Eigen::Vector2d Pinhole::project(const Eigen::Vector3d &v3D) { +Eigen::Vector2d Pinhole::project(const Eigen::Vector3d &v3D) const { Eigen::Vector2d res; res[0] = mvParameters[0] * v3D[0] / v3D[2] + mvParameters[2]; res[1] = mvParameters[1] * v3D[1] / v3D[2] + mvParameters[3]; @@ -43,7 +43,7 @@ Eigen::Vector2d Pinhole::project(const Eigen::Vector3d &v3D) { return res; } -Eigen::Vector2f Pinhole::project(const Eigen::Vector3f &v3D) { +Eigen::Vector2f Pinhole::project(const Eigen::Vector3f &v3D) const { Eigen::Vector2f res; res[0] = mvParameters[0] * v3D[0] / v3D[2] + mvParameters[2]; res[1] = mvParameters[1] * v3D[1] / v3D[2] + mvParameters[3]; @@ -51,7 +51,7 @@ Eigen::Vector2f Pinhole::project(const Eigen::Vector3f &v3D) { return res; } -Eigen::Vector2f Pinhole::projectMat(const cv::Point3f &p3D) { +Eigen::Vector2f Pinhole::projectMat(const cv::Point3f &p3D) const { cv::Point2f point = this->project(p3D); return Eigen::Vector2f(point.x, point.y); } @@ -60,12 +60,12 @@ float Pinhole::uncertainty2(const Eigen::Matrix &p2D) { return 1.0; } -Eigen::Vector3f Pinhole::unprojectEig(const cv::Point2f &p2D) { +Eigen::Vector3f Pinhole::unprojectEig(const cv::Point2f &p2D) const { return Eigen::Vector3f((p2D.x - mvParameters[2]) / mvParameters[0], (p2D.y - mvParameters[3]) / mvParameters[1], 1.f); } -cv::Point3f Pinhole::unproject(const cv::Point2f &p2D) { +cv::Point3f Pinhole::unproject(const cv::Point2f &p2D) const { return cv::Point3f((p2D.x - mvParameters[2]) / mvParameters[0], (p2D.y - mvParameters[3]) / mvParameters[1], 1.f); } @@ -97,20 +97,20 @@ bool Pinhole::ReconstructWithTwoViews(const std::vector &vKeys1, vbTriangulated); } -cv::Mat Pinhole::toK() { +cv::Mat Pinhole::toK() const { cv::Mat K = (cv::Mat_(3, 3) << mvParameters[0], 0.f, mvParameters[2], 0.f, mvParameters[1], mvParameters[3], 0.f, 0.f, 1.f); return K; } -Eigen::Matrix3f Pinhole::toK_() { +Eigen::Matrix3f Pinhole::toK_() const { Eigen::Matrix3f K; 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, +bool Pinhole::epipolarConstrain(const std::shared_ptr &pCamera2, const cv::KeyPoint &kp1, const cv::KeyPoint &kp2, const Eigen::Matrix3f &R12, @@ -154,10 +154,10 @@ std::istream &operator>>(std::istream &is, Pinhole &ph) { return is; } -bool Pinhole::IsEqual(GeometricCamera *pCam) { +bool Pinhole::IsEqual(const std::shared_ptr &pCam) { if (pCam->GetType() != GeometricCamera::CAM_PINHOLE) return false; - Pinhole *pPinholeCam = (Pinhole *)pCam; + std::shared_ptr pPinholeCam = std::static_pointer_cast(pCam); if (size() != pPinholeCam->size()) return false; @@ -170,4 +170,4 @@ bool Pinhole::IsEqual(GeometricCamera *pCam) { } return is_same_camera; } -} // namespace ORB_SLAM3 +} // namespace MORB_SLAM diff --git a/src/Config.cc b/src/Config.cc deleted file mode 100644 index d7457f9a..00000000 --- a/src/Config.cc +++ /dev/null @@ -1,28 +0,0 @@ -/** - * This file is part of ORB-SLAM3 - * - * Copyright (C) 2017-2021 Carlos Campos, Richard Elvira, Juan J. Gómez - * Rodríguez, José M.M. Montiel and Juan D. Tardós, University of Zaragoza. - * Copyright (C) 2014-2016 Raúl Mur-Artal, José M.M. Montiel and Juan D. Tardós, - * University of Zaragoza. - * - * ORB-SLAM3 is free software: you can redistribute it and/or modify it under - * the terms of the GNU General Public License as published by the Free Software - * Foundation, either version 3 of the License, or (at your option) any later - * version. - * - * ORB-SLAM3 is distributed in the hope that it will be useful, but WITHOUT ANY - * WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS FOR - * A PARTICULAR PURPOSE. See the GNU General Public License for more details. - * - * You should have received a copy of the GNU General Public License along with - * ORB-SLAM3. If not, see . - */ - -#include "Config.h" - -namespace ORB_SLAM3 { - -bool ConfigParser::ParseConfigFile(std::string &strConfigFile) { return true; } - -} // namespace ORB_SLAM3 diff --git a/src/Converter.cc b/src/Converter.cc index c842918d..e2c3ab8d 100644 --- a/src/Converter.cc +++ b/src/Converter.cc @@ -19,9 +19,9 @@ * ORB-SLAM3. If not, see . */ -#include "Converter.h" +#include "MORB_SLAM/Converter.h" -namespace ORB_SLAM3 { +namespace MORB_SLAM { std::vector Converter::toDescriptorVector(const cv::Mat &Descriptors) { std::vector vDesc; @@ -282,4 +282,4 @@ Sophus::Sim3f Converter::toSophus(const g2o::Sim3 &S) { S.translation().cast()); } -} // namespace ORB_SLAM3 +} // namespace MORB_SLAM diff --git a/src/Frame.cc b/src/Frame.cc index e69fb70b..158fad65 100644 --- a/src/Frame.cc +++ b/src/Frame.cc @@ -19,22 +19,22 @@ * ORB-SLAM3. If not, see . */ -#include "Frame.h" +#include "MORB_SLAM/Frame.h" -#include -#include +#include +#include #include -#include "Converter.h" -#include "G2oTypes.h" -#include "GeometricCamera.h" -#include "KeyFrame.h" -#include "MapPoint.h" -#include "ORBextractor.h" -#include "ORBmatcher.h" +#include "MORB_SLAM/Converter.h" +#include "MORB_SLAM/G2oTypes.h" +#include "MORB_SLAM/CameraModels/GeometricCamera.h" +#include "MORB_SLAM/KeyFrame.h" +#include "MORB_SLAM/MapPoint.h" +#include "MORB_SLAM/ORBextractor.h" +#include "MORB_SLAM/ORBmatcher.h" -namespace ORB_SLAM3 { +namespace MORB_SLAM { long unsigned int Frame::nNextId = 0; bool Frame::mbInitialComputations = true; @@ -46,13 +46,13 @@ float Frame::mfGridElementWidthInv, Frame::mfGridElementHeightInv; cv::BFMatcher Frame::BFmatcher = cv::BFMatcher(cv::NORM_HAMMING); Frame::Frame() - : mpcpi(NULL), + : mpcpi(nullptr), mbHasPose(false), mbHasVelocity(false), - mpImuPreintegrated(NULL), - mpPrevFrame(NULL), - mpImuPreintegratedFrame(NULL), - mpReferenceKF(static_cast(NULL)), + mpImuPreintegrated(nullptr), + mpPrevFrame(nullptr), + mpImuPreintegratedFrame(nullptr), + mpReferenceKF(nullptr), mbIsSet(false), mbImuPreintegrated(false) { #ifdef REGISTER_TIMES @@ -115,6 +115,7 @@ Frame::Frame(const Frame &frame) mbIsSet(frame.mbIsSet), mbImuPreintegrated(frame.mbImuPreintegrated), mpMutexImu(frame.mpMutexImu), + camera{frame.camera}, mpCamera(frame.mpCamera), mpCamera2(frame.mpCamera2), Nleft(frame.Nleft), @@ -147,13 +148,13 @@ Frame::Frame(const Frame &frame) #endif } -Frame::Frame(const cv::Mat &imLeft, const cv::Mat &imRight, - const double &timeStamp, ORBextractor *extractorLeft, - ORBextractor *extractorRight, ORBVocabulary *voc, cv::Mat &K, +Frame::Frame(const Camera_ptr &cam, const cv::Mat &imLeft, const cv::Mat &imRight, + const double &timeStamp, const std::shared_ptr &extractorLeft, + const std::shared_ptr &extractorRight, ORBVocabulary *voc, cv::Mat &K, cv::Mat &distCoef, const float &bf, const float &thDepth, - GeometricCamera *pCamera, Frame *pPrevF, - const IMU::Calib &ImuCalib) - : mpcpi(NULL), + const std::shared_ptr &pCamera, const std::string &pNameFile, int pnNumDataset, + Frame *pPrevF, const IMU::Calib &ImuCalib) + : mpcpi(nullptr), mbHasPose(false), mbHasVelocity(false), mpORBvocabulary(voc), @@ -166,12 +167,15 @@ Frame::Frame(const cv::Mat &imLeft, const cv::Mat &imRight, mbf(bf), mThDepth(thDepth), mImuCalib(ImuCalib), - mpImuPreintegrated(NULL), + mpImuPreintegrated(nullptr), mpPrevFrame(pPrevF), - mpImuPreintegratedFrame(NULL), - mpReferenceKF(static_cast(NULL)), + mpImuPreintegratedFrame(nullptr), + mpReferenceKF(nullptr), + mNameFile{pNameFile}, + mnDataset{pnNumDataset}, mbIsSet(false), mbImuPreintegrated(false), + camera(cam), mpCamera(pCamera), mpCamera2(nullptr) { // Frame ID @@ -191,10 +195,9 @@ Frame::Frame(const cv::Mat &imLeft, const cv::Mat &imRight, std::chrono::steady_clock::time_point time_StartExtORB = std::chrono::steady_clock::now(); #endif - thread threadLeft(&Frame::ExtractORB, this, 0, imLeft, 0, 0); - thread threadRight(&Frame::ExtractORB, this, 1, imRight, 0, 0); - threadLeft.join(); - threadRight.join(); + auto leftFut = camera->queueLeft(std::bind(&Frame::ExtractORB, this, true, imLeft, 0, 0)); + auto rightFut = camera->queueRight(std::bind(&Frame::ExtractORB, this, false, imRight, 0, 0)); + if(!leftFut.get() || !rightFut.get()) return; #ifdef REGISTER_TIMES std::chrono::steady_clock::time_point time_EndExtORB = std::chrono::steady_clock::now(); @@ -205,6 +208,26 @@ Frame::Frame(const cv::Mat &imLeft, const cv::Mat &imRight, .count(); #endif + // This is done only for the first Frame (or after a change in the + // calibration) + if (mbInitialComputations) { + ComputeImageBounds(imLeft); + + mfGridElementWidthInv = + static_cast(FRAME_GRID_COLS) / (mnMaxX - mnMinX); + mfGridElementHeightInv = + static_cast(FRAME_GRID_ROWS) / (mnMaxY - mnMinY); + + fx = K.at(0, 0); + fy = K.at(1, 1); + cx = K.at(0, 2); + cy = K.at(1, 2); + invfx = 1.0f / fx; + invfy = 1.0f / fy; + + mbInitialComputations = false; + } + N = mvKeys.size(); if (mvKeys.empty()) return; @@ -225,31 +248,11 @@ Frame::Frame(const cv::Mat &imLeft, const cv::Mat &imRight, .count(); #endif - mvpMapPoints = vector(N, static_cast(NULL)); + mvpMapPoints = vector(N, nullptr); mvbOutlier = vector(N, false); mmProjectPoints.clear(); mmMatchedInImage.clear(); - // This is done only for the first Frame (or after a change in the - // calibration) - if (mbInitialComputations) { - ComputeImageBounds(imLeft); - - mfGridElementWidthInv = - static_cast(FRAME_GRID_COLS) / (mnMaxX - mnMinX); - mfGridElementHeightInv = - static_cast(FRAME_GRID_ROWS) / (mnMaxY - mnMinY); - - fx = K.at(0, 0); - fy = K.at(1, 1); - cx = K.at(0, 2); - cy = K.at(1, 2); - invfx = 1.0f / fx; - invfy = 1.0f / fy; - - mbInitialComputations = false; - } - mb = mbf / fx; if (pPrevF) { @@ -258,31 +261,34 @@ Frame::Frame(const cv::Mat &imLeft, const cv::Mat &imRight, mVw.setZero(); } - mpMutexImu = new std::mutex(); + mpMutexImu = std::make_shared(); // Set no stereo fisheye information Nleft = -1; Nright = -1; - mvLeftToRightMatch = vector(0); - mvRightToLeftMatch = vector(0); - mvStereo3Dpoints = vector(0); + // mvLeftToRightMatch = vector(0); + // mvRightToLeftMatch = vector(0); + // mvStereo3Dpoints = vector(0); monoLeft = -1; monoRight = -1; AssignFeaturesToGrid(); } -Frame::Frame(const cv::Mat &imGray, const cv::Mat &imDepth, - const double &timeStamp, ORBextractor *extractor, +Frame::~Frame(){} + +Frame::Frame(const Camera_ptr &cam, const cv::Mat &imGray, const cv::Mat &imDepth, + const double &timeStamp, const std::shared_ptr &extractor, ORBVocabulary *voc, cv::Mat &K, cv::Mat &distCoef, const float &bf, - const float &thDepth, GeometricCamera *pCamera, Frame *pPrevF, + const float &thDepth, const std::shared_ptr &pCamera, + const std::string &pNameFile, int pnNumDataset, Frame *pPrevF, const IMU::Calib &ImuCalib) - : mpcpi(NULL), + : mpcpi(nullptr), mbHasPose(false), mbHasVelocity(false), mpORBvocabulary(voc), mpORBextractorLeft(extractor), - mpORBextractorRight(static_cast(NULL)), + mpORBextractorRight(nullptr), mTimeStamp(timeStamp), mK(K.clone()), mK_(Converter::toMatrix3f(K)), @@ -290,12 +296,15 @@ Frame::Frame(const cv::Mat &imGray, const cv::Mat &imDepth, mbf(bf), mThDepth(thDepth), mImuCalib(ImuCalib), - mpImuPreintegrated(NULL), + mpImuPreintegrated(nullptr), mpPrevFrame(pPrevF), - mpImuPreintegratedFrame(NULL), - mpReferenceKF(static_cast(NULL)), + mpImuPreintegratedFrame(nullptr), + mpReferenceKF(nullptr), + mNameFile{pNameFile}, + mnDataset{pnNumDataset}, mbIsSet(false), mbImuPreintegrated(false), + camera{cam}, mpCamera(pCamera), mpCamera2(nullptr) { // Frame ID @@ -315,7 +324,7 @@ Frame::Frame(const cv::Mat &imGray, const cv::Mat &imDepth, std::chrono::steady_clock::time_point time_StartExtORB = std::chrono::steady_clock::now(); #endif - ExtractORB(0, imGray, 0, 0); + ExtractORB(true, imGray, 0, 0); #ifdef REGISTER_TIMES std::chrono::steady_clock::time_point time_EndExtORB = std::chrono::steady_clock::now(); @@ -326,21 +335,6 @@ Frame::Frame(const cv::Mat &imGray, const cv::Mat &imDepth, .count(); #endif - N = mvKeys.size(); - - if (mvKeys.empty()) return; - - UndistortKeyPoints(); - - ComputeStereoFromRGBD(imDepth); - - mvpMapPoints = vector(N, static_cast(NULL)); - - mmProjectPoints.clear(); - mmMatchedInImage.clear(); - - mvbOutlier = vector(N, false); - // This is done only for the first Frame (or after a change in the // calibration) if (mbInitialComputations) { @@ -361,6 +355,20 @@ Frame::Frame(const cv::Mat &imGray, const cv::Mat &imDepth, mbInitialComputations = false; } + N = mvKeys.size(); + if (mvKeys.empty()) return; + + UndistortKeyPoints(); + + ComputeStereoFromRGBD(imDepth); + + mvpMapPoints = vector(N, nullptr); + + mmProjectPoints.clear(); + mmMatchedInImage.clear(); + + mvbOutlier = vector(N, false); + mb = mbf / fx; if (pPrevF) { @@ -369,7 +377,7 @@ Frame::Frame(const cv::Mat &imGray, const cv::Mat &imDepth, mVw.setZero(); } - mpMutexImu = new std::mutex(); + mpMutexImu = std::make_shared(); // Set no stereo fisheye information Nleft = -1; @@ -383,29 +391,33 @@ Frame::Frame(const cv::Mat &imGray, const cv::Mat &imDepth, AssignFeaturesToGrid(); } -Frame::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, const IMU::Calib &ImuCalib) - : mpcpi(NULL), +Frame::Frame(const Camera_ptr &cam, const cv::Mat &imGray, const double &timeStamp, + const std::shared_ptr &extractor, ORBVocabulary *voc, + const std::shared_ptr &pCamera, cv::Mat &distCoef, const float &bf, + const float &thDepth, const std::string &pNameFile, int pnNumDataset, + Frame *pPrevF, const IMU::Calib &ImuCalib) + : mpcpi(nullptr), mbHasPose(false), mbHasVelocity(false), mpORBvocabulary(voc), mpORBextractorLeft(extractor), - mpORBextractorRight(static_cast(NULL)), + mpORBextractorRight(nullptr), mTimeStamp(timeStamp), - mK(static_cast(pCamera)->toK()), - mK_(static_cast(pCamera)->toK_()), + mK(reinterpret_pointer_cast(pCamera)->toK()), + mK_(reinterpret_pointer_cast(pCamera)->toK_()), mDistCoef(distCoef.clone()), mbf(bf), mThDepth(thDepth), mImuCalib(ImuCalib), - mpImuPreintegrated(NULL), + mpImuPreintegrated(nullptr), mpPrevFrame(pPrevF), - mpImuPreintegratedFrame(NULL), - mpReferenceKF(static_cast(NULL)), + mpImuPreintegratedFrame(nullptr), + mpReferenceKF(nullptr), + mNameFile{pNameFile}, + mnDataset{pnNumDataset}, mbIsSet(false), mbImuPreintegrated(false), + camera{cam}, mpCamera(pCamera), mpCamera2(nullptr) { // Frame ID @@ -425,7 +437,7 @@ Frame::Frame(const cv::Mat &imGray, const double &timeStamp, std::chrono::steady_clock::time_point time_StartExtORB = std::chrono::steady_clock::now(); #endif - ExtractORB(0, imGray, 0, 1000); + ExtractORB(true, imGray, 0, 1000); #ifdef REGISTER_TIMES std::chrono::steady_clock::time_point time_EndExtORB = std::chrono::steady_clock::now(); @@ -436,24 +448,6 @@ Frame::Frame(const cv::Mat &imGray, const double &timeStamp, .count(); #endif - N = mvKeys.size(); - if (mvKeys.empty()) return; - - UndistortKeyPoints(); - - // Set no stereo information - mvuRight = vector(N, -1); - mvDepth = vector(N, -1); - mnCloseMPs = 0; - - mvpMapPoints = vector(N, static_cast(NULL)); - - mmProjectPoints.clear(); // = map(N, - // static_cast(NULL)); - mmMatchedInImage.clear(); - - mvbOutlier = vector(N, false); - // This is done only for the first Frame (or after a change in the // calibration) if (mbInitialComputations) { @@ -464,16 +458,34 @@ Frame::Frame(const cv::Mat &imGray, const double &timeStamp, mfGridElementHeightInv = static_cast(FRAME_GRID_ROWS) / static_cast(mnMaxY - mnMinY); - fx = static_cast(mpCamera)->toK().at(0, 0); - fy = static_cast(mpCamera)->toK().at(1, 1); - cx = static_cast(mpCamera)->toK().at(0, 2); - cy = static_cast(mpCamera)->toK().at(1, 2); + fx = reinterpret_pointer_cast(mpCamera)->toK().at(0, 0); + fy = reinterpret_pointer_cast(mpCamera)->toK().at(1, 1); + cx = reinterpret_pointer_cast(mpCamera)->toK().at(0, 2); + cy = reinterpret_pointer_cast(mpCamera)->toK().at(1, 2); invfx = 1.0f / fx; invfy = 1.0f / fy; mbInitialComputations = false; } + N = mvKeys.size(); + if (mvKeys.empty()) return; + + UndistortKeyPoints(); + + // Set no stereo information + mvuRight = vector(N, -1); + mvDepth = vector(N, -1); + mnCloseMPs = 0; + + mvpMapPoints = vector(N, nullptr); + + mmProjectPoints.clear(); // = map(N, + // static_cast(nullptr)); + mmMatchedInImage.clear(); + + mvbOutlier = vector(N, false); + mb = mbf / fx; // Set no stereo fisheye information @@ -495,7 +507,7 @@ Frame::Frame(const cv::Mat &imGray, const double &timeStamp, mVw.setZero(); } - mpMutexImu = new std::mutex(); + mpMutexImu = std::make_shared(); } void Frame::AssignFeaturesToGrid() { @@ -527,10 +539,10 @@ void Frame::AssignFeaturesToGrid() { } } -void Frame::ExtractORB(int flag, const cv::Mat &im, const int x0, +void Frame::ExtractORB(bool isLeft, const cv::Mat &im, const int x0, const int x1) { vector vLapping = {x0, x1}; - if (flag == 0) + if (isLeft) monoLeft = (*mpORBextractorLeft)(im, cv::Mat(), mvKeys, mDescriptors, vLapping); else @@ -842,7 +854,7 @@ void Frame::UndistortKeyPoints() { // Undistort points mat = mat.reshape(2); - cv::undistortPoints(mat, mat, static_cast(mpCamera)->toK(), + cv::undistortPoints(mat, mat, reinterpret_pointer_cast(mpCamera)->toK(), mDistCoef, cv::Mat(), mK); mat = mat.reshape(1); @@ -869,7 +881,7 @@ void Frame::ComputeImageBounds(const cv::Mat &imLeft) { mat.at(3, 1) = imLeft.rows; mat = mat.reshape(2); - cv::undistortPoints(mat, mat, static_cast(mpCamera)->toK(), + cv::undistortPoints(mat, mat, reinterpret_pointer_cast(mpCamera)->toK(), mDistCoef, cv::Mat(), mK); mat = mat.reshape(1); @@ -895,7 +907,7 @@ void Frame::ComputeStereoMatches() { const int nRows = mpORBextractorLeft->mvImagePyramid[0].rows; // Assign keypoints to row table - vector> vRowIndices(nRows, vector()); + vector> vRowIndices(nRows); for (int i = 0; i < nRows; i++) vRowIndices[i].reserve(200); @@ -1027,7 +1039,7 @@ void Frame::ComputeStereoMatches() { } mvDepth[iL] = mbf / disparity; mvuRight[iL] = bestuR; - vDistIdx.push_back(pair(bestDist, iL)); + vDistIdx.emplace_back(bestDist, iL); } } } @@ -1087,16 +1099,18 @@ bool Frame::imuIsPreintegrated() { void Frame::setIntegrated() { unique_lock lock(*mpMutexImu); + std::cout << "-----------------------------------------------------------" << std::endl; mbImuPreintegrated = true; } -Frame::Frame(const cv::Mat &imLeft, const cv::Mat &imRight, - const double &timeStamp, ORBextractor *extractorLeft, - ORBextractor *extractorRight, ORBVocabulary *voc, cv::Mat &K, +Frame::Frame(const Camera_ptr &cam, const cv::Mat &imLeft, const cv::Mat &imRight, + const double &timeStamp, const std::shared_ptr &extractorLeft, + const std::shared_ptr &extractorRight, ORBVocabulary *voc, cv::Mat &K, cv::Mat &distCoef, const float &bf, const float &thDepth, - GeometricCamera *pCamera, GeometricCamera *pCamera2, + const std::shared_ptr &pCamera, const std::shared_ptr &pCamera2, + const std::string &pNameFile, int pnNumDataset, Sophus::SE3f &Tlr, Frame *pPrevF, const IMU::Calib &ImuCalib) - : mpcpi(NULL), + : mpcpi(nullptr), mbHasPose(false), mbHasVelocity(false), mpORBvocabulary(voc), @@ -1109,11 +1123,14 @@ Frame::Frame(const cv::Mat &imLeft, const cv::Mat &imRight, mbf(bf), mThDepth(thDepth), mImuCalib(ImuCalib), - mpImuPreintegrated(NULL), + mpImuPreintegrated(nullptr), mpPrevFrame(pPrevF), - mpImuPreintegratedFrame(NULL), - mpReferenceKF(static_cast(NULL)), + mpImuPreintegratedFrame(nullptr), + mpReferenceKF(nullptr), + mNameFile{pNameFile}, + mnDataset{pnNumDataset}, mbImuPreintegrated(false), + camera{cam}, mpCamera(pCamera), mpCamera2(pCamera2) @@ -1138,15 +1155,14 @@ Frame::Frame(const cv::Mat &imLeft, const cv::Mat &imRight, 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(); + auto leftFut = camera->queueLeft(std::bind(&Frame::ExtractORB, this, true, imLeft, + reinterpret_pointer_cast(mpCamera)->mvLappingArea[0], + reinterpret_pointer_cast(mpCamera)->mvLappingArea[1])); + auto rightFut = camera->queueRight(std::bind( + &Frame::ExtractORB, this, false, imRight, + reinterpret_pointer_cast(mpCamera2)->mvLappingArea[0], + reinterpret_pointer_cast(mpCamera2)->mvLappingArea[1])); + if(!leftFut.get() || !rightFut.get()) return; #ifdef REGISTER_TIMES std::chrono::steady_clock::time_point time_EndExtORB = std::chrono::steady_clock::now(); @@ -1157,12 +1173,6 @@ Frame::Frame(const cv::Mat &imLeft, const cv::Mat &imRight, .count(); #endif - Nleft = mvKeys.size(); - Nright = mvKeysRight.size(); - N = Nleft + Nright; - - if (N == 0) return; - // This is done only for the first Frame (or after a change in the // calibration) if (mbInitialComputations) { @@ -1183,6 +1193,12 @@ Frame::Frame(const cv::Mat &imLeft, const cv::Mat &imRight, mbInitialComputations = false; } + Nleft = mvKeys.size(); + Nright = mvKeysRight.size(); + N = Nleft + Nright; + + if (N == 0) return; + mb = mbf / fx; // Sophus/Eigen @@ -1214,7 +1230,7 @@ Frame::Frame(const cv::Mat &imLeft, const cv::Mat &imRight, AssignFeaturesToGrid(); - mpMutexImu = new std::mutex(); + mpMutexImu = std::make_shared(); UndistortKeyPoints(); } @@ -1256,7 +1272,7 @@ void Frame::ComputeStereoFishEyeMatches() { sigma1 = mvLevelSigma2[mvKeys[(*it)[0].queryIdx + monoLeft].octave], sigma2 = mvLevelSigma2[mvKeysRight[(*it)[0].trainIdx + monoRight].octave]; - float depth = static_cast(mpCamera)->TriangulateMatches( + float depth = reinterpret_pointer_cast(mpCamera)->TriangulateMatches( mpCamera2, mvKeys[(*it)[0].queryIdx + monoLeft], mvKeysRight[(*it)[0].trainIdx + monoRight], mRlr, mtlr, sigma1, sigma2, p3D); @@ -1349,4 +1365,4 @@ Eigen::Vector3f Frame::UnprojectStereoFishEye(const int &i) { return mRwc * mvStereo3Dpoints[i] + mOw; } -} // namespace ORB_SLAM3 +} // namespace MORB_SLAM diff --git a/src/FrameDrawer.cc b/src/FrameDrawer.cc index 9a75cbdd..bd83c689 100644 --- a/src/FrameDrawer.cc +++ b/src/FrameDrawer.cc @@ -19,16 +19,16 @@ * ORB-SLAM3. If not, see . */ -#include "FrameDrawer.h" +#include "MORB_SLAM/FrameDrawer.h" #include #include -#include "ImprovedTypes.hpp" -#include "MapPoint.h" -#include "Atlas.h" -#include "Tracking.h" +#include "MORB_SLAM/ImprovedTypes.hpp" +#include "MORB_SLAM/MapPoint.h" +#include "MORB_SLAM/Atlas.h" +#include "MORB_SLAM/Tracking.h" -namespace ORB_SLAM3 { +namespace MORB_SLAM { FrameDrawer::FrameDrawer(const Atlas_ptr &pAtlas) : both(false), mpAtlas(pAtlas) { mState = Tracker::SYSTEM_NOT_READY; @@ -336,4 +336,4 @@ void FrameDrawer::Update(const Tracking_ptr &pTracker) { mState = static_cast(pTracker->mLastProcessedState); } -} // namespace ORB_SLAM3 +} // namespace MORB_SLAM diff --git a/src/G2oTypes.cc b/src/G2oTypes.cc index 7e275e82..00371816 100644 --- a/src/G2oTypes.cc +++ b/src/G2oTypes.cc @@ -19,11 +19,11 @@ * ORB-SLAM3. If not, see . */ -#include "G2oTypes.h" +#include "MORB_SLAM/G2oTypes.h" -#include "Converter.h" -#include "ImuTypes.h" -namespace ORB_SLAM3 { +#include "MORB_SLAM/Converter.h" +#include "MORB_SLAM/ImuTypes.h" +namespace MORB_SLAM { ImuCamPose::ImuCamPose(KeyFrame* pKF) : its(0) { // Load IMU pose @@ -726,7 +726,7 @@ void EdgeInertialGS::linearizeOplus() { Rbw1 * (VP2->estimate().twb - VP1->estimate().twb - VV1->estimate() * dt); } -EdgePriorPoseImu::EdgePriorPoseImu(ConstraintPoseImu* c) { +EdgePriorPoseImu::EdgePriorPoseImu(std::shared_ptr c) { resize(4); Rwb = c->Rwb; twb = c->twb; @@ -853,4 +853,4 @@ Eigen::Matrix3d Skew(const Eigen::Vector3d& w) { return W; } -} // namespace ORB_SLAM3 +} // namespace MORB_SLAM diff --git a/src/GeometricTools.cc b/src/GeometricTools.cc index a97002c3..6c66991f 100644 --- a/src/GeometricTools.cc +++ b/src/GeometricTools.cc @@ -19,11 +19,11 @@ * ORB-SLAM3. If not, see . */ -#include "GeometricTools.h" +#include "MORB_SLAM/GeometricTools.h" -#include "KeyFrame.h" +#include "MORB_SLAM/KeyFrame.h" -namespace ORB_SLAM3 { +namespace MORB_SLAM { Eigen::Matrix3f GeometricTools::ComputeF12(KeyFrame *&pKF1, KeyFrame *&pKF2) { Sophus::SE3 Tc1w = pKF1->GetPose(); @@ -71,4 +71,4 @@ bool GeometricTools::Triangulate(Eigen::Vector3f &x_c1, Eigen::Vector3f &x_c2, return true; } -} // namespace ORB_SLAM3 +} // namespace MORB_SLAM diff --git a/src/ImuTypes.cc b/src/ImuTypes.cc index b9b3eb12..ef0536a9 100644 --- a/src/ImuTypes.cc +++ b/src/ImuTypes.cc @@ -19,14 +19,14 @@ * ORB-SLAM3. If not, see . */ -#include "ImuTypes.h" +#include "MORB_SLAM/ImuTypes.h" #include -#include "Converter.h" -#include "GeometricTools.h" +#include "MORB_SLAM/Converter.h" +#include "MORB_SLAM/GeometricTools.h" -namespace ORB_SLAM3 { +namespace MORB_SLAM { namespace IMU { @@ -406,4 +406,4 @@ Calib::Calib(const Calib &calib) { } // namespace IMU -} // namespace ORB_SLAM3 +} // namespace MORB_SLAM diff --git a/src/KeyFrame.cc b/src/KeyFrame.cc index cb9800c7..034ba7a0 100644 --- a/src/KeyFrame.cc +++ b/src/KeyFrame.cc @@ -19,14 +19,14 @@ * ORB-SLAM3. If not, see . */ -#include "KeyFrame.h" +#include "MORB_SLAM/KeyFrame.h" #include -#include "Converter.h" -#include "ImuTypes.h" +#include "MORB_SLAM/Converter.h" +#include "MORB_SLAM/ImuTypes.h" -namespace ORB_SLAM3 { +namespace MORB_SLAM { long unsigned int KeyFrame::nNextId = 0; @@ -65,10 +65,10 @@ KeyFrame::KeyFrame() mb(0), mThDepth(0), N(0), - /*mvKeys(static_cast>(NULL)), - mvKeysUn(static_cast>(NULL)), - mvuRight(static_cast>(NULL)), - mvDepth(static_cast>(NULL)),*/ + /*mvKeys(static_cast>(nullptr)), + mvKeysUn(static_cast>(nullptr)), + mvuRight(static_cast>(nullptr)), + mvDepth(static_cast>(nullptr)),*/ mnScaleLevels(0), mfScaleFactor(0), mfLogScaleFactor(0), @@ -79,11 +79,11 @@ KeyFrame::KeyFrame() mnMinY(0), mnMaxX(0), mnMaxY(0), - mPrevKF(static_cast(NULL)), - mNextKF(static_cast(NULL)), + mPrevKF(nullptr), + mNextKF(nullptr), mbHasVelocity(false), mbFirstConnection(true), - mpParent(NULL), + mpParent(nullptr), mbNotErase(false), mbToBeErased(false), mbBad(false), @@ -91,7 +91,7 @@ KeyFrame::KeyFrame() NLeft(0), NRight(0) {} -KeyFrame::KeyFrame(Frame &F, Map *pMap, KeyFrameDatabase *pKFDB) +KeyFrame::KeyFrame(Frame &F, std::shared_ptr pMap, KeyFrameDatabase *pKFDB) : bImu(pMap->isImuInitialized()), mnFrameId(F.mnId), mTimeStamp(F.mTimeStamp), @@ -143,8 +143,8 @@ KeyFrame::KeyFrame(Frame &F, Map *pMap, KeyFrameDatabase *pKFDB) mnMinY(F.mnMinY), mnMaxX(F.mnMaxX), mnMaxY(F.mnMaxY), - mPrevKF(NULL), - mNextKF(NULL), + mPrevKF(nullptr), + mNextKF(nullptr), mpImuPreintegrated(F.mpImuPreintegrated), mImuCalib(F.mImuCalib), mNameFile(F.mNameFile), @@ -156,7 +156,7 @@ KeyFrame::KeyFrame(Frame &F, Map *pMap, KeyFrameDatabase *pKFDB) mpKeyFrameDB(pKFDB), mpORBvocabulary(F.mpORBvocabulary), mbFirstConnection(true), - mpParent(NULL), + mpParent(nullptr), mbNotErase(false), mbToBeErased(false), mbBad(false), @@ -384,15 +384,15 @@ void KeyFrame::AddMapPoint(MapPoint *pMP, const size_t &idx) { void KeyFrame::EraseMapPointMatch(const int &idx) { unique_lock lock(mMutexFeatures); - mvpMapPoints[idx] = static_cast(NULL); + mvpMapPoints[idx] = nullptr; } void KeyFrame::EraseMapPointMatch(MapPoint *pMP) { tuple indexes = pMP->GetIndexInKeyFrame(this); int leftIndex = get<0>(indexes), rightIndex = get<1>(indexes); - if (leftIndex != -1) mvpMapPoints[leftIndex] = static_cast(NULL); + if (leftIndex != -1) mvpMapPoints[leftIndex] = nullptr; if (rightIndex != -1) - mvpMapPoints[rightIndex] = static_cast(NULL); + mvpMapPoints[rightIndex] = nullptr; } void KeyFrame::ReplaceMapPointMatch(const int &idx, MapPoint *pMP) { @@ -479,7 +479,7 @@ void KeyFrame::UpdateConnections(bool upParent) { // In case no keyframe counter is over threshold add the one with maximum // counter int nmax = 0; - KeyFrame *pKFmax = NULL; + KeyFrame *pKFmax = nullptr; int th = 15; vector> vPairs; @@ -846,18 +846,18 @@ IMU::Bias KeyFrame::GetImuBias() { return mImuBias; } -Map *KeyFrame::GetMap() { +std::shared_ptr KeyFrame::GetMap() { unique_lock lock(mMutexMap); return mpMap; } -void KeyFrame::UpdateMap(Map *pMap) { +void KeyFrame::UpdateMap(std::shared_ptr pMap) { unique_lock lock(mMutexMap); mpMap = pMap; } void KeyFrame::PreSave(set &spKF, set &spMP, - set &spCam) { + set> &spCam) { // Save the id of each MapPoint in this KF, there can be null // pointer in the vector mvBackupMapPointsId.clear(); @@ -932,7 +932,7 @@ void KeyFrame::PreSave(set &spKF, set &spMP, void KeyFrame::PostLoad(map &mpKFid, map &mpMPid, - map &mpCamId) { + map> &mpCamId) { // Rebuild the empty variables // Pose @@ -948,7 +948,7 @@ void KeyFrame::PostLoad(map &mpKFid, if (mvBackupMapPointsId[i] != -1) mvpMapPoints[i] = mpMPid[mvBackupMapPointsId[i]]; else - mvpMapPoints[i] = static_cast(NULL); + mvpMapPoints[i] = nullptr; } // Conected KeyFrames with him weight @@ -1157,4 +1157,4 @@ void KeyFrame::SetKeyFrameDatabase(KeyFrameDatabase *pKFDB) { mpKeyFrameDB = pKFDB; } -} // namespace ORB_SLAM3 +} // namespace MORB_SLAM diff --git a/src/KeyFrameDatabase.cc b/src/KeyFrameDatabase.cc index 45d5a5b0..bed20300 100644 --- a/src/KeyFrameDatabase.cc +++ b/src/KeyFrameDatabase.cc @@ -19,16 +19,16 @@ * ORB-SLAM3. If not, see . */ -#include "KeyFrameDatabase.h" +#include "MORB_SLAM/KeyFrameDatabase.h" #include #include "DBoW2/BowVector.h" -#include "KeyFrame.h" +#include "MORB_SLAM/KeyFrame.h" using namespace std; -namespace ORB_SLAM3 { +namespace MORB_SLAM { KeyFrameDatabase::KeyFrameDatabase(const ORBVocabulary& voc) : mpVoc(&voc) { mvInvertedFile.resize(voc.size()); @@ -68,7 +68,7 @@ void KeyFrameDatabase::clear() { mvInvertedFile.resize(mpVoc->size()); } -void KeyFrameDatabase::clearMap(Map* pMap) { +void KeyFrameDatabase::clearMap(std::shared_ptr pMap) { unique_lock lock(mMutex); // Erase elements in the Inverse File for the entry @@ -705,7 +705,7 @@ void KeyFrameDatabase::DetectNBestCandidates(KeyFrame* pKF, } vector KeyFrameDatabase::DetectRelocalizationCandidates(Frame* F, - Map* pMap) { + std::shared_ptr pMap) { list lKFsSharingWords; // Search all keyframes that share a word with current frame @@ -822,4 +822,4 @@ void KeyFrameDatabase::SetORBVocabulary(ORBVocabulary* pORBVoc) { mvInvertedFile.resize(mpVoc->size()); } -} // namespace ORB_SLAM3 +} // namespace MORB_SLAM diff --git a/src/LocalMapping.cc b/src/LocalMapping.cc index 443189a7..17e8183e 100644 --- a/src/LocalMapping.cc +++ b/src/LocalMapping.cc @@ -19,18 +19,19 @@ * ORB-SLAM3. If not, see . */ -#include "LocalMapping.h" +#include "MORB_SLAM/LocalMapping.h" #include #include -#include "Converter.h" -#include "GeometricTools.h" -#include "LoopClosing.h" -#include "ORBmatcher.h" -#include "Optimizer.h" +#include "MORB_SLAM/Converter.h" +#include "MORB_SLAM/GeometricTools.h" +#include "MORB_SLAM/LoopClosing.h" +#include "MORB_SLAM/ORBmatcher.h" +#include "MORB_SLAM/Optimizer.h" -namespace ORB_SLAM3 { +#include +namespace MORB_SLAM { LocalMapping::LocalMapping(System* pSys, const Atlas_ptr &pAtlas, const float bMonocular, bool bInertial, const string& _strSeqName) @@ -448,8 +449,8 @@ void LocalMapping::CreateNewMapPoints() { KeyFrame* pKF2 = vpNeighKFs[i]; - GeometricCamera *pCamera1 = mpCurrentKeyFrame->mpCamera, - *pCamera2 = pKF2->mpCamera; + std::shared_ptr pCamera1 = mpCurrentKeyFrame->mpCamera, + pCamera2 = pKF2->mpCamera; // Check first that baseline is not too short Eigen::Vector3f Ow2 = pKF2->GetCameraCenter(); @@ -999,8 +1000,8 @@ void LocalMapping::KeyFrameCulling() { pKF->mpImuPreintegrated); pKF->mNextKF->mPrevKF = pKF->mPrevKF; pKF->mPrevKF->mNextKF = pKF->mNextKF; - pKF->mNextKF = NULL; - pKF->mPrevKF = NULL; + pKF->mNextKF = nullptr; + pKF->mPrevKF = nullptr; pKF->SetBadFlag(); } else if (!mpCurrentKeyFrame->GetMap()->GetIniertialBA2() && ((pKF->GetImuPosition() - pKF->mPrevKF->GetImuPosition()) @@ -1010,8 +1011,8 @@ void LocalMapping::KeyFrameCulling() { pKF->mpImuPreintegrated); pKF->mNextKF->mPrevKF = pKF->mPrevKF; pKF->mPrevKF->mNextKF = pKF->mNextKF; - pKF->mNextKF = NULL; - pKF->mPrevKF = NULL; + pKF->mNextKF = nullptr; + pKF->mPrevKF = nullptr; pKF->SetBadFlag(); } } @@ -1043,7 +1044,7 @@ void LocalMapping::RequestReset() { cout << "LM: Map reset, Done!!!" << endl; } -void LocalMapping::RequestResetActiveMap(Map* pMap) { +void LocalMapping::RequestResetActiveMap(std::shared_ptr pMap) { { unique_lock lock(mMutexReset); cout << "LM: Active map reset recieved" << endl; @@ -1167,13 +1168,21 @@ void LocalMapping::InitializeIMU(float priorG, float priorA, bool bFIBA) { Eigen::Matrix3f Rwg; Eigen::Vector3f dirG; dirG.setZero(); + // std::cout << "Keyframes---------------------------------------------: " << N << std::endl; for (vector::iterator itKF = vpKF.begin(); itKF != vpKF.end(); itKF++) { - if (!(*itKF)->mpImuPreintegrated) continue; - if (!(*itKF)->mPrevKF) continue; + + // std::cout << "Hello" << std::endl; + if (!(*itKF)->mpImuPreintegrated) continue; // || isnan((*itKF)->mpImuPreintegrated->GetUpdatedDeltaVelocity().sum()) + if (!(*itKF)->mPrevKF) continue; // || isnan((*itKF)->mPrevKF->GetImuRotation().sum() + + // std::cout << "initDirG------------------------------------------------------: " << dirG << std::endl; + // std::cout << "getImuRot------------------------------------------------------: " << (*itKF)->mPrevKF->GetImuRotation() << std::endl; + // std::cout << "getUpdDeltaV------------------------------------------------------: " <<(*itKF)->mpImuPreintegrated->GetUpdatedDeltaVelocity() << std::endl; dirG -= (*itKF)->mPrevKF->GetImuRotation() * (*itKF)->mpImuPreintegrated->GetUpdatedDeltaVelocity(); + // std::cout << "dirGLoop------------------------------------------------------: " << dirG << std::endl; Eigen::Vector3f _vel = ((*itKF)->GetImuPosition() - (*itKF)->mPrevKF->GetImuPosition()) / (*itKF)->mpImuPreintegrated->dT; @@ -1181,16 +1190,29 @@ void LocalMapping::InitializeIMU(float priorG, float priorA, bool bFIBA) { (*itKF)->mPrevKF->SetVelocity(_vel); } + // if(dirG.sum() == 0){ mRwg << 0.99600422382354736,0.059227496385574341,0.066840663552284241, + // 0.059227496385574341,0.12209725379943848,-0.99074935913085938, + // -0.066840663552284241,0.99074935913085938,0.11810147762298584; + // } else{ + std::cout << "dirGBeforeNorm------------------------------------------------------: " << dirG << std::endl; dirG = dirG / dirG.norm(); + std::cout << "dirGAfterNorm------------------------------------------------------: " << dirG << std::endl; Eigen::Vector3f gI(0.0f, 0.0f, -1.0f); Eigen::Vector3f v = gI.cross(dirG); const float nv = v.norm(); + const float cosg = gI.dot(dirG); const float ang = acos(cosg); + + std::cout << "v------------------------------------------------------: " << v << std::endl; + std::cout << "ang------------------------------------------------------: " << ang << std::endl; + std::cout << "nv------------------------------------------------------: " << nv << std::endl; + Eigen::Vector3f vzg = v * ang / nv; Rwg = Sophus::SO3f::exp(vzg).matrix(); mRwg = Rwg.cast(); mTinit = mpCurrentKeyFrame->mTimeStamp - mFirstTs; +//} } else { mRwg = Eigen::Matrix3d::Identity(); mbg = mpCurrentKeyFrame->GetGyroBias().cast(); @@ -1242,11 +1264,11 @@ void LocalMapping::InitializeIMU(float priorG, float priorA, bool bFIBA) { if (bFIBA) { if (priorA != 0.f) Optimizer::FullInertialBA(mpAtlas->GetCurrentMap(), 100, false, - mpCurrentKeyFrame->mnId, NULL, true, priorG, + mpCurrentKeyFrame->mnId, nullptr, true, priorG, priorA); else Optimizer::FullInertialBA(mpAtlas->GetCurrentMap(), 100, false, - mpCurrentKeyFrame->mnId, NULL, false); + mpCurrentKeyFrame->mnId, nullptr, false); } // std::chrono::steady_clock::time_point t5 = std::chrono::steady_clock::now(); // UNUSED @@ -1439,4 +1461,4 @@ double LocalMapping::GetCurrKFTime() { KeyFrame* LocalMapping::GetCurrKF() { return mpCurrentKeyFrame; } -} // namespace ORB_SLAM3 +} // namespace MORB_SLAM diff --git a/src/LoopClosing.cc b/src/LoopClosing.cc index a0ce746f..d47542c7 100644 --- a/src/LoopClosing.cc +++ b/src/LoopClosing.cc @@ -19,19 +19,19 @@ * ORB-SLAM3. If not, see . */ -#include "LoopClosing.h" +#include "MORB_SLAM/LoopClosing.h" #include #include -#include "ImprovedTypes.hpp" -#include "Converter.h" -#include "G2oTypes.h" -#include "ORBmatcher.h" -#include "Optimizer.h" -#include "Sim3Solver.h" +#include "MORB_SLAM/ImprovedTypes.hpp" +#include "MORB_SLAM/Converter.h" +#include "MORB_SLAM/G2oTypes.h" +#include "MORB_SLAM/ORBmatcher.h" +#include "MORB_SLAM/Optimizer.h" +#include "MORB_SLAM/Sim3Solver.h" -namespace ORB_SLAM3 { +namespace MORB_SLAM { LoopClosing::LoopClosing(const Atlas_ptr &pAtlas, KeyFrameDatabase* pDB, ORBVocabulary* pVoc, const bool bFixScale, @@ -51,8 +51,8 @@ LoopClosing::LoopClosing(const Atlas_ptr &pAtlas, KeyFrameDatabase* pDB, mpKeyFrameDB(pDB), mpORBVocabulary(pVoc), mnCovisibilityConsistencyTh(3), - mpLastCurrentKF(static_cast(NULL)), - mpMatchedKF(NULL), + mpLastCurrentKF(nullptr), + mpMatchedKF(nullptr), mbLoopDetected(false), mnLoopNumCoincidences(0), mnLoopNumNotFound(0), @@ -62,7 +62,7 @@ LoopClosing::LoopClosing(const Atlas_ptr &pAtlas, KeyFrameDatabase* pDB, mbRunningGBA(false), mbFinishedGBA(true), mbStopGBA(false), - mpThreadGBA(NULL), + mpThreadGBA(nullptr), mbFixScale(bFixScale), mnFullBAIdx(0), mstrFolderSubTraj("SubTrajectories/"), @@ -574,7 +574,7 @@ bool LoopClosing::DetectAndReffineSim3FromLastKF( vector vpMatchedMP; vpMatchedMP.resize(mpCurrentKF->GetMapPointMatches().size(), - static_cast(NULL)); + nullptr); nNumProjMatches = FindMatchesByProjection(pCurrentKF, pMatchedKF, gScw_estimation, @@ -629,7 +629,6 @@ bool LoopClosing::DetectCommonRegionsFromBoW( std::vector vpCovKFi = pKFi->GetBestCovisibilityKeyFrames(nNumCovisibles); if (vpCovKFi.empty()) { - std::cout << "Covisible list empty" << std::endl; vpCovKFi.push_back(pKFi); } else { vpCovKFi.push_back(vpCovKFi[0]); @@ -661,9 +660,9 @@ bool LoopClosing::DetectCommonRegionsFromBoW( int nMostBoWNumMatches = 0; std::vector vpMatchedPoints = std::vector( - mpCurrentKF->GetMapPointMatches().size(), static_cast(NULL)); + mpCurrentKF->GetMapPointMatches().size(), nullptr); std::vector vpKeyFrameMatchedMP = std::vector( - mpCurrentKF->GetMapPointMatches().size(), static_cast(NULL)); + mpCurrentKF->GetMapPointMatches().size(), nullptr); // int nIndexMostBoWMatchesKF=0; // UNUSED for (size_t j = 0; j < vpCovKFi.size(); ++j) { @@ -766,10 +765,10 @@ bool LoopClosing::DetectCommonRegionsFromBoW( vector vpMatchedMP; vpMatchedMP.resize(mpCurrentKF->GetMapPointMatches().size(), - static_cast(NULL)); + nullptr); vector vpMatchedKF; vpMatchedKF.resize(mpCurrentKF->GetMapPointMatches().size(), - static_cast(NULL)); + nullptr); int numProjMatches = matcher.SearchByProjection( mpCurrentKF, mScw, vpMapPoints, vpKeyFrames, vpMatchedMP, vpMatchedKF, 8, 1.5); @@ -801,7 +800,7 @@ bool LoopClosing::DetectCommonRegionsFromBoW( vector vpMatchedMP; vpMatchedMP.resize(mpCurrentKF->GetMapPointMatches().size(), - static_cast(NULL)); + nullptr); int numProjOptMatches = matcher.SearchByProjection( mpCurrentKF, mScw, vpMapPoints, vpMatchedMP, 5, 1.0); @@ -970,7 +969,7 @@ int LoopClosing::FindMatchesByProjection( ORBmatcher matcher(0.9, true); vpMatchedMapPoints.resize(pCurrentKF->GetMapPointMatches().size(), - static_cast(NULL)); + nullptr); int num_matches = matcher.SearchByProjection(pCurrentKF, mScw, vpMapPoints, vpMatchedMapPoints, 3, 1.5); @@ -1033,7 +1032,7 @@ void LoopClosing::CorrectLoop() { mg2oLoopScw.translation() / mg2oLoopScw.scale()); mpCurrentKF->SetPose(correctedTcw.cast()); - Map* pLoopMap = mpCurrentKF->GetMap(); + std::shared_ptr pLoopMap = mpCurrentKF->GetMap(); #ifdef REGISTER_TIMES /*KeyFrame* pKF = mpCurrentKF; @@ -1289,8 +1288,8 @@ void LoopClosing::MergeLocal() { // and MPs from the current map. Later, the elements of the current map will // be transform to the new active map reference, in order to keep real time // tracking - Map* pCurrentMap = mpCurrentKF->GetMap(); - Map* pMergeMap = mpMergeMatchedKF->GetMap(); + std::shared_ptr pCurrentMap = mpCurrentKF->GetMap(); + std::shared_ptr pMergeMap = mpMergeMatchedKF->GetMap(); // std::cout << "Merge local, Active map: " << pCurrentMap->GetId() << // std::endl; std::cout << "Merge local, Non-Active map: " << @@ -1859,8 +1858,8 @@ void LoopClosing::MergeLocal2() { } // cout << "Local Map stopped" << endl; - Map* pCurrentMap = mpCurrentKF->GetMap(); - Map* pMergeMap = mpMergeMatchedKF->GetMap(); + std::shared_ptr pCurrentMap = mpCurrentKF->GetMap(); + std::shared_ptr pMergeMap = mpMergeMatchedKF->GetMap(); { float s_on = mSold_new.scale(); @@ -2163,13 +2162,13 @@ void LoopClosing::SearchAndFuse(const KeyFrameAndPose& CorrectedPosesMap, mit != mend; mit++) { int num_replaces = 0; KeyFrame* pKFi = mit->first; - Map* pMap = pKFi->GetMap(); + std::shared_ptr pMap = pKFi->GetMap(); g2o::Sim3 g2oScw = mit->second; Sophus::Sim3f Scw = Converter::toSophus(g2oScw); vector vpReplacePoints(vpMapPoints.size(), - static_cast(NULL)); + nullptr); /*int numFused = */matcher.Fuse(pKFi, Scw, vpMapPoints, 4, vpReplacePoints); // Get Map Mutex @@ -2201,7 +2200,7 @@ void LoopClosing::SearchAndFuse(const vector& vConectedKFs, mit++) { int num_replaces = 0; KeyFrame* pKF = (*mit); - Map* pMap = pKF->GetMap(); + std::shared_ptr pMap = pKF->GetMap(); Sophus::SE3f Tcw = pKF->GetPose(); Sophus::Sim3f Scw(Tcw.unit_quaternion(), Tcw.translation()); Scw.setScale(1.f); @@ -2210,7 +2209,7 @@ void LoopClosing::SearchAndFuse(const vector& vConectedKFs, Scw.translation() - Tcw.translation() << std::endl << Scw.scale() - 1.f << std::endl;*/ vector vpReplacePoints(vpMapPoints.size(), - static_cast(NULL)); + nullptr); matcher.Fuse(pKF, Scw, vpMapPoints, 4, vpReplacePoints); // Get Map Mutex @@ -2244,7 +2243,7 @@ void LoopClosing::RequestReset() { } } -void LoopClosing::RequestResetActiveMap(Map* pMap) { +void LoopClosing::RequestResetActiveMap(std::shared_ptr pMap) { { unique_lock lock(mMutexReset); mbResetActiveMapRequested = true; @@ -2281,7 +2280,7 @@ void LoopClosing::ResetIfRequested() { } } -void LoopClosing::RunGlobalBundleAdjustment(Map* pActiveMap, +void LoopClosing::RunGlobalBundleAdjustment(std::shared_ptr pActiveMap, unsigned long nLoopKF) { Verbose::PrintMess("Starting Global Bundle Adjustment", Verbose::VERBOSITY_NORMAL); @@ -2556,4 +2555,4 @@ bool LoopClosing::isFinished() { return mbFinished; } -} // namespace ORB_SLAM3 +} // namespace MORB_SLAM diff --git a/src/MLPnPsolver.cpp b/src/MLPnPsolver.cpp index 0e31d5f8..a8f6dc8e 100644 --- a/src/MLPnPsolver.cpp +++ b/src/MLPnPsolver.cpp @@ -46,12 +46,12 @@ * SUCH DAMAGE. * ******************************************************************************/ -#include "MLPnPsolver.h" +#include "MORB_SLAM/MLPnPsolver.h" #include -namespace ORB_SLAM3 { +namespace MORB_SLAM { MLPnPsolver::MLPnPsolver(const Frame &F, const vector &vpMapPointMatches): mnInliersi(0), mnIterations(0), mnBestInliers(0), N(0), mpCamera(F.mpCamera){ mvpMapPointMatches = vpMapPointMatches; diff --git a/src/Map.cc b/src/Map.cc index 7259d344..a5e1eb62 100644 --- a/src/Map.cc +++ b/src/Map.cc @@ -19,16 +19,16 @@ * ORB-SLAM3. If not, see . */ -#include "Map.h" +#include "MORB_SLAM/Map.h" #include -namespace ORB_SLAM3 { +namespace MORB_SLAM { long unsigned int Map::nNextId = 0; Map::Map() - : mpFirstRegionKF(static_cast(NULL)), + : mpFirstRegionKF(nullptr), mbFail(false), mbImuInitialized(false), mnMapChange(0), @@ -42,11 +42,11 @@ Map::Map() mbIMU_BA1(false), mbIMU_BA2(false) { mnId = nNextId++; - mThumbnail = static_cast(NULL); + mThumbnail = nullptr; } Map::Map(int initKFid) - : mpFirstRegionKF(static_cast(NULL)), + : mpFirstRegionKF(nullptr), mbFail(false), mbImuInitialized(false), mnMapChange(0), @@ -61,7 +61,7 @@ Map::Map(int initKFid) mbIMU_BA1(false), mbIMU_BA2(false) { mnId = nNextId++; - mThumbnail = static_cast(NULL); + mThumbnail = nullptr; } Map::~Map() { @@ -72,7 +72,7 @@ Map::~Map() { mspKeyFrames.clear(); if (mThumbnail) delete mThumbnail; - mThumbnail = static_cast(NULL); + mThumbnail = nullptr; mvpReferenceMapPoints.clear(); mvpKeyFrameOrigins.clear(); @@ -153,11 +153,13 @@ int Map::GetLastBigChangeIdx() { vector Map::GetAllKeyFrames() { unique_lock lock(mMutexMap); + // std::cout << "Size of mspKeyFrames: " << mspKeyFrames.size() << std::endl; return vector(mspKeyFrames.begin(), mspKeyFrames.end()); } vector Map::GetAllMapPoints() { unique_lock lock(mMutexMap); + // std::cout << "Size of mspMapPoints: " << mspMapPoints.size() << std::endl; return vector(mspMapPoints.begin(), mspMapPoints.end()); } @@ -207,7 +209,7 @@ void Map::clear() { send = mspKeyFrames.end(); sit != send; sit++) { KeyFrame* pKF = *sit; - pKF->UpdateMap(static_cast(NULL)); + pKF->UpdateMap(nullptr); // delete *sit; } @@ -319,7 +321,12 @@ void Map::SetLastMapChange(int currentChangeId) { mnMapChangeNotified = currentChangeId; } -void Map::PreSave(std::set& spCams) { +void Map::PreSave(std::set>& spCams, std::shared_ptr sharedMap) { + + if(this != sharedMap.get()){ + throw std::runtime_error("The shared map is not equivalent to this"); + } + int nMPWithoutObs = 0; std::set tmp_mspMapPoints; @@ -336,7 +343,7 @@ void Map::PreSave(std::set& spCams) { for (map>::iterator it = mpObs.begin(), end = mpObs.end(); it != end; ++it) { - if (it->first->GetMap() != this || it->first->isBad()) { + if ((it->first->GetMap() != sharedMap) || it->first->isBad()) { pMPi->EraseObservation(it->first); } } @@ -386,7 +393,12 @@ void Map::PostLoad( KeyFrameDatabase* pKFDB, ORBVocabulary* pORBVoc /*, map& mpKeyFrameId*/, - map& mpCams) { + map>& mpCams, std::shared_ptr sharedMap) { + + if(this != sharedMap.get()){ + throw std::runtime_error("The shared map is not equivalent to this"); + } + std::copy(mvpBackupMapPoints.begin(), mvpBackupMapPoints.end(), std::inserter(mspMapPoints, mspMapPoints.begin())); std::copy(mvpBackupKeyFrames.begin(), mvpBackupKeyFrames.end(), @@ -396,7 +408,7 @@ void Map::PostLoad( for (MapPoint* pMPi : mspMapPoints) { if (!pMPi || pMPi->isBad()) continue; - pMPi->UpdateMap(this); + pMPi->UpdateMap(sharedMap); mpMapPointId[pMPi->mnId] = pMPi; } @@ -404,7 +416,7 @@ void Map::PostLoad( for (KeyFrame* pKFi : mspKeyFrames) { if (!pKFi || pKFi->isBad()) continue; - pKFi->UpdateMap(this); + pKFi->UpdateMap(sharedMap); pKFi->SetORBVocabulary(pORBVoc); pKFi->SetKeyFrameDatabase(pKFDB); mpKeyFrameId[pKFi->mnId] = pKFi; @@ -441,4 +453,4 @@ void Map::PostLoad( mvpBackupMapPoints.clear(); } -} // namespace ORB_SLAM3 +} // namespace MORB_SLAM diff --git a/src/MapDrawer.cc b/src/MapDrawer.cc index 51a7ab19..5e34ccb2 100644 --- a/src/MapDrawer.cc +++ b/src/MapDrawer.cc @@ -19,18 +19,18 @@ * ORB-SLAM3. If not, see . */ -#include "MapDrawer.h" +#include "MORB_SLAM/MapDrawer.h" #include #include #include -#include "Atlas.h" -#include "KeyFrame.h" -#include "MapPoint.h" +#include "MORB_SLAM/Atlas.h" +#include "MORB_SLAM/KeyFrame.h" +#include "MORB_SLAM/MapPoint.h" -namespace ORB_SLAM3 { +namespace MORB_SLAM { MapDrawer::MapDrawer(const Atlas_ptr &pAtlas, const std::string &strSettingPath): mpAtlas(pAtlas){ cv::FileStorage fSettings(strSettingPath, cv::FileStorage::READ); @@ -120,7 +120,7 @@ bool MapDrawer::ParseViewerParamFile(cv::FileStorage &fSettings) { } void MapDrawer::DrawMapPoints() { - Map *pActiveMap = mpAtlas->GetCurrentMap(); + std::shared_ptr pActiveMap = mpAtlas->GetCurrentMap(); if (!pActiveMap) return; const vector &vpMPs = pActiveMap->GetAllMapPoints(); @@ -162,7 +162,7 @@ void MapDrawer::DrawKeyFrames(const bool bDrawKF, const bool bDrawGraph, const float h = w * 0.75; const float z = w * 0.6; - Map *pActiveMap = mpAtlas->GetCurrentMap(); + std::shared_ptr pActiveMap = mpAtlas->GetCurrentMap(); // DEBUG LBA std::set sOptKFs = pActiveMap->msOptKFs; std::set sFixedKFs = pActiveMap->msFixedKFs; @@ -295,10 +295,10 @@ void MapDrawer::DrawKeyFrames(const bool bDrawKF, const bool bDrawGraph, glEnd(); } - vector vpMaps = mpAtlas->GetAllMaps(); + vector> vpMaps = mpAtlas->GetAllMaps(); if (bDrawKF) { - for (Map *pMap : vpMaps) { + for (std::shared_ptr pMap : vpMaps) { if (pMap == pActiveMap) continue; vector vpKFs = pMap->GetAllKeyFrames(); @@ -419,4 +419,4 @@ void MapDrawer::GetCurrentOpenGLCameraMatrix(pangolin::OpenGlMatrix &M, MOw.m[13] = Twc(1, 3); MOw.m[14] = Twc(2, 3); } -} // namespace ORB_SLAM3 +} // namespace MORB_SLAM diff --git a/src/MapPoint.cc b/src/MapPoint.cc index 979ae419..87ec00cc 100644 --- a/src/MapPoint.cc +++ b/src/MapPoint.cc @@ -19,13 +19,13 @@ * ORB-SLAM3. If not, see . */ -#include "MapPoint.h" +#include "MORB_SLAM/MapPoint.h" #include -#include "ORBmatcher.h" +#include "MORB_SLAM/ORBmatcher.h" -namespace ORB_SLAM3 { +namespace MORB_SLAM { long unsigned int MapPoint::nNextId = 0; mutex MapPoint::mGlobalMutex; @@ -45,11 +45,11 @@ MapPoint::MapPoint() mnVisible(1), mnFound(1), mbBad(false), - mpReplaced(static_cast(NULL)) { - mpReplaced = static_cast(NULL); + mpReplaced(nullptr) { + mpReplaced = nullptr; } -MapPoint::MapPoint(const Eigen::Vector3f& Pos, KeyFrame* pRefKF, Map* pMap) +MapPoint::MapPoint(const Eigen::Vector3f& Pos, KeyFrame* pRefKF, std::shared_ptr pMap) : mnFirstKFid(pRefKF->mnId), mnFirstFrame(pRefKF->mnFrameId), nObs(0), @@ -66,7 +66,7 @@ MapPoint::MapPoint(const Eigen::Vector3f& Pos, KeyFrame* pRefKF, Map* pMap) mnVisible(1), mnFound(1), mbBad(false), - mpReplaced(static_cast(NULL)), + mpReplaced(nullptr), mfMinDistance(0), mfMaxDistance(0), mpMap(pMap) { @@ -84,7 +84,7 @@ MapPoint::MapPoint(const Eigen::Vector3f& Pos, KeyFrame* pRefKF, Map* pMap) } MapPoint::MapPoint(const double invDepth, cv::Point2f uv_init, KeyFrame* pRefKF, - KeyFrame* pHostKF, Map* pMap) + KeyFrame* pHostKF, std::shared_ptr pMap) : mnFirstKFid(pRefKF->mnId), mnFirstFrame(pRefKF->mnFrameId), nObs(0), @@ -101,7 +101,7 @@ MapPoint::MapPoint(const double invDepth, cv::Point2f uv_init, KeyFrame* pRefKF, mnVisible(1), mnFound(1), mbBad(false), - mpReplaced(static_cast(NULL)), + mpReplaced(nullptr), mfMinDistance(0), mfMaxDistance(0), mpMap(pMap) { @@ -119,7 +119,7 @@ MapPoint::MapPoint(const double invDepth, cv::Point2f uv_init, KeyFrame* pRefKF, mnId = nNextId++; } -MapPoint::MapPoint(const Eigen::Vector3f& Pos, Map* pMap, Frame* pFrame, +MapPoint::MapPoint(const Eigen::Vector3f& Pos, std::shared_ptr pMap, Frame* pFrame, const int& idxF) : mnFirstKFid(-1), mnFirstFrame(pFrame->mnId), @@ -133,11 +133,11 @@ MapPoint::MapPoint(const Eigen::Vector3f& Pos, Map* pMap, Frame* pFrame, mnCorrectedReference(0), mnBAGlobalForKF(0), mnOriginMapId(pMap->GetId()), - mpRefKF(static_cast(NULL)), + mpRefKF(nullptr), mnVisible(1), mnFound(1), mbBad(false), - mpReplaced(NULL), + mpReplaced(nullptr), mpMap(pMap) { SetWorldPos(Pos); @@ -578,12 +578,12 @@ void MapPoint::PrintObservations() { } } -Map* MapPoint::GetMap() { +std::shared_ptr MapPoint::GetMap() { unique_lock lock(mMutexMap); return mpMap; } -void MapPoint::UpdateMap(Map* pMap) { +void MapPoint::UpdateMap(std::shared_ptr pMap) { unique_lock lock(mMutexMap); mpMap = pMap; } @@ -628,7 +628,7 @@ void MapPoint::PostLoad(map& mpKFid, cout << "ERROR: MP without KF reference " << mBackupRefKFId << "; Num obs: " << nObs << endl; } - mpReplaced = static_cast(NULL); + mpReplaced = nullptr; if (mBackupReplacedId >= 0) { map::iterator it = mpMPid.find(mBackupReplacedId); @@ -654,4 +654,4 @@ void MapPoint::PostLoad(map& mpKFid, mBackupObservationsId2.clear(); } -} // namespace ORB_SLAM3 +} // namespace MORB_SLAM diff --git a/src/ORBextractor.cc b/src/ORBextractor.cc index 340a7afa..5c4c9d3f 100644 --- a/src/ORBextractor.cc +++ b/src/ORBextractor.cc @@ -54,7 +54,7 @@ * */ -#include "ORBextractor.h" +#include "MORB_SLAM/ORBextractor.h" #include #include @@ -66,7 +66,7 @@ using namespace cv; using namespace std; -namespace ORB_SLAM3 { +namespace MORB_SLAM { const int PATCH_SIZE = 31; const int HALF_PATCH_SIZE = 15; @@ -477,7 +477,7 @@ void ExtractorNode::DivideNode(ExtractorNode& n1, ExtractorNode& n2, const int halfX = ceil(static_cast(UR.x - UL.x) / 2); const int halfY = ceil(static_cast(BR.y - UL.y) / 2); - // Define boundaries of childs + // Define boundaries of children n1.UL = UL; n1.UR = cv::Point2i(UL.x + halfX, UL.y); n1.BL = cv::Point2i(UL.x, UL.y + halfY); @@ -502,7 +502,7 @@ void ExtractorNode::DivideNode(ExtractorNode& n1, ExtractorNode& n2, n4.BR = BR; n4.vKeys.reserve(vKeys.size()); - // Associate points to childs + // Associate points to children for (size_t i = 0; i < vKeys.size(); i++) { const cv::KeyPoint& kp = vKeys[i]; if (kp.pt.x < n1.UR.x) { @@ -552,18 +552,16 @@ vector ORBextractor::DistributeOctTree( vpIniNodes.resize(nIni); for (int i = 0; i < nIni; i++) { - ExtractorNode ni; - ni.UL = cv::Point2i(hX * static_cast(i), 0); - ni.UR = cv::Point2i(hX * static_cast(i + 1), 0); - ni.BL = cv::Point2i(ni.UL.x, maxY - minY); - ni.BR = cv::Point2i(ni.UR.x, maxY - minY); - ni.vKeys.reserve(vToDistributeKeys.size()); - - lNodes.push_back(ni); + auto UL = cv::Point2i(hX * static_cast(i), 0); + auto UR = cv::Point2i(hX * static_cast(i + 1), 0); + auto BL = cv::Point2i(UL.x, maxY - minY); + auto BR = cv::Point2i(UR.x, maxY - minY); + + lNodes.emplace_back(UL, UR, BL, BR, vToDistributeKeys.size()); vpIniNodes[i] = &lNodes.back(); } - // Associate points to childs + // Associate points to children for (size_t i = 0; i < vToDistributeKeys.size(); i++) { const cv::KeyPoint& kp = vToDistributeKeys[i]; vpIniNodes[kp.pt.x / hX]->vKeys.push_back(kp); @@ -609,13 +607,13 @@ vector ORBextractor::DistributeOctTree( ExtractorNode n1, n2, n3, n4; lit->DivideNode(n1, n2, n3, n4); - // Add childs if they contain points + // Add children if they contain points if (n1.vKeys.size() > 0) { lNodes.push_front(n1); if (n1.vKeys.size() > 1) { nToExpand++; - vSizeAndPointerToNode.push_back( - make_pair(n1.vKeys.size(), &lNodes.front())); + vSizeAndPointerToNode.emplace_back( + n1.vKeys.size(), &lNodes.front()); lNodes.front().lit = lNodes.begin(); } } @@ -623,8 +621,8 @@ vector ORBextractor::DistributeOctTree( lNodes.push_front(n2); if (n2.vKeys.size() > 1) { nToExpand++; - vSizeAndPointerToNode.push_back( - make_pair(n2.vKeys.size(), &lNodes.front())); + vSizeAndPointerToNode.emplace_back( + n2.vKeys.size(), &lNodes.front()); lNodes.front().lit = lNodes.begin(); } } @@ -632,8 +630,8 @@ vector ORBextractor::DistributeOctTree( lNodes.push_front(n3); if (n3.vKeys.size() > 1) { nToExpand++; - vSizeAndPointerToNode.push_back( - make_pair(n3.vKeys.size(), &lNodes.front())); + vSizeAndPointerToNode.emplace_back( + n3.vKeys.size(), &lNodes.front()); lNodes.front().lit = lNodes.begin(); } } @@ -641,8 +639,8 @@ vector ORBextractor::DistributeOctTree( lNodes.push_front(n4); if (n4.vKeys.size() > 1) { nToExpand++; - vSizeAndPointerToNode.push_back( - make_pair(n4.vKeys.size(), &lNodes.front())); + vSizeAndPointerToNode.emplace_back( + n4.vKeys.size(), &lNodes.front()); lNodes.front().lit = lNodes.begin(); } } @@ -670,36 +668,36 @@ vector ORBextractor::DistributeOctTree( ExtractorNode n1, n2, n3, n4; vPrevSizeAndPointerToNode[j].second->DivideNode(n1, n2, n3, n4); - // Add childs if they contain points + // Add children if they contain points if (n1.vKeys.size() > 0) { lNodes.push_front(n1); if (n1.vKeys.size() > 1) { - vSizeAndPointerToNode.push_back( - make_pair(n1.vKeys.size(), &lNodes.front())); + vSizeAndPointerToNode.emplace_back( + n1.vKeys.size(), &lNodes.front()); lNodes.front().lit = lNodes.begin(); } } if (n2.vKeys.size() > 0) { lNodes.push_front(n2); if (n2.vKeys.size() > 1) { - vSizeAndPointerToNode.push_back( - make_pair(n2.vKeys.size(), &lNodes.front())); + vSizeAndPointerToNode.emplace_back( + n2.vKeys.size(), &lNodes.front()); lNodes.front().lit = lNodes.begin(); } } if (n3.vKeys.size() > 0) { lNodes.push_front(n3); if (n3.vKeys.size() > 1) { - vSizeAndPointerToNode.push_back( - make_pair(n3.vKeys.size(), &lNodes.front())); + vSizeAndPointerToNode.emplace_back( + n3.vKeys.size(), &lNodes.front()); lNodes.front().lit = lNodes.begin(); } } if (n4.vKeys.size() > 0) { lNodes.push_front(n4); if (n4.vKeys.size() > 1) { - vSizeAndPointerToNode.push_back( - make_pair(n4.vKeys.size(), &lNodes.front())); + vSizeAndPointerToNode.emplace_back( + n4.vKeys.size(), &lNodes.front()); lNodes.front().lit = lNodes.begin(); } } @@ -843,157 +841,6 @@ void ORBextractor::ComputeKeyPointsOctTree( computeOrientation(mvImagePyramid[level], allKeypoints[level], umax); } -void ORBextractor::ComputeKeyPointsOld( - std::vector >& allKeypoints) { - allKeypoints.resize(nlevels); - - float imageRatio = (float)mvImagePyramid[0].cols / mvImagePyramid[0].rows; - - for (int level = 0; level < nlevels; ++level) { - const int nDesiredFeatures = mnFeaturesPerLevel[level]; - - const int levelCols = sqrt((float)nDesiredFeatures / (5 * imageRatio)); - const int levelRows = imageRatio * levelCols; - - const int minBorderX = EDGE_THRESHOLD; - const int minBorderY = minBorderX; - const int maxBorderX = mvImagePyramid[level].cols - EDGE_THRESHOLD; - const int maxBorderY = mvImagePyramid[level].rows - EDGE_THRESHOLD; - - const int W = maxBorderX - minBorderX; - const int H = maxBorderY - minBorderY; - const int cellW = ceil((float)W / levelCols); - const int cellH = ceil((float)H / levelRows); - - const int nCells = levelRows * levelCols; - const int nfeaturesCell = ceil((float)nDesiredFeatures / nCells); - - vector > > cellKeyPoints( - levelRows, vector >(levelCols)); - - vector > nToRetain(levelRows, vector(levelCols, 0)); - vector > nTotal(levelRows, vector(levelCols, 0)); - vector > bNoMore(levelRows, vector(levelCols, false)); - vector iniXCol(levelCols); - vector iniYRow(levelRows); - int nNoMore = 0; - int nToDistribute = 0; - - float hY = cellH + 6; - - for (int i = 0; i < levelRows; i++) { - const float iniY = minBorderY + i * cellH - 3; - iniYRow[i] = iniY; - - if (i == levelRows - 1) { - hY = maxBorderY + 3 - iniY; - if (hY <= 0) continue; - } - - float hX = cellW + 6; - - for (int j = 0; j < levelCols; j++) { - float iniX; - - if (i == 0) { - iniX = minBorderX + j * cellW - 3; - iniXCol[j] = iniX; - } else { - iniX = iniXCol[j]; - } - - if (j == levelCols - 1) { - hX = maxBorderX + 3 - iniX; - if (hX <= 0) continue; - } - - Mat cellImage = mvImagePyramid[level] - .rowRange(iniY, iniY + hY) - .colRange(iniX, iniX + hX); - - cellKeyPoints[i][j].reserve(nfeaturesCell * 5); - - FAST(cellImage, cellKeyPoints[i][j], iniThFAST, true); - - if (cellKeyPoints[i][j].size() <= 3) { - cellKeyPoints[i][j].clear(); - - FAST(cellImage, cellKeyPoints[i][j], minThFAST, true); - } - - const int nKeys = cellKeyPoints[i][j].size(); - nTotal[i][j] = nKeys; - - if (nKeys > nfeaturesCell) { - nToRetain[i][j] = nfeaturesCell; - bNoMore[i][j] = false; - } else { - nToRetain[i][j] = nKeys; - nToDistribute += nfeaturesCell - nKeys; - bNoMore[i][j] = true; - nNoMore++; - } - } - } - - // Retain by score - - while (nToDistribute > 0 && nNoMore < nCells) { - int nNewFeaturesCell = - nfeaturesCell + ceil((float)nToDistribute / (nCells - nNoMore)); - nToDistribute = 0; - - for (int i = 0; i < levelRows; i++) { - for (int j = 0; j < levelCols; j++) { - if (!bNoMore[i][j]) { - if (nTotal[i][j] > nNewFeaturesCell) { - nToRetain[i][j] = nNewFeaturesCell; - bNoMore[i][j] = false; - } else { - nToRetain[i][j] = nTotal[i][j]; - nToDistribute += nNewFeaturesCell - nTotal[i][j]; - bNoMore[i][j] = true; - nNoMore++; - } - } - } - } - } - - vector& keypoints = allKeypoints[level]; - keypoints.reserve(nDesiredFeatures * 2); - - const int scaledPatchSize = PATCH_SIZE * mvScaleFactor[level]; - - // Retain by score and transform coordinates - for (int i = 0; i < levelRows; i++) { - for (int j = 0; j < levelCols; j++) { - vector& keysCell = cellKeyPoints[i][j]; - KeyPointsFilter::retainBest(keysCell, nToRetain[i][j]); - if ((int)keysCell.size() > nToRetain[i][j]) - keysCell.resize(nToRetain[i][j]); - - for (size_t k = 0, kend = keysCell.size(); k < kend; k++) { - keysCell[k].pt.x += iniXCol[j]; - keysCell[k].pt.y += iniYRow[i]; - keysCell[k].octave = level; - keysCell[k].size = scaledPatchSize; - keypoints.push_back(keysCell[k]); - } - } - } - - if ((int)keypoints.size() > nDesiredFeatures) { - KeyPointsFilter::retainBest(keypoints, nDesiredFeatures); - keypoints.resize(nDesiredFeatures); - } - } - - // and compute orientations - for (int level = 0; level < nlevels; ++level) - computeOrientation(mvImagePyramid[level], allKeypoints[level], umax); -} - static void computeDescriptors(const Mat& image, vector& keypoints, Mat& descriptors, const vector& pattern) { descriptors = Mat::zeros((int)keypoints.size(), 32, CV_8UC1); @@ -1018,25 +865,25 @@ int ORBextractor::operator()(InputArray _image, InputArray _mask, vector > allKeypoints; ComputeKeyPointsOctTree(allKeypoints); - // ComputeKeyPointsOld(allKeypoints); - Mat descriptors; int nkeypoints = 0; for (int level = 0; level < nlevels; ++level) nkeypoints += (int)allKeypoints[level].size(); - if (nkeypoints == 0) + // _keypoints = vector(nkeypoints); + if (nkeypoints == 0){ _descriptors.release(); - else { - _descriptors.create(nkeypoints, 32, CV_8U); - descriptors = _descriptors.getMat(); + _keypoints.clear(); + return 0; } - - //_keypoints.clear(); - //_keypoints.reserve(nkeypoints); - _keypoints = vector(nkeypoints); - - int offset = 0; + + _descriptors.create(nkeypoints, 32, CV_8U); + Mat descriptors = _descriptors.getMat(); + + _keypoints.clear(); + _keypoints.reserve(nkeypoints); + + // int offset = 0; // Modified for speeding up stereo fisheye matching int monoIndex = 0, stereoIndex = nkeypoints - 1; for (int level = 0; level < nlevels; ++level) { @@ -1046,15 +893,15 @@ int ORBextractor::operator()(InputArray _image, InputArray _mask, if (nkeypointsLevel == 0) continue; // preprocess the resized image - Mat workingMat = mvImagePyramid[level].clone(); - GaussianBlur(workingMat, workingMat, Size(7, 7), 2, 2, BORDER_REFLECT_101); + Mat workingMat;// = mvImagePyramid[level].clone(); + GaussianBlur(mvImagePyramid[level], workingMat, Size(7, 7), 2, 2, BORDER_REFLECT_101); // Compute the descriptors // Mat desc = descriptors.rowRange(offset, offset + nkeypointsLevel); Mat desc = cv::Mat(nkeypointsLevel, 32, CV_8U); computeDescriptors(workingMat, keypoints, desc, pattern); - offset += nkeypointsLevel; + // offset += nkeypointsLevel; float scale = mvScaleFactor[level]; // getScale(level, firstLevel, scaleFactor); @@ -1069,11 +916,11 @@ int ORBextractor::operator()(InputArray _image, InputArray _mask, if (keypoint->pt.x >= vLappingArea[0] && keypoint->pt.x <= vLappingArea[1]) { - _keypoints.at(stereoIndex) = (*keypoint); + _keypoints.push_back(*keypoint); desc.row(i).copyTo(descriptors.row(stereoIndex)); stereoIndex--; } else { - _keypoints.at(monoIndex) = (*keypoint); + _keypoints.push_back(*keypoint); desc.row(i).copyTo(descriptors.row(monoIndex)); monoIndex++; } @@ -1092,7 +939,7 @@ void ORBextractor::ComputePyramid(cv::Mat image) { cvRound((float)image.rows * scale)); Size wholeSize(sz.width + EDGE_THRESHOLD * 2, sz.height + EDGE_THRESHOLD * 2); - Mat temp(wholeSize, image.type()), masktemp; + Mat temp(wholeSize, image.type()); mvImagePyramid[level] = temp(Rect(EDGE_THRESHOLD, EDGE_THRESHOLD, sz.width, sz.height)); @@ -1111,4 +958,4 @@ void ORBextractor::ComputePyramid(cv::Mat image) { } } -} // namespace ORB_SLAM3 +} // namespace MORB_SLAM diff --git a/src/ORBmatcher.cc b/src/ORBmatcher.cc index 12e0990c..e0c6b590 100644 --- a/src/ORBmatcher.cc +++ b/src/ORBmatcher.cc @@ -19,7 +19,7 @@ * ORB-SLAM3. If not, see . */ -#include "ORBmatcher.h" +#include "MORB_SLAM/ORBmatcher.h" #include #include @@ -30,7 +30,7 @@ using namespace std; -namespace ORB_SLAM3 { +namespace MORB_SLAM { const int ORBmatcher::TH_HIGH = 100; const int ORBmatcher::TH_LOW = 50; @@ -219,7 +219,7 @@ int ORBmatcher::SearchByBoW(KeyFrame *pKF, Frame &F, vector &vpMapPointMatches) { const vector vpMapPointsKF = pKF->GetMapPointMatches(); - vpMapPointMatches = vector(F.N, static_cast(NULL)); + vpMapPointMatches = vector(F.N, nullptr); const DBoW2::FeatureVector &vFeatVecKF = pKF->mFeatVec; @@ -385,7 +385,7 @@ int ORBmatcher::SearchByBoW(KeyFrame *pKF, Frame &F, for (int i = 0; i < HISTO_LENGTH; i++) { if (i == ind1 || i == ind2 || i == ind3) continue; for (size_t j = 0, jend = rotHist[i].size(); j < jend; j++) { - vpMapPointMatches[rotHist[i][j]] = static_cast(NULL); + vpMapPointMatches[rotHist[i][j]] = nullptr; nmatches--; } } @@ -410,7 +410,7 @@ int ORBmatcher::SearchByProjection(KeyFrame *pKF, Sophus::Sim3f &Scw, // Set of MapPoints already found in the KeyFrame set spAlreadyFound(vpMatched.begin(), vpMatched.end()); - spAlreadyFound.erase(static_cast(NULL)); + spAlreadyFound.erase(nullptr); int nmatches = 0; @@ -511,7 +511,7 @@ int ORBmatcher::SearchByProjection(KeyFrame *pKF, Sophus::Sim3 &Scw, // Set of MapPoints already found in the KeyFrame set spAlreadyFound(vpMatched.begin(), vpMatched.end()); - spAlreadyFound.erase(static_cast(NULL)); + spAlreadyFound.erase(nullptr); int nmatches = 0; @@ -712,7 +712,7 @@ int ORBmatcher::SearchByBoW(KeyFrame *pKF1, KeyFrame *pKF2, const cv::Mat &Descriptors2 = pKF2->mDescriptors; vpMatches12 = - vector(vpMapPoints1.size(), static_cast(NULL)); + vector(vpMapPoints1.size(), nullptr); vector vbMatched2(vpMapPoints2.size(), false); vector rotHist[HISTO_LENGTH]; @@ -809,7 +809,7 @@ int ORBmatcher::SearchByBoW(KeyFrame *pKF1, KeyFrame *pKF2, for (int i = 0; i < HISTO_LENGTH; i++) { if (i == ind1 || i == ind2 || i == ind3) continue; for (size_t j = 0, jend = rotHist[i].size(); j < jend; j++) { - vpMatches12[rotHist[i][j]] = static_cast(NULL); + vpMatches12[rotHist[i][j]] = nullptr; nmatches--; } } @@ -838,7 +838,7 @@ int ORBmatcher::SearchForTriangulation( Eigen::Matrix3f R12; // for fastest computation Eigen::Vector3f t12; // for fastest computation - GeometricCamera *pCamera1 = pKF1->mpCamera, *pCamera2 = pKF2->mpCamera; + std::shared_ptr pCamera1 = pKF1->mpCamera, pCamera2 = pKF2->mpCamera; if (!pKF1->mpCamera2 && !pKF2->mpCamera2) { T12 = T1w * Tw2; @@ -1043,7 +1043,7 @@ int ORBmatcher::SearchForTriangulation( int ORBmatcher::Fuse(KeyFrame *pKF, const vector &vpMapPoints, const float th, const bool bRight) { - GeometricCamera *pCamera; + std::shared_ptr pCamera; Sophus::SE3f Tcw; Eigen::Vector3f Ow; @@ -1722,7 +1722,7 @@ int ORBmatcher::SearchByProjection(Frame &CurrentFrame, const Frame &LastFrame, if (i != ind1 && i != ind2 && i != ind3) { for (size_t j = 0, jend = rotHist[i].size(); j < jend; j++) { CurrentFrame.mvpMapPoints[rotHist[i][j]] = - static_cast(NULL); + nullptr; nmatches--; } } @@ -1831,7 +1831,7 @@ int ORBmatcher::SearchByProjection(Frame &CurrentFrame, KeyFrame *pKF, for (int i = 0; i < HISTO_LENGTH; i++) { if (i != ind1 && i != ind2 && i != ind3) { for (size_t j = 0, jend = rotHist[i].size(); j < jend; j++) { - CurrentFrame.mvpMapPoints[rotHist[i][j]] = NULL; + CurrentFrame.mvpMapPoints[rotHist[i][j]] = nullptr; nmatches--; } } @@ -1893,4 +1893,4 @@ int ORBmatcher::DescriptorDistance(const cv::Mat &a, const cv::Mat &b) { return dist; } -} // namespace ORB_SLAM3 +} // namespace MORB_SLAM diff --git a/src/OptimizableTypes.cpp b/src/OptimizableTypes.cpp index 40160beb..23cb892e 100644 --- a/src/OptimizableTypes.cpp +++ b/src/OptimizableTypes.cpp @@ -19,9 +19,9 @@ * ORB-SLAM3. If not, see . */ -#include "OptimizableTypes.h" +#include "MORB_SLAM/OptimizableTypes.h" -namespace ORB_SLAM3 { +namespace MORB_SLAM { bool EdgeSE3ProjectXYZOnlyPose::read(std::istream& is) { for (int i = 0; i < 2; i++) { is >> _measurement[i]; @@ -310,4 +310,4 @@ bool EdgeInverseSim3ProjectXYZ::write(std::ostream& os) const { return os.good(); } -} // namespace ORB_SLAM3 +} // namespace MORB_SLAM diff --git a/src/Optimizer.cc b/src/Optimizer.cc index da4c8be0..608911ea 100644 --- a/src/Optimizer.cc +++ b/src/Optimizer.cc @@ -19,7 +19,7 @@ * ORB-SLAM3. If not, see . */ -#include "Optimizer.h" +#include "MORB_SLAM/Optimizer.h" #include #include @@ -27,9 +27,9 @@ #include #include -#include "Converter.h" -#include "G2oTypes.h" -#include "OptimizableTypes.h" +#include "MORB_SLAM/Converter.h" +#include "MORB_SLAM/G2oTypes.h" +#include "MORB_SLAM/OptimizableTypes.h" #include "g2o/core/block_solver.h" #include "g2o/core/optimization_algorithm_gauss_newton.h" #include "g2o/core/optimization_algorithm_levenberg.h" @@ -39,12 +39,12 @@ #include "g2o/solvers/linear_solver_eigen.h" #include "g2o/types/types_six_dof_expmap.h" -namespace ORB_SLAM3 { +namespace MORB_SLAM { bool sortByVal(const pair& a, const pair& b) { return (a.second < b.second); } -void Optimizer::GlobalBundleAdjustemnt(Map* pMap, int nIterations, +void Optimizer::GlobalBundleAdjustemnt(std::shared_ptr pMap, int nIterations, bool* pbStopFlag, const unsigned long nLoopKF, const bool bRobust) { @@ -60,7 +60,7 @@ void Optimizer::BundleAdjustment(const vector& vpKFs, vector vbNotIncludedMP; vbNotIncludedMP.resize(vpMP.size()); - Map* pMap = vpKFs[0]->GetMap(); + std::shared_ptr pMap = vpKFs[0]->GetMap(); g2o::SparseOptimizer optimizer; g2o::BlockSolver_6_3::LinearSolverType* linearSolver; @@ -81,10 +81,10 @@ void Optimizer::BundleAdjustment(const vector& vpKFs, const int nExpectedSize = (vpKFs.size()) * vpMP.size(); - vector vpEdgesMono; + vector vpEdgesMono; vpEdgesMono.reserve(nExpectedSize); - vector vpEdgesBody; + vector vpEdgesBody; vpEdgesBody.reserve(nExpectedSize); vector vpEdgeKFMono; @@ -146,7 +146,7 @@ void Optimizer::BundleAdjustment(const vector& vpKFs, mit != observations.end(); mit++) { KeyFrame* pKF = mit->first; if (pKF->isBad() || pKF->mnId > maxKFid) continue; - if (optimizer.vertex(id) == NULL || optimizer.vertex(pKF->mnId) == NULL) + if (optimizer.vertex(id) == nullptr || optimizer.vertex(pKF->mnId) == nullptr) continue; nEdges++; @@ -158,7 +158,7 @@ void Optimizer::BundleAdjustment(const vector& vpKFs, Eigen::Matrix obs; obs << kpUn.pt.x, kpUn.pt.y; - ORB_SLAM3::EdgeSE3ProjectXYZ* e = new ORB_SLAM3::EdgeSE3ProjectXYZ(); + MORB_SLAM::EdgeSE3ProjectXYZ* e = new MORB_SLAM::EdgeSE3ProjectXYZ(); e->setVertex(0, dynamic_cast( optimizer.vertex(id))); @@ -230,8 +230,8 @@ void Optimizer::BundleAdjustment(const vector& vpKFs, cv::KeyPoint kp = pKF->mvKeysRight[rightIndex]; obs << kp.pt.x, kp.pt.y; - ORB_SLAM3::EdgeSE3ProjectXYZToBody* e = - new ORB_SLAM3::EdgeSE3ProjectXYZToBody(); + MORB_SLAM::EdgeSE3ProjectXYZToBody* e = + new MORB_SLAM::EdgeSE3ProjectXYZToBody(); e->setVertex(0, dynamic_cast( optimizer.vertex(id))); @@ -300,7 +300,7 @@ void Optimizer::BundleAdjustment(const vector& vpKFs, vector vpMonoMPsOpt, vpStereoMPsOpt; for (size_t i2 = 0, iend = vpEdgesMono.size(); i2 < iend; i2++) { - ORB_SLAM3::EdgeSE3ProjectXYZ* e = vpEdgesMono[i2]; + MORB_SLAM::EdgeSE3ProjectXYZ* e = vpEdgesMono[i2]; MapPoint* pMP = vpMapPointEdgeMono[i2]; KeyFrame* pKFedge = vpEdgeKFMono[i2]; @@ -361,7 +361,7 @@ void Optimizer::BundleAdjustment(const vector& vpKFs, } } -void Optimizer::FullInertialBA(Map* pMap, int its, const bool bFixLocal, +void Optimizer::FullInertialBA(std::shared_ptr pMap, int its, const bool bFixLocal, const long unsigned int nLoopId, bool* pbStopFlag, bool bInit, float priorG, float priorA, Eigen::VectorXd* vSingVal, @@ -786,8 +786,8 @@ int Optimizer::PoseOptimization(Frame* pFrame) { // Set MapPoint vertices const int N = pFrame->N; - vector vpEdgesMono; - vector vpEdgesMono_FHR; + vector vpEdgesMono; + vector vpEdgesMono_FHR; vector vnIndexEdgeMono, vnIndexEdgeRight; vpEdgesMono.reserve(N); vpEdgesMono_FHR.reserve(N); @@ -819,8 +819,8 @@ int Optimizer::PoseOptimization(Frame* pFrame) { const cv::KeyPoint& kpUn = pFrame->mvKeysUn[i]; obs << kpUn.pt.x, kpUn.pt.y; - ORB_SLAM3::EdgeSE3ProjectXYZOnlyPose* e = - new ORB_SLAM3::EdgeSE3ProjectXYZOnlyPose(); + MORB_SLAM::EdgeSE3ProjectXYZOnlyPose* e = + new MORB_SLAM::EdgeSE3ProjectXYZOnlyPose(); e->setVertex(0, dynamic_cast( optimizer.vertex(0))); @@ -890,8 +890,8 @@ int Optimizer::PoseOptimization(Frame* pFrame) { Eigen::Matrix obs; obs << kpUn.pt.x, kpUn.pt.y; - ORB_SLAM3::EdgeSE3ProjectXYZOnlyPose* e = - new ORB_SLAM3::EdgeSE3ProjectXYZOnlyPose(); + MORB_SLAM::EdgeSE3ProjectXYZOnlyPose* e = + new MORB_SLAM::EdgeSE3ProjectXYZOnlyPose(); e->setVertex(0, dynamic_cast( optimizer.vertex(0))); @@ -918,8 +918,8 @@ int Optimizer::PoseOptimization(Frame* pFrame) { pFrame->mvbOutlier[i] = false; - ORB_SLAM3::EdgeSE3ProjectXYZOnlyPoseToBody* e = - new ORB_SLAM3::EdgeSE3ProjectXYZOnlyPoseToBody(); + MORB_SLAM::EdgeSE3ProjectXYZOnlyPoseToBody* e = + new MORB_SLAM::EdgeSE3ProjectXYZOnlyPoseToBody(); e->setVertex(0, dynamic_cast( optimizer.vertex(0))); @@ -968,7 +968,7 @@ int Optimizer::PoseOptimization(Frame* pFrame) { nBad = 0; for (size_t i = 0, iend = vpEdgesMono.size(); i < iend; i++) { - ORB_SLAM3::EdgeSE3ProjectXYZOnlyPose* e = vpEdgesMono[i]; + MORB_SLAM::EdgeSE3ProjectXYZOnlyPose* e = vpEdgesMono[i]; const size_t idx = vnIndexEdgeMono[i]; @@ -991,7 +991,7 @@ int Optimizer::PoseOptimization(Frame* pFrame) { } for (size_t i = 0, iend = vpEdgesMono_FHR.size(); i < iend; i++) { - ORB_SLAM3::EdgeSE3ProjectXYZOnlyPoseToBody* e = vpEdgesMono_FHR[i]; + MORB_SLAM::EdgeSE3ProjectXYZOnlyPoseToBody* e = vpEdgesMono_FHR[i]; const size_t idx = vnIndexEdgeRight[i]; @@ -1051,7 +1051,7 @@ int Optimizer::PoseOptimization(Frame* pFrame) { } void Optimizer::LocalBundleAdjustment(KeyFrame* pKF, bool* pbStopFlag, - Map* pMap, int& num_fixedKF, + std::shared_ptr pMap, int& num_fixedKF, int& num_OptKF, int& num_MPs, int& num_edges) { // Local KeyFrames: First Breath Search from Current Keyframe @@ -1059,7 +1059,7 @@ void Optimizer::LocalBundleAdjustment(KeyFrame* pKF, bool* pbStopFlag, lLocalKeyFrames.push_back(pKF); pKF->mnBALocalForKF = pKF->mnId; - Map* pCurrentMap = pKF->GetMap(); + std::shared_ptr pCurrentMap = pKF->GetMap(); const vector vNeighKFs = pKF->GetVectorCovisibleKeyFrames(); for (int i = 0, iend = vNeighKFs.size(); i < iend; i++) { @@ -1186,10 +1186,10 @@ void Optimizer::LocalBundleAdjustment(KeyFrame* pKF, bool* pbStopFlag, const int nExpectedSize = (lLocalKeyFrames.size() + lFixedCameras.size()) * lLocalMapPoints.size(); - vector vpEdgesMono; + vector vpEdgesMono; vpEdgesMono.reserve(nExpectedSize); - vector vpEdgesBody; + vector vpEdgesBody; vpEdgesBody.reserve(nExpectedSize); vector vpEdgeKFMono; @@ -1250,7 +1250,7 @@ void Optimizer::LocalBundleAdjustment(KeyFrame* pKF, bool* pbStopFlag, Eigen::Matrix obs; obs << kpUn.pt.x, kpUn.pt.y; - ORB_SLAM3::EdgeSE3ProjectXYZ* e = new ORB_SLAM3::EdgeSE3ProjectXYZ(); + MORB_SLAM::EdgeSE3ProjectXYZ* e = new MORB_SLAM::EdgeSE3ProjectXYZ(); e->setVertex(0, dynamic_cast( optimizer.vertex(id))); @@ -1319,8 +1319,8 @@ void Optimizer::LocalBundleAdjustment(KeyFrame* pKF, bool* pbStopFlag, cv::KeyPoint kp = pKFi->mvKeysRight[rightIndex]; obs << kp.pt.x, kp.pt.y; - ORB_SLAM3::EdgeSE3ProjectXYZToBody* e = - new ORB_SLAM3::EdgeSE3ProjectXYZToBody(); + MORB_SLAM::EdgeSE3ProjectXYZToBody* e = + new MORB_SLAM::EdgeSE3ProjectXYZToBody(); e->setVertex(0, dynamic_cast( optimizer.vertex(id))); @@ -1365,7 +1365,7 @@ void Optimizer::LocalBundleAdjustment(KeyFrame* pKF, bool* pbStopFlag, // Check inlier observations for (size_t i = 0, iend = vpEdgesMono.size(); i < iend; i++) { - ORB_SLAM3::EdgeSE3ProjectXYZ* e = vpEdgesMono[i]; + MORB_SLAM::EdgeSE3ProjectXYZ* e = vpEdgesMono[i]; MapPoint* pMP = vpMapPointEdgeMono[i]; if (pMP->isBad()) continue; @@ -1377,7 +1377,7 @@ void Optimizer::LocalBundleAdjustment(KeyFrame* pKF, bool* pbStopFlag, } for (size_t i = 0, iend = vpEdgesBody.size(); i < iend; i++) { - ORB_SLAM3::EdgeSE3ProjectXYZToBody* e = vpEdgesBody[i]; + MORB_SLAM::EdgeSE3ProjectXYZToBody* e = vpEdgesBody[i]; MapPoint* pMP = vpMapPointEdgeBody[i]; if (pMP->isBad()) continue; @@ -1441,7 +1441,7 @@ void Optimizer::LocalBundleAdjustment(KeyFrame* pKF, bool* pbStopFlag, } void Optimizer::OptimizeEssentialGraph( - Map* pMap, KeyFrame* pLoopKF, KeyFrame* pCurKF, + std::shared_ptr pMap, KeyFrame* pLoopKF, KeyFrame* pCurKF, const LoopClosing::KeyFrameAndPose& NonCorrectedSim3, const LoopClosing::KeyFrameAndPose& CorrectedSim3, const map>& LoopConnections, @@ -1767,7 +1767,7 @@ void Optimizer::OptimizeEssentialGraph(KeyFrame* pCurKF, solver->setUserLambdaInit(1e-16); optimizer.setAlgorithm(solver); - Map* pMap = pCurKF->GetMap(); + std::shared_ptr pMap = pCurKF->GetMap(); const unsigned int nMaxKFid = pMap->GetMaxKFid(); vector> vScw(nMaxKFid + 1); @@ -2086,7 +2086,7 @@ int Optimizer::OptimizeSim3(KeyFrame* pKF1, KeyFrame* pKF2, const Eigen::Vector3f t2w = pKF2->GetTranslation(); // Set Sim3 vertex - ORB_SLAM3::VertexSim3Expmap* vSim3 = new ORB_SLAM3::VertexSim3Expmap(); + MORB_SLAM::VertexSim3Expmap* vSim3 = new MORB_SLAM::VertexSim3Expmap(); vSim3->_fix_scale = bFixScale; vSim3->setEstimate(g2oS12); vSim3->setId(0); @@ -2098,8 +2098,8 @@ int Optimizer::OptimizeSim3(KeyFrame* pKF1, KeyFrame* pKF2, // Set MapPoint vertices const int N = vpMatches1.size(); const vector vpMapPoints1 = pKF1->GetMapPointMatches(); - vector vpEdges12; - vector vpEdges21; + vector vpEdges12; + vector vpEdges21; vector vnIndexEdge; vector vbIsInKF2; @@ -2191,7 +2191,7 @@ int Optimizer::OptimizeSim3(KeyFrame* pKF1, KeyFrame* pKF2, const cv::KeyPoint& kpUn1 = pKF1->mvKeysUn[i]; obs1 << kpUn1.pt.x, kpUn1.pt.y; - ORB_SLAM3::EdgeSim3ProjectXYZ* e12 = new ORB_SLAM3::EdgeSim3ProjectXYZ(); + MORB_SLAM::EdgeSim3ProjectXYZ* e12 = new MORB_SLAM::EdgeSim3ProjectXYZ(); e12->setVertex( 0, dynamic_cast(optimizer.vertex(id2))); @@ -2228,8 +2228,8 @@ int Optimizer::OptimizeSim3(KeyFrame* pKF1, KeyFrame* pKF2, nOutKF2++; } - ORB_SLAM3::EdgeInverseSim3ProjectXYZ* e21 = - new ORB_SLAM3::EdgeInverseSim3ProjectXYZ(); + MORB_SLAM::EdgeInverseSim3ProjectXYZ* e21 = + new MORB_SLAM::EdgeInverseSim3ProjectXYZ(); e21->setVertex( 0, dynamic_cast(optimizer.vertex(id1))); @@ -2259,17 +2259,17 @@ int Optimizer::OptimizeSim3(KeyFrame* pKF1, KeyFrame* pKF2, int nBad = 0; int nBadOutKF2 = 0; for (size_t i = 0; i < vpEdges12.size(); i++) { - ORB_SLAM3::EdgeSim3ProjectXYZ* e12 = vpEdges12[i]; - ORB_SLAM3::EdgeInverseSim3ProjectXYZ* e21 = vpEdges21[i]; + MORB_SLAM::EdgeSim3ProjectXYZ* e12 = vpEdges12[i]; + MORB_SLAM::EdgeInverseSim3ProjectXYZ* e21 = vpEdges21[i]; if (!e12 || !e21) continue; if (e12->chi2() > th2 || e21->chi2() > th2) { size_t idx = vnIndexEdge[i]; - vpMatches1[idx] = static_cast(NULL); + vpMatches1[idx] = nullptr; optimizer.removeEdge(e12); optimizer.removeEdge(e21); - vpEdges12[i] = static_cast(NULL); - vpEdges21[i] = static_cast(NULL); + vpEdges12[i] = static_cast(nullptr); + vpEdges21[i] = static_cast(nullptr); nBad++; if (!vbIsInKF2[i]) { @@ -2298,8 +2298,8 @@ int Optimizer::OptimizeSim3(KeyFrame* pKF1, KeyFrame* pKF2, int nIn = 0; mAcumHessian = Eigen::MatrixXd::Zero(7, 7); for (size_t i = 0; i < vpEdges12.size(); i++) { - ORB_SLAM3::EdgeSim3ProjectXYZ* e12 = vpEdges12[i]; - ORB_SLAM3::EdgeInverseSim3ProjectXYZ* e21 = vpEdges21[i]; + MORB_SLAM::EdgeSim3ProjectXYZ* e12 = vpEdges12[i]; + MORB_SLAM::EdgeInverseSim3ProjectXYZ* e21 = vpEdges21[i]; if (!e12 || !e21) continue; e12->computeError(); @@ -2307,7 +2307,7 @@ int Optimizer::OptimizeSim3(KeyFrame* pKF1, KeyFrame* pKF2, if (e12->chi2() > th2 || e21->chi2() > th2) { size_t idx = vnIndexEdge[i]; - vpMatches1[idx] = static_cast(NULL); + vpMatches1[idx] = nullptr; } else { nIn++; } @@ -2321,10 +2321,10 @@ int Optimizer::OptimizeSim3(KeyFrame* pKF1, KeyFrame* pKF2, return nIn; } -void Optimizer::LocalInertialBA(KeyFrame* pKF, bool* pbStopFlag, Map* pMap, +void Optimizer::LocalInertialBA(KeyFrame* pKF, bool* pbStopFlag, std::shared_ptr pMap, int& num_fixedKF, int& num_OptKF, int& num_MPs, int& num_edges, bool bLarge, bool bRecInit) { - Map* pCurrentMap = pKF->GetMap(); + std::shared_ptr pCurrentMap = pKF->GetMap(); int maxOpt = 10; int opt_it = 10; @@ -2519,9 +2519,9 @@ void Optimizer::LocalInertialBA(KeyFrame* pKF, bool* pbStopFlag, Map* pMap, } // Create intertial constraints - vector vei(N, (EdgeInertial*)NULL); - vector vegr(N, (EdgeGyroRW*)NULL); - vector vear(N, (EdgeAccRW*)NULL); + vector vei(N, (EdgeInertial*)nullptr); + vector vegr(N, (EdgeGyroRW*)nullptr); + vector vear(N, (EdgeAccRW*)nullptr); for (int i = 0; i < N; i++) { KeyFrame* pKFi = vpOptimizableKFs[i]; @@ -2976,7 +2976,7 @@ Eigen::MatrixXd Optimizer::Marginalize(const Eigen::MatrixXd& H, return res; } -void Optimizer::InertialOptimization(Map* pMap, Eigen::Matrix3d& Rwg, +void Optimizer::InertialOptimization(std::shared_ptr pMap, Eigen::Matrix3d& Rwg, double& scale, Eigen::Vector3d& bg, Eigen::Vector3d& ba, bool bMono, Eigen::MatrixXd& covInertial, @@ -3155,7 +3155,7 @@ void Optimizer::InertialOptimization(Map* pMap, Eigen::Matrix3d& Rwg, } } -void Optimizer::InertialOptimization(Map* pMap, Eigen::Vector3d& bg, +void Optimizer::InertialOptimization(std::shared_ptr pMap, Eigen::Vector3d& bg, Eigen::Vector3d& ba, float priorG, float priorA) { int its = 200; // Check number of iterations @@ -3313,7 +3313,7 @@ void Optimizer::InertialOptimization(Map* pMap, Eigen::Vector3d& bg, } } -void Optimizer::InertialOptimization(Map* pMap, Eigen::Matrix3d& Rwg, +void Optimizer::InertialOptimization(std::shared_ptr pMap, Eigen::Matrix3d& Rwg, double& scale) { int its = 10; long unsigned int maxKFid = pMap->GetMaxKFid(); @@ -3454,7 +3454,7 @@ void Optimizer::LocalBundleAdjustment(KeyFrame* pMainKF, long unsigned int maxKFid = 0; set spKeyFrameBA; - Map* pCurrentMap = pMainKF->GetMap(); + std::shared_ptr pCurrentMap = pMainKF->GetMap(); // Set fixed KeyFrame vertices int numInsertedPoints = 0; @@ -3526,7 +3526,7 @@ void Optimizer::LocalBundleAdjustment(KeyFrame* pMainKF, const int nExpectedSize = (vpAdjustKF.size() + vpFixedKF.size()) * vpMPs.size(); - vector vpEdgesMono; + vector vpEdgesMono; vpEdgesMono.reserve(nExpectedSize); vector vpEdgeKFMono; @@ -3585,7 +3585,7 @@ void Optimizer::LocalBundleAdjustment(KeyFrame* pMainKF, Eigen::Matrix obs; obs << kpUn.pt.x, kpUn.pt.y; - ORB_SLAM3::EdgeSE3ProjectXYZ* e = new ORB_SLAM3::EdgeSE3ProjectXYZ(); + MORB_SLAM::EdgeSE3ProjectXYZ* e = new MORB_SLAM::EdgeSE3ProjectXYZ(); e->setVertex(0, dynamic_cast( optimizer.vertex(id))); @@ -3663,7 +3663,7 @@ void Optimizer::LocalBundleAdjustment(KeyFrame* pMainKF, // Check inlier observations int badMonoMP = 0, badStereoMP = 0; for (size_t i = 0, iend = vpEdgesMono.size(); i < iend; i++) { - ORB_SLAM3::EdgeSE3ProjectXYZ* e = vpEdgesMono[i]; + MORB_SLAM::EdgeSE3ProjectXYZ* e = vpEdgesMono[i]; MapPoint* pMP = vpMapPointEdgeMono[i]; if (pMP->isBad()) continue; @@ -3705,7 +3705,7 @@ void Optimizer::LocalBundleAdjustment(KeyFrame* pMainKF, // Check inlier observations int badMonoMP = 0, badStereoMP = 0; for (size_t i = 0, iend = vpEdgesMono.size(); i < iend; i++) { - ORB_SLAM3::EdgeSE3ProjectXYZ* e = vpEdgesMono[i]; + MORB_SLAM::EdgeSE3ProjectXYZ* e = vpEdgesMono[i]; MapPoint* pMP = vpMapPointEdgeMono[i]; if (pMP->isBad()) continue; @@ -3796,7 +3796,7 @@ void Optimizer::LocalBundleAdjustment(KeyFrame* pMainKF, vector vpMonoMPsBad, vpStereoMPsBad; for (size_t i = 0, iend = vpEdgesMono.size(); i < iend; i++) { - ORB_SLAM3::EdgeSE3ProjectXYZ* e = vpEdgesMono[i]; + MORB_SLAM::EdgeSE3ProjectXYZ* e = vpEdgesMono[i]; MapPoint* pMP = vpMapPointEdgeMono[i]; KeyFrame* pKFedge = vpEdgeKFMono[i]; @@ -3851,7 +3851,7 @@ void Optimizer::LocalBundleAdjustment(KeyFrame* pMainKF, } void Optimizer::MergeInertialBA(KeyFrame* pCurrKF, KeyFrame* pMergeKF, - bool* pbStopFlag, Map* pMap, + bool* pbStopFlag, std::shared_ptr pMap, LoopClosing::KeyFrameAndPose& corrPoses) { const int Nd = 6; @@ -4076,9 +4076,9 @@ void Optimizer::MergeInertialBA(KeyFrame* pCurrKF, KeyFrame* pMergeKF, } // Create intertial constraints - vector vei(N, (EdgeInertial*)NULL); - vector vegr(N, (EdgeGyroRW*)NULL); - vector vear(N, (EdgeAccRW*)NULL); + vector vei(N, (EdgeInertial*)nullptr); + vector vegr(N, (EdgeGyroRW*)nullptr); + vector vear(N, (EdgeAccRW*)nullptr); for (int i = 0; i < N; i++) { // cout << "inserting inertial edge " << i << endl; KeyFrame* pKFi = vpOptimizableKFs[i]; @@ -4215,7 +4215,7 @@ void Optimizer::MergeInertialBA(KeyFrame* pCurrKF, KeyFrame* pMergeKF, continue; } - if (optimizer.vertex(id) == NULL || optimizer.vertex(pKFi->mnId) == NULL) + if (optimizer.vertex(id) == nullptr || optimizer.vertex(pKFi->mnId) == nullptr) continue; if (!pKFi->isBad()) { @@ -4750,13 +4750,13 @@ int Optimizer::PoseInertialOptimizationLastKeyFrame(Frame* pFrame, } pFrame->mpcpi = - new ConstraintPoseImu(VP->estimate().Rwb, VP->estimate().twb, + std::make_shared(VP->estimate().Rwb, VP->estimate().twb, VV->estimate(), VG->estimate(), VA->estimate(), H); return nInitialCorrespondences - nBad; } -static ConstraintPoseImu* oldMpcpi = nullptr; +static std::shared_ptr oldMpcpi = nullptr; int Optimizer::PoseInertialOptimizationLastFrame(Frame* pFrame, bool bRecInit) { g2o::SparseOptimizer optimizer; @@ -5147,21 +5147,20 @@ int Optimizer::PoseInertialOptimizationLastFrame(Frame* pFrame, bool bRecInit) { H = Marginalize(H, 0, 14); - pFrame->mpcpi = new ConstraintPoseImu( + pFrame->mpcpi = std::make_shared( VP->estimate().Rwb, VP->estimate().twb, VV->estimate(), VG->estimate(), VA->estimate(), H.block<15, 15>(15, 15)); if (oldMpcpi == pFp->mpcpi) { std::cerr << "\033[22;34mSAME MPCPI" << std::endl; } else { oldMpcpi = pFp->mpcpi; - delete pFp->mpcpi; - pFp->mpcpi = NULL; + pFp->mpcpi = nullptr; } return nInitialCorrespondences - nBad; } void Optimizer::OptimizeEssentialGraph4DoF( - Map* pMap, KeyFrame* pLoopKF, KeyFrame* pCurKF, + std::shared_ptr pMap, KeyFrame* pLoopKF, KeyFrame* pCurKF, const LoopClosing::KeyFrameAndPose& NonCorrectedSim3, const LoopClosing::KeyFrameAndPose& CorrectedSim3, const map>& LoopConnections) { @@ -5291,7 +5290,7 @@ void Optimizer::OptimizeEssentialGraph4DoF( Siw = vScw[nIDi]; // 1.1.0 Spanning tree edge - KeyFrame* pParentKF = static_cast(NULL); + KeyFrame* pParentKF = nullptr; if (pParentKF) { int nIDj = pParentKF->mnId; @@ -5471,4 +5470,4 @@ void Optimizer::OptimizeEssentialGraph4DoF( pMap->IncreaseChangeIndex(); } -} // namespace ORB_SLAM3 +} // namespace MORB_SLAM diff --git a/src/Settings.cc b/src/Settings.cc index abf93b15..bb7349c9 100644 --- a/src/Settings.cc +++ b/src/Settings.cc @@ -19,18 +19,18 @@ * ORB-SLAM3. If not, see . */ -#include "Settings.h" +#include "MORB_SLAM/Settings.h" -#include "ImprovedTypes.hpp" +#include "MORB_SLAM/ImprovedTypes.hpp" #include #include #include #include -#include "CameraModels/KannalaBrandt8.h" -#include "CameraModels/Pinhole.h" +#include "MORB_SLAM/CameraModels/KannalaBrandt8.h" +#include "MORB_SLAM/CameraModels/Pinhole.h" -namespace ORB_SLAM3 { +namespace MORB_SLAM { template <> float Settings::readParameter(cv::FileStorage& fSettings, @@ -152,7 +152,7 @@ Settings::Settings(const std::string& configFile, const int& sensor) std::cout << "\t-Loaded camera 1" << std::endl; // Read second camera if stereo (not rectified) - if (sensor_ == ORB_SLAM3::CameraType::STEREO || sensor_ == ORB_SLAM3::CameraType::IMU_STEREO) { + if (sensor_ == MORB_SLAM::CameraType::STEREO || sensor_ == MORB_SLAM::CameraType::IMU_STEREO) { readCamera2(fSettings); std::cout << "\t-Loaded camera 2" << std::endl; } @@ -161,13 +161,13 @@ Settings::Settings(const std::string& configFile, const int& sensor) readImageInfo(fSettings); std::cout << "\t-Loaded image info" << std::endl; - if (sensor_ == ORB_SLAM3::CameraType::IMU_MONOCULAR || sensor_ == ORB_SLAM3::CameraType::IMU_STEREO || - sensor_ == ORB_SLAM3::CameraType::IMU_RGBD) { + if (sensor_ == MORB_SLAM::CameraType::IMU_MONOCULAR || sensor_ == MORB_SLAM::CameraType::IMU_STEREO || + sensor_ == MORB_SLAM::CameraType::IMU_RGBD) { readIMU(fSettings); std::cout << "\t-Loaded IMU calibration" << std::endl; } - if (sensor_ == ORB_SLAM3::CameraType::RGBD || sensor_ == ORB_SLAM3::CameraType::IMU_RGBD) { + if (sensor_ == MORB_SLAM::CameraType::RGBD || sensor_ == MORB_SLAM::CameraType::IMU_RGBD) { readRGBD(fSettings); std::cout << "\t-Loaded RGB-D calibration" << std::endl; } @@ -207,7 +207,7 @@ void Settings::readCamera1(cv::FileStorage& fSettings) { vCalibration = {fx, fy, cx, cy}; - calibration1_ = new Pinhole(vCalibration); + calibration1_ = std::make_shared(vCalibration); originalCalib1_ = new Pinhole(vCalibration); // Check if it is a distorted PinHole @@ -232,7 +232,7 @@ void Settings::readCamera1(cv::FileStorage& fSettings) { } // Check if we need to correct distortion from the images - if ((sensor_ == ORB_SLAM3::CameraType::MONOCULAR || sensor_ == ORB_SLAM3::CameraType::IMU_MONOCULAR) && + if ((sensor_ == MORB_SLAM::CameraType::MONOCULAR || sensor_ == MORB_SLAM::CameraType::IMU_MONOCULAR) && vPinHoleDistorsion1_.size() != 0) { bNeedToUndistort_ = true; } @@ -247,7 +247,7 @@ void Settings::readCamera1(cv::FileStorage& fSettings) { vCalibration = {fx, fy, cx, cy}; - calibration1_ = new Pinhole(vCalibration); + calibration1_ = std::make_shared(vCalibration); originalCalib1_ = new Pinhole(vCalibration); // Rectified images are assumed to be ideal PinHole images (no distortion) @@ -267,17 +267,17 @@ void Settings::readCamera1(cv::FileStorage& fSettings) { vCalibration = {fx, fy, cx, cy, k0, k1, k2, k3}; - calibration1_ = new KannalaBrandt8(vCalibration); + calibration1_ = std::make_shared(vCalibration); originalCalib1_ = new KannalaBrandt8(vCalibration); - if (sensor_ == ORB_SLAM3::CameraType::STEREO || sensor_ == ORB_SLAM3::CameraType::IMU_STEREO) { + if (sensor_ == MORB_SLAM::CameraType::STEREO || sensor_ == MORB_SLAM::CameraType::IMU_STEREO) { int colBegin = readParameter(fSettings, "Camera1.overlappingBegin", found); int colEnd = readParameter(fSettings, "Camera1.overlappingEnd", found); std::vector vOverlapping = {colBegin, colEnd}; - static_cast(calibration1_)->mvLappingArea = vOverlapping; + std::reinterpret_pointer_cast(calibration1_)->mvLappingArea = vOverlapping; } } else { std::cerr << "Error: " << cameraModel << " not known" << std::endl; @@ -299,7 +299,7 @@ void Settings::readCamera2(cv::FileStorage& fSettings) { vCalibration = {fx, fy, cx, cy}; - calibration2_ = new Pinhole(vCalibration); + calibration2_ = std::make_shared(vCalibration); originalCalib2_ = new Pinhole(vCalibration); // Check if it is a distorted PinHole @@ -336,7 +336,7 @@ void Settings::readCamera2(cv::FileStorage& fSettings) { vCalibration = {fx, fy, cx, cy, k0, k1, k2, k3}; - calibration2_ = new KannalaBrandt8(vCalibration); + calibration2_ = std::make_shared(vCalibration); originalCalib2_ = new KannalaBrandt8(vCalibration); int colBegin = @@ -344,7 +344,7 @@ void Settings::readCamera2(cv::FileStorage& fSettings) { int colEnd = readParameter(fSettings, "Camera2.overlappingEnd", found); std::vector vOverlapping = {colBegin, colEnd}; - static_cast(calibration2_)->mvLappingArea = vOverlapping; + std::reinterpret_pointer_cast(calibration2_)->mvLappingArea = vOverlapping; } // Load stereo extrinsic calibration @@ -388,7 +388,7 @@ void Settings::readImageInfo(cv::FileStorage& fSettings) { calibration1_->setParameter( calibration1_->getParameter(3) * scaleRowFactor, 3); - if ((sensor_ == ORB_SLAM3::CameraType::STEREO || sensor_ == ORB_SLAM3::CameraType::IMU_STEREO) && + if ((sensor_ == MORB_SLAM::CameraType::STEREO || sensor_ == MORB_SLAM::CameraType::IMU_STEREO) && cameraType_ != Rectified) { calibration2_->setParameter( calibration2_->getParameter(1) * scaleRowFactor, 1); @@ -412,7 +412,7 @@ void Settings::readImageInfo(cv::FileStorage& fSettings) { calibration1_->setParameter( calibration1_->getParameter(2) * scaleColFactor, 2); - if ((sensor_ == ORB_SLAM3::CameraType::STEREO || sensor_ == ORB_SLAM3::CameraType::IMU_STEREO) && + if ((sensor_ == MORB_SLAM::CameraType::STEREO || sensor_ == MORB_SLAM::CameraType::IMU_STEREO) && cameraType_ != Rectified) { calibration2_->setParameter( calibration2_->getParameter(0) * scaleColFactor, 0); @@ -420,14 +420,14 @@ void Settings::readImageInfo(cv::FileStorage& fSettings) { calibration2_->getParameter(2) * scaleColFactor, 2); if (cameraType_ == KannalaBrandt) { - static_cast(calibration1_)->mvLappingArea[0] *= + std::reinterpret_pointer_cast(calibration1_)->mvLappingArea[0] *= scaleColFactor; - static_cast(calibration1_)->mvLappingArea[1] *= + std::reinterpret_pointer_cast(calibration1_)->mvLappingArea[1] *= scaleColFactor; - static_cast(calibration2_)->mvLappingArea[0] *= + std::reinterpret_pointer_cast(calibration2_)->mvLappingArea[0] *= scaleColFactor; - static_cast(calibration2_)->mvLappingArea[1] *= + std::reinterpret_pointer_cast(calibration2_)->mvLappingArea[1] *= scaleColFactor; } } @@ -519,9 +519,9 @@ void Settings::readOtherParameters(cv::FileStorage& fSettings) { void Settings::precomputeRectificationMaps() { // Precompute rectification maps, new calibrations, ... - cv::Mat K1 = static_cast(calibration1_)->toK(); + cv::Mat K1 = std::reinterpret_pointer_cast(calibration1_)->toK(); K1.convertTo(K1, CV_64F); - cv::Mat K2 = static_cast(calibration2_)->toK(); + cv::Mat K2 = std::reinterpret_pointer_cast(calibration2_)->toK(); K2.convertTo(K2, CV_64F); cv::Mat cvTlr; @@ -554,7 +554,7 @@ void Settings::precomputeRectificationMaps() { bf_ = b_ * P1.at(0, 0); // Update relative pose between camera 1 and IMU if necessary - if (sensor_ == ORB_SLAM3::CameraType::IMU_STEREO) { + if (sensor_ == MORB_SLAM::CameraType::IMU_STEREO) { Eigen::Matrix3f eigenR_r1_u1; cv::cv2eigen(R_r1_u1, eigenR_r1_u1); Sophus::SE3f T_r1_u1(eigenR_r1_u1, Eigen::Vector3f::Zero()); @@ -587,8 +587,8 @@ std::ostream& operator<<(std::ostream& output, const Settings& settings) { output << " ]" << std::endl; } std::cout << "bout to start camera 2 stuff\n"; - if (settings.sensor_ == ORB_SLAM3::CameraType::STEREO || - settings.sensor_ == ORB_SLAM3::CameraType::IMU_STEREO) { + if (settings.sensor_ == MORB_SLAM::CameraType::STEREO || + settings.sensor_ == MORB_SLAM::CameraType::IMU_STEREO) { output << "\t-Camera 2 parameters ("; if (settings.cameraType_ == Settings::PinHole || settings.cameraType_ == Settings::Rectified) { @@ -630,8 +630,8 @@ std::ostream& operator<<(std::ostream& output, const Settings& settings) { } output << " ]" << std::endl; - if ((settings.sensor_ == ORB_SLAM3::CameraType::STEREO || - settings.sensor_ == ORB_SLAM3::CameraType::IMU_STEREO) && + if ((settings.sensor_ == MORB_SLAM::CameraType::STEREO || + settings.sensor_ == MORB_SLAM::CameraType::IMU_STEREO) && settings.cameraType_ == Settings::KannalaBrandt) { output << "\t-Camera 2 parameters after resize: [ "; for (size_t i = 0; i < settings.calibration2_->size(); i++) { @@ -644,16 +644,16 @@ std::ostream& operator<<(std::ostream& output, const Settings& settings) { output << "\t-Sequence FPS: " << settings.fps_ << std::endl; // Stereo stuff - if (settings.sensor_ == ORB_SLAM3::CameraType::STEREO || - settings.sensor_ == ORB_SLAM3::CameraType::IMU_STEREO) { + if (settings.sensor_ == MORB_SLAM::CameraType::STEREO || + settings.sensor_ == MORB_SLAM::CameraType::IMU_STEREO) { output << "\t-Stereo baseline: " << settings.b_ << std::endl; output << "\t-Stereo depth threshold : " << settings.thDepth_ << std::endl; if (settings.cameraType_ == Settings::KannalaBrandt) { auto vOverlapping1 = - static_cast(settings.calibration1_)->mvLappingArea; + std::reinterpret_pointer_cast(settings.calibration1_)->mvLappingArea; auto vOverlapping2 = - static_cast(settings.calibration2_)->mvLappingArea; + std::reinterpret_pointer_cast(settings.calibration2_)->mvLappingArea; output << "\t-Camera 1 overlapping area: [ " << vOverlapping1[0] << " , " << vOverlapping1[1] << " ]" << std::endl; output << "\t-Camera 2 overlapping area: [ " << vOverlapping2[0] << " , " @@ -661,9 +661,9 @@ std::ostream& operator<<(std::ostream& output, const Settings& settings) { } } - if (settings.sensor_ == ORB_SLAM3::CameraType::IMU_MONOCULAR || - settings.sensor_ == ORB_SLAM3::CameraType::IMU_STEREO || - settings.sensor_ == ORB_SLAM3::CameraType::IMU_RGBD) { + if (settings.sensor_ == MORB_SLAM::CameraType::IMU_MONOCULAR || + settings.sensor_ == MORB_SLAM::CameraType::IMU_STEREO || + settings.sensor_ == MORB_SLAM::CameraType::IMU_RGBD) { output << "\t-Gyro noise: " << settings.noiseGyro_ << std::endl; output << "\t-Accelerometer noise: " << settings.noiseAcc_ << std::endl; output << "\t-Gyro walk: " << settings.gyroWalk_ << std::endl; @@ -671,8 +671,8 @@ std::ostream& operator<<(std::ostream& output, const Settings& settings) { output << "\t-IMU frequency: " << settings.imuFrequency_ << std::endl; } - if (settings.sensor_ == ORB_SLAM3::CameraType::RGBD || - settings.sensor_ == ORB_SLAM3::CameraType::IMU_RGBD) { + if (settings.sensor_ == MORB_SLAM::CameraType::RGBD || + settings.sensor_ == MORB_SLAM::CameraType::IMU_RGBD) { output << "\t-RGB-D depth map factor: " << settings.depthMapFactor_ << std::endl; } @@ -684,4 +684,4 @@ std::ostream& operator<<(std::ostream& output, const Settings& settings) { return output; } -}; // namespace ORB_SLAM3 +}; // namespace MORB_SLAM diff --git a/src/Sim3Solver.cc b/src/Sim3Solver.cc index d8b48df2..8ef9c91c 100644 --- a/src/Sim3Solver.cc +++ b/src/Sim3Solver.cc @@ -19,17 +19,17 @@ * ORB-SLAM3. If not, see . */ -#include "Sim3Solver.h" +#include "MORB_SLAM/Sim3Solver.h" #include #include #include #include "DUtils/Random.h" -#include "KeyFrame.h" -#include "ORBmatcher.h" +#include "MORB_SLAM/KeyFrame.h" +#include "MORB_SLAM/ORBmatcher.h" -namespace ORB_SLAM3 { +namespace MORB_SLAM { Sim3Solver::Sim3Solver(KeyFrame *pKF1, KeyFrame *pKF2, const vector &vpMatched12, @@ -425,7 +425,7 @@ float Sim3Solver::GetEstimatedScale() { return mBestScale; } void Sim3Solver::Project(const vector &vP3Dw, vector &vP2D, Eigen::Matrix4f Tcw, - GeometricCamera *pCamera) { + const std::shared_ptr &pCamera) { Eigen::Matrix3f Rcw = Tcw.block<3, 3>(0, 0); Eigen::Vector3f tcw = Tcw.block<3, 1>(0, 3); @@ -441,7 +441,7 @@ void Sim3Solver::Project(const vector &vP3Dw, void Sim3Solver::FromCameraToImage(const vector &vP3Dc, vector &vP2D, - GeometricCamera *pCamera) { + const std::shared_ptr &pCamera) { vP2D.clear(); vP2D.reserve(vP3Dc.size()); @@ -451,4 +451,4 @@ void Sim3Solver::FromCameraToImage(const vector &vP3Dc, } } -} // namespace ORB_SLAM3 +} // namespace MORB_SLAM diff --git a/src/System.cc b/src/System.cc index 3ed92046..79c3b81c 100644 --- a/src/System.cc +++ b/src/System.cc @@ -19,12 +19,13 @@ * ORB-SLAM3. If not, see . */ -#include "System.h" +#include "MORB_SLAM/System.h" #include +#include #include -#include "ImprovedTypes.hpp" +#include "MORB_SLAM/ImprovedTypes.hpp" #include #include #include @@ -40,9 +41,9 @@ #include #include -#include "Converter.h" +#include "MORB_SLAM/Converter.h" -namespace ORB_SLAM3 { +namespace MORB_SLAM { Verbose::eLevel Verbose::th = Verbose::VERBOSITY_NORMAL; @@ -55,6 +56,7 @@ System::System(const std::string& strVocFile, const std::string& strSettingsFile mbResetActiveMap(false), mbActivateLocalizationMode(false), mbDeactivateLocalizationMode(false) { + cameras.push_back(make_shared(mSensor)); // for now just hard code the sensor we are using, TODO make multicam // Output welcome message std::cout << std::endl << "ORB-SLAM3 Copyright (C) 2017-2020 Carlos Campos, Richard Elvira, " @@ -206,7 +208,7 @@ System::System(const std::string& strVocFile, const std::string& strSettingsFile this, mpAtlas, mSensor == CameraType::MONOCULAR || mSensor == CameraType::IMU_MONOCULAR, mSensor == CameraType::IMU_MONOCULAR || mSensor == CameraType::IMU_STEREO || mSensor == CameraType::IMU_RGBD, strSequence); - mptLocalMapping = new thread(&ORB_SLAM3::LocalMapping::Run, mpLocalMapper); + mptLocalMapping = new thread(&MORB_SLAM::LocalMapping::Run, mpLocalMapper); if (settings_) mpLocalMapper->mThFarPoints = settings_->thFarPoints(); else @@ -223,7 +225,7 @@ System::System(const std::string& strVocFile, const std::string& strSettingsFile mpLoopCloser = new LoopClosing(mpAtlas, mpKeyFrameDatabase, mpVocabulary, mSensor != CameraType::MONOCULAR, activeLC); // mSensor!=CameraType::MONOCULAR); - mptLoopClosing = new thread(&ORB_SLAM3::LoopClosing::Run, mpLoopCloser); + mptLoopClosing = new thread(&MORB_SLAM::LoopClosing::Run, mpLoopCloser); // Set pointers between threads mpTracker->SetLocalMapper(mpLocalMapper); @@ -252,10 +254,10 @@ Sophus::SE3f System::TrackStereo(const cv::Mat& imLeft, const cv::Mat& imRight, cv::Mat imLeftToFeed, imRightToFeed; if (settings_ && settings_->needToRectify()) { - cv::Mat M1l = settings_->M1l(); - cv::Mat M2l = settings_->M2l(); - cv::Mat M1r = settings_->M1r(); - cv::Mat M2r = settings_->M2r(); + const cv::Mat &M1l = settings_->M1l(); + const cv::Mat &M2l = settings_->M2l(); + const cv::Mat &M1r = settings_->M1r(); + const cv::Mat &M2r = settings_->M2r(); cv::remap(imLeft, imLeftToFeed, M1l, M2l, cv::INTER_LINEAR); cv::remap(imRight, imRightToFeed, M1r, M2r, cv::INTER_LINEAR); @@ -263,8 +265,8 @@ Sophus::SE3f System::TrackStereo(const cv::Mat& imLeft, const cv::Mat& imRight, cv::resize(imLeft, imLeftToFeed, settings_->newImSize()); cv::resize(imRight, imRightToFeed, settings_->newImSize()); } else { - imLeftToFeed = imLeft.clone(); - imRightToFeed = imRight.clone(); + imLeftToFeed = imLeft; + imRightToFeed = imRight; } // Check mode change @@ -307,7 +309,7 @@ Sophus::SE3f System::TrackStereo(const cv::Mat& imLeft, const cv::Mat& imRight, // std::cout << "start GrabImageStereo" << std::endl; Sophus::SE3f Tcw = mpTracker->GrabImageStereo(imLeftToFeed, imRightToFeed, - timestamp, filename); + timestamp, filename, cameras[0]); // for now we know cameras[0] is providing the image // std::cout << "out grabber" << std::endl; @@ -378,7 +380,7 @@ Sophus::SE3f System::TrackRGBD(const cv::Mat& im, const cv::Mat& depthmap, mpTracker->GrabImuData(vImuMeas[i_imu]); Sophus::SE3f Tcw = - mpTracker->GrabImageRGBD(imToFeed, imDepthToFeed, timestamp, filename); + mpTracker->GrabImageRGBD(imToFeed, imDepthToFeed, timestamp, filename, cameras[0]); // for now we know cameras[0] is providing the image unique_lock lock2(mMutexState); mTrackingState = mpTracker->mState; @@ -449,7 +451,7 @@ Sophus::SE3f System::TrackMonocular(const cv::Mat& im, const double& timestamp, mpTracker->GrabImuData(vImuMeas[i_imu]); Sophus::SE3f Tcw = - mpTracker->GrabImageMonocular(imToFeed, timestamp, filename); + mpTracker->GrabImageMonocular(imToFeed, timestamp, filename, cameras[0]); // for now we know cameras[0] is providing the image unique_lock lock2(mMutexState); mTrackingState = mpTracker->mState; @@ -553,7 +555,7 @@ void System::SaveTrajectoryTUM(const string& filename) { // For each frame we have a reference keyframe (lRit), the timestamp (lT) and // a flag which is true when tracking failed (lbL). - list::iterator lRit = mpTracker->mlpReferences.begin(); + list::iterator lRit = mpTracker->mlpReferences.begin(); list::iterator lT = mpTracker->mlFrameTimes.begin(); list::iterator lbL = mpTracker->mlbLost.begin(); for (list::iterator @@ -628,12 +630,12 @@ void System::SaveTrajectoryEuRoC(const string& filename) { endl; return; }*/ - vector vpMaps = mpAtlas->GetAllMaps(); + vector> vpMaps = mpAtlas->GetAllMaps(); size_t numMaxKFs = 0; - Map* pBiggerMap = nullptr; + std::shared_ptr pBiggerMap = nullptr; std::cout << "There are " << std::to_string(vpMaps.size()) << " maps in the atlas" << std::endl; - for (Map* pMap : vpMaps) { + for (std::shared_ptr pMap : vpMaps) { std::cout << " Map " << std::to_string(pMap->GetId()) << " has " << std::to_string(pMap->GetAllKeyFrames().size()) << " KFs" << std::endl; @@ -672,7 +674,7 @@ void System::SaveTrajectoryEuRoC(const string& filename) { // For each frame we have a reference keyframe (lRit), the timestamp (lT) and // a flag which is true when tracking failed (lbL). - list::iterator lRit = mpTracker->mlpReferences.begin(); + list::iterator lRit = mpTracker->mlpReferences.begin(); list::iterator lT = mpTracker->mlFrameTimes.begin(); list::iterator lbL = mpTracker->mlbLost.begin(); @@ -742,7 +744,7 @@ void System::SaveTrajectoryEuRoC(const string& filename) { cout << endl << "End of saving trajectory to " << filename << " ..." << endl; } -void System::SaveTrajectoryEuRoC(const string& filename, Map* pMap) { +void System::SaveTrajectoryEuRoC(const string& filename, std::shared_ptr pMap) { cout << endl << "Saving trajectory of map " << pMap->GetId() << " to " << filename << " ..." << endl; @@ -778,7 +780,7 @@ void System::SaveTrajectoryEuRoC(const string& filename, Map* pMap) { // For each frame we have a reference keyframe (lRit), the timestamp (lT) and // a flag which is true when tracking failed (lbL). - list::iterator lRit = mpTracker->mlpReferences.begin(); + list::iterator lRit = mpTracker->mlpReferences.begin(); list::iterator lT = mpTracker->mlFrameTimes.begin(); list::iterator lbL = mpTracker->mlbLost.begin(); @@ -893,7 +895,7 @@ transformation. // For each frame we have a reference keyframe (lRit), the timestamp (lT) and a flag // which is true when tracking failed (lbL). - list::iterator lRit = + list::iterator lRit = mpTracker->mlpReferences.begin(); list::iterator lT = mpTracker->mlFrameTimes.begin(); list::iterator lbL = mpTracker->mlbLost.begin(); @@ -1044,10 +1046,10 @@ void System::SaveKeyFrameTrajectoryEuRoC(const string& filename) { cout << endl << "Saving keyframe trajectory to " << filename << " ... Euroc" << endl; - vector vpMaps = mpAtlas->GetAllMaps(); - Map* pBiggerMap = nullptr; + vector> vpMaps = mpAtlas->GetAllMaps(); + std::shared_ptr pBiggerMap = nullptr; size_t numMaxKFs = 0; - for (Map* pMap : vpMaps) { + for (std::shared_ptr pMap : vpMaps) { if (pMap && pMap->GetAllKeyFrames().size() > numMaxKFs) { numMaxKFs = pMap->GetAllKeyFrames().size(); pBiggerMap = pMap; @@ -1096,7 +1098,7 @@ void System::SaveKeyFrameTrajectoryEuRoC(const string& filename) { f.close(); } -void System::SaveKeyFrameTrajectoryEuRoC(const string& filename, Map* pMap) { +void System::SaveKeyFrameTrajectoryEuRoC(const string& filename, std::shared_ptr pMap) { cout << endl << "Saving keyframe trajectory of map " << pMap->GetId() << " to " << filename << " ..." << endl; @@ -1164,13 +1166,13 @@ transformation. // For each frame we have a reference keyframe (lRit), the timestamp (lT) and a flag // which is true when tracking failed (lbL). - list::iterator lRit = + list::iterator lRit = mpTracker->mlpReferences.begin(); list::iterator lT = mpTracker->mlFrameTimes.begin(); for(list::iterator lit=mpTracker->mlRelativeFramePoses.begin(), lend=mpTracker->mlRelativeFramePoses.end();lit!=lend;lit++, lRit++, lT++) { - ORB_SLAM3::KeyFrame* pKF = *lRit; + MORB_SLAM::KeyFrame* pKF = *lRit; cv::Mat Trw = cv::Mat::eye(4,4,CV_32F); @@ -1221,13 +1223,13 @@ void System::SaveTrajectoryKITTI(const string& filename) { // For each frame we have a reference keyframe (lRit), the timestamp (lT) and // a flag which is true when tracking failed (lbL). - list::iterator lRit = mpTracker->mlpReferences.begin(); + list::iterator lRit = mpTracker->mlpReferences.begin(); list::iterator lT = mpTracker->mlFrameTimes.begin(); for (list::iterator lit = mpTracker->mlRelativeFramePoses.begin(), lend = mpTracker->mlRelativeFramePoses.end(); lit != lend; lit++, lRit++, lT++) { - ORB_SLAM3::KeyFrame* pKF = *lRit; + MORB_SLAM::KeyFrame* pKF = *lRit; Sophus::SE3f Trw; @@ -1548,4 +1550,4 @@ string System::CalculateCheckSum(string filename, int type) { return checksum; } -} // namespace ORB_SLAM3 +} // namespace MORB_SLAM diff --git a/src/Tracking.cc b/src/Tracking.cc index 31b1d8b5..e72d80e3 100644 --- a/src/Tracking.cc +++ b/src/Tracking.cc @@ -19,29 +19,30 @@ * ORB-SLAM3. If not, see . */ -#include "Tracking.h" +#include "MORB_SLAM/Tracking.h" -#include "ImprovedTypes.hpp" +#include "MORB_SLAM/ImprovedTypes.hpp" #include #include #include +#include -#include "Converter.h" -#include "G2oTypes.h" -#include "GeometricTools.h" -#include "KannalaBrandt8.h" -#include "MLPnPsolver.h" -#include "ORBmatcher.h" -#include "Optimizer.h" -#include "Pinhole.h" +#include "MORB_SLAM/Converter.h" +#include "MORB_SLAM/G2oTypes.h" +#include "MORB_SLAM/GeometricTools.h" +#include "MORB_SLAM/CameraModels/KannalaBrandt8.h" +#include "MORB_SLAM/MLPnPsolver.h" +#include "MORB_SLAM/ORBmatcher.h" +#include "MORB_SLAM/Optimizer.h" +#include "MORB_SLAM/CameraModels/Pinhole.h" using namespace std; -namespace ORB_SLAM3 { +namespace MORB_SLAM { Tracking::Tracking(System* pSys, ORBVocabulary* pVoc, const Atlas_ptr &pAtlas, KeyFrameDatabase* pKFDB, const string& strSettingPath, - const int sensor, Settings* settings, const string& _nameSeq) + const CameraType::eSensor sensor, Settings* settings, const string& _nameSeq) : mState(Tracker::NO_IMAGES_YET), mSensor(sensor), mTrackedFr(0), @@ -51,10 +52,10 @@ Tracking::Tracking(System* pSys, ORBVocabulary* pVoc, const Atlas_ptr &pAtlas, mbVO(false), mpORBVocabulary(pVoc), mpKeyFrameDB(pKFDB), - mbReadyToInitializate(false), + mbReadyToInitialize(false), mpSystem(pSys), mpAtlas(pAtlas), - mpLastKeyFrame(static_cast(NULL)), + mpLastKeyFrame(nullptr), mnLastRelocFrameId(0), time_recently_lost(5.0), mnFirstFrameId(0), @@ -81,8 +82,7 @@ Tracking::Tracking(System* pSys, ORBVocabulary* pVoc, const Atlas_ptr &pAtlas, } bool b_parse_imu = true; - if (sensor == CameraType::IMU_MONOCULAR || sensor == CameraType::IMU_STEREO || - sensor == CameraType::IMU_RGBD) { + if (CameraType::isInertial(mSensor)) { b_parse_imu = ParseIMUParamFile(fSettings); if (!b_parse_imu) { std::cout << "*Error with the IMU parameters in the config file*" @@ -95,10 +95,7 @@ Tracking::Tracking(System* pSys, ORBVocabulary* pVoc, const Atlas_ptr &pAtlas, if (!b_parse_cam || !b_parse_orb || !b_parse_imu) { std::cerr << "**ERROR in the config file, the format is not correct**" << std::endl; - try { - throw -1; - } catch (exception& e) { - } + throw std::runtime_error("**ERROR in the config file, the format is not correct**"); } } @@ -107,10 +104,10 @@ Tracking::Tracking(System* pSys, ORBVocabulary* pVoc, const Atlas_ptr &pAtlas, mbInitWith3KFs = false; mnNumDataset = 0; - vector vpCams = mpAtlas->GetAllCameras(); + vector> vpCams = mpAtlas->GetAllCameras(); std::cout << "There are " << vpCams.size() << " cameras in the atlas" << std::endl; - for (GeometricCamera* pCam : vpCams) { + for (std::shared_ptr pCam : vpCams) { std::cout << "Camera " << pCam->GetId(); if (pCam->GetType() == GeometricCamera::CAM_PINHOLE) { std::cout << " is pinhole" << std::endl; @@ -578,8 +575,7 @@ void Tracking::newParameterLoader(Settings* settings) { mK_(0, 2) = mpCamera->getParameter(2); mK_(1, 2) = mpCamera->getParameter(3); - if ((mSensor == CameraType::STEREO || mSensor == CameraType::IMU_STEREO || - mSensor == CameraType::IMU_RGBD) && + if (CameraType::hasMulticam(mSensor) && settings->cameraType() == Settings::KannalaBrandt) { mpCamera2 = settings->camera2(); mpCamera2 = mpAtlas->AddCamera(mpCamera2); @@ -587,8 +583,7 @@ void Tracking::newParameterLoader(Settings* settings) { mTlr = settings->Tlr(); } - if (mSensor == CameraType::STEREO || mSensor == CameraType::RGBD || - mSensor == CameraType::IMU_STEREO || mSensor == CameraType::IMU_RGBD) { + if (CameraType::hasMulticam(mSensor)) { mbf = settings->bf(); mThDepth = settings->b() * settings->thDepth(); } @@ -612,15 +607,15 @@ void Tracking::newParameterLoader(Settings* settings) { int fMinThFAST = settings->minThFAST(); float fScaleFactor = settings->scaleFactor(); - mpORBextractorLeft = new ORBextractor(nFeatures, fScaleFactor, nLevels, + mpORBextractorLeft = std::make_shared(nFeatures, fScaleFactor, nLevels, fIniThFAST, fMinThFAST); if (mSensor == CameraType::STEREO || mSensor == CameraType::IMU_STEREO) - mpORBextractorRight = new ORBextractor(nFeatures, fScaleFactor, nLevels, + mpORBextractorRight = std::make_shared(nFeatures, fScaleFactor, nLevels, fIniThFAST, fMinThFAST); if (mSensor == CameraType::MONOCULAR || mSensor == CameraType::IMU_MONOCULAR) - mpIniORBextractor = new ORBextractor(5 * nFeatures, fScaleFactor, nLevels, + mpIniORBextractor = std::make_shared(5 * nFeatures, fScaleFactor, nLevels, fIniThFAST, fMinThFAST); // IMU parameters @@ -634,7 +629,7 @@ void Tracking::newParameterLoader(Settings* settings) { float Naw = settings->accWalk(); const float sf = sqrt(mImuFreq); - mpImuCalib = new IMU::Calib(Tbc, Ng * sf, Na * sf, Ngw / sf, Naw / sf); + mpImuCalib = std::make_shared(Tbc, Ng * sf, Na * sf, Ngw / sf, Naw / sf); mpImuPreintegratedFromLastKF = new IMU::Preintegrated(IMU::Bias(), *mpImuCalib); @@ -749,7 +744,7 @@ bool Tracking::ParseCamParamFile(cv::FileStorage& fSettings) { vector vCamCalib{fx, fy, cx, cy}; - mpCamera = new Pinhole(vCamCalib); + mpCamera = std::make_shared(vCamCalib); mpCamera = mpAtlas->AddCamera(mpCamera); @@ -871,7 +866,7 @@ bool Tracking::ParseCamParamFile(cv::FileStorage& fSettings) { } vector vCamCalib{fx, fy, cx, cy, k1, k2, k3, k4}; - mpCamera = new KannalaBrandt8(vCamCalib); + mpCamera = std::make_shared(vCamCalib); mpCamera = mpAtlas->AddCamera(mpCamera); std::cout << "- Camera: Fisheye" << std::endl; std::cout << "- Image scale: " << mImageScale << std::endl; @@ -897,8 +892,7 @@ bool Tracking::ParseCamParamFile(cv::FileStorage& fSettings) { mK_(1, 2) = cy; } - if (mSensor == CameraType::STEREO || mSensor == CameraType::IMU_STEREO || - mSensor == CameraType::IMU_RGBD) { + if (CameraType::hasMulticam(mSensor)) { // Right camera // Camera calibration parameters cv::FileNode node = fSettings["Camera2.fx"]; @@ -1043,20 +1037,20 @@ bool Tracking::ParseCamParamFile(cv::FileStorage& fSettings) { rightLappingEnd = rightLappingEnd * mImageScale; } - static_cast(mpCamera)->mvLappingArea[0] = + reinterpret_pointer_cast(mpCamera)->mvLappingArea[0] = leftLappingBegin; - static_cast(mpCamera)->mvLappingArea[1] = + reinterpret_pointer_cast(mpCamera)->mvLappingArea[1] = leftLappingEnd; vector vCamCalib2{fx, fy, cx, cy, k1, k2, k3, k4}; - mpCamera2 = new KannalaBrandt8(vCamCalib2); + mpCamera2 = std::make_shared(vCamCalib2); mpCamera2 = mpAtlas->AddCamera(mpCamera2); mTlr = Converter::toSophus(cvTlr); - static_cast(mpCamera2)->mvLappingArea[0] = + reinterpret_pointer_cast(mpCamera2)->mvLappingArea[0] = rightLappingBegin; - static_cast(mpCamera2)->mvLappingArea[1] = + reinterpret_pointer_cast(mpCamera2)->mvLappingArea[1] = rightLappingEnd; std::cout << "- Camera1 Lapping: " << leftLappingBegin << ", " @@ -1091,8 +1085,7 @@ bool Tracking::ParseCamParamFile(cv::FileStorage& fSettings) { << std::endl; } - if (mSensor == CameraType::STEREO || mSensor == CameraType::RGBD || - mSensor == CameraType::IMU_STEREO || mSensor == CameraType::IMU_RGBD) { + if (CameraType::hasMulticam(mSensor)) { cv::FileNode node = fSettings["Camera.bf"]; if (!node.empty() && node.isReal()) { mbf = node.real(); @@ -1123,8 +1116,7 @@ bool Tracking::ParseCamParamFile(cv::FileStorage& fSettings) { else cout << "- color order: BGR (ignored if grayscale)" << endl; - if (mSensor == CameraType::STEREO || mSensor == CameraType::RGBD || - mSensor == CameraType::IMU_STEREO || mSensor == CameraType::IMU_RGBD) { + if (CameraType::hasMulticam(mSensor)) { float fx = mpCamera->getParameter(0); cv::FileNode node = fSettings["ThDepth"]; if (!node.empty() && node.isReal()) { @@ -1221,15 +1213,15 @@ bool Tracking::ParseORBParamFile(cv::FileStorage& fSettings) { return false; } - mpORBextractorLeft = new ORBextractor(nFeatures, fScaleFactor, nLevels, + mpORBextractorLeft = std::make_shared(nFeatures, fScaleFactor, nLevels, fIniThFAST, fMinThFAST); if (mSensor == CameraType::STEREO || mSensor == CameraType::IMU_STEREO) - mpORBextractorRight = new ORBextractor(nFeatures, fScaleFactor, nLevels, + mpORBextractorRight = std::make_shared(nFeatures, fScaleFactor, nLevels, fIniThFAST, fMinThFAST); if (mSensor == CameraType::MONOCULAR || mSensor == CameraType::IMU_MONOCULAR) - mpIniORBextractor = new ORBextractor(5 * nFeatures, fScaleFactor, nLevels, + mpIniORBextractor = std::make_shared(5 * nFeatures, fScaleFactor, nLevels, fIniThFAST, fMinThFAST); cout << endl << "ORB Extractor Parameters: " << endl; @@ -1277,7 +1269,7 @@ bool Tracking::ParseIMUParamFile(cv::FileStorage& fSettings) { node = fSettings["IMU.Frequency"]; if (!node.empty() && node.isInt()) { mImuFreq = node.operator int(); - mImuPer = 0.001; // 1.0 / (double) mImuFreq; + mImuPer = 0.001; // 1.0 / (double) mImuFreq; //TODO: ESTO ESTA BIEN? } else { std::cerr << "*IMU.Frequency parameter doesn't exist or is not an integer*" << std::endl; @@ -1344,7 +1336,7 @@ bool Tracking::ParseIMUParamFile(cv::FileStorage& fSettings) { cout << "IMU accelerometer noise: " << Na << " m/s^2/sqrt(Hz)" << endl; cout << "IMU accelerometer walk: " << Naw << " m/s^3/sqrt(Hz)" << endl; - mpImuCalib = new IMU::Calib(Tbc, Ng * sf, Na * sf, Ngw / sf, Naw / sf); + mpImuCalib = std::make_shared(Tbc, Ng * sf, Na * sf, Ngw / sf, Naw / sf); mpImuPreintegratedFromLastKF = new IMU::Preintegrated(IMU::Bias(), *mpImuCalib); @@ -1363,7 +1355,8 @@ void Tracking::SetLoopClosing(LoopClosing* pLoopClosing) { Sophus::SE3f Tracking::GrabImageStereo(const cv::Mat& imRectLeft, const cv::Mat& imRectRight, const double& timestamp, - string filename) { + const string &filename, + const Camera_ptr &cam) { // cout << "GrabImageStereo" << endl; mImGray = imRectLeft; @@ -1393,28 +1386,25 @@ Sophus::SE3f Tracking::GrabImageStereo(const cv::Mat& imRectLeft, // cout << "Incoming frame creation" << endl; if (mSensor == CameraType::STEREO && !mpCamera2) - mCurrentFrame = Frame(mImGray, imGrayRight, timestamp, mpORBextractorLeft, + mCurrentFrame = Frame(cam, mImGray, imGrayRight, timestamp, mpORBextractorLeft, mpORBextractorRight, mpORBVocabulary, mK, mDistCoef, - mbf, mThDepth, mpCamera); + mbf, mThDepth, mpCamera, filename, mnNumDataset); else if (mSensor == CameraType::STEREO && mpCamera2) - mCurrentFrame = Frame(mImGray, imGrayRight, timestamp, mpORBextractorLeft, + mCurrentFrame = Frame(cam, mImGray, imGrayRight, timestamp, mpORBextractorLeft, mpORBextractorRight, mpORBVocabulary, mK, mDistCoef, - mbf, mThDepth, mpCamera, mpCamera2, mTlr); + mbf, mThDepth, mpCamera, mpCamera2, filename, mnNumDataset, mTlr); else if (mSensor == CameraType::IMU_STEREO && !mpCamera2) - mCurrentFrame = Frame(mImGray, imGrayRight, timestamp, mpORBextractorLeft, + mCurrentFrame = Frame(cam, mImGray, imGrayRight, timestamp, mpORBextractorLeft, mpORBextractorRight, mpORBVocabulary, mK, mDistCoef, - mbf, mThDepth, mpCamera, &mLastFrame, *mpImuCalib); + mbf, mThDepth, mpCamera, filename, mnNumDataset, &mLastFrame, *mpImuCalib); else if (mSensor == CameraType::IMU_STEREO && mpCamera2) mCurrentFrame = - Frame(mImGray, imGrayRight, timestamp, mpORBextractorLeft, + Frame(cam, mImGray, imGrayRight, timestamp, mpORBextractorLeft, mpORBextractorRight, mpORBVocabulary, mK, mDistCoef, mbf, - mThDepth, mpCamera, mpCamera2, mTlr, &mLastFrame, *mpImuCalib); + mThDepth, mpCamera, mpCamera2, filename, mnNumDataset, mTlr, &mLastFrame, *mpImuCalib); // cout << "Incoming frame ended" << endl; - mCurrentFrame.mNameFile = filename; - mCurrentFrame.mnDataset = mnNumDataset; - #ifdef REGISTER_TIMES vdORBExtract_ms.push_back(mCurrentFrame.mTimeORB_Ext); vdStereoMatch_ms.push_back(mCurrentFrame.mTimeStereoMatch); @@ -1428,7 +1418,8 @@ Sophus::SE3f Tracking::GrabImageStereo(const cv::Mat& imRectLeft, } Sophus::SE3f Tracking::GrabImageRGBD(const cv::Mat& imRGB, const cv::Mat& imD, - const double& timestamp, string filename) { + const double& timestamp, const string &filename, + const Camera_ptr &cam) { mImGray = imRGB; cv::Mat imDepth = imD; @@ -1449,15 +1440,12 @@ Sophus::SE3f Tracking::GrabImageRGBD(const cv::Mat& imRGB, const cv::Mat& imD, if (mSensor == CameraType::RGBD) mCurrentFrame = - Frame(mImGray, imDepth, timestamp, mpORBextractorLeft, mpORBVocabulary, - mK, mDistCoef, mbf, mThDepth, mpCamera); + Frame(cam, mImGray, imDepth, timestamp, mpORBextractorLeft, mpORBVocabulary, + mK, mDistCoef, mbf, mThDepth, mpCamera, filename, mnNumDataset); else if (mSensor == CameraType::IMU_RGBD) mCurrentFrame = - Frame(mImGray, imDepth, timestamp, mpORBextractorLeft, mpORBVocabulary, - mK, mDistCoef, mbf, mThDepth, mpCamera, &mLastFrame, *mpImuCalib); - - mCurrentFrame.mNameFile = filename; - mCurrentFrame.mnDataset = mnNumDataset; + Frame(cam, mImGray, imDepth, timestamp, mpORBextractorLeft, mpORBVocabulary, + mK, mDistCoef, mbf, mThDepth, mpCamera, filename, mnNumDataset, &mLastFrame, *mpImuCalib); #ifdef REGISTER_TIMES vdORBExtract_ms.push_back(mCurrentFrame.mTimeORB_Ext); @@ -1470,7 +1458,8 @@ Sophus::SE3f Tracking::GrabImageRGBD(const cv::Mat& imRGB, const cv::Mat& imD, Sophus::SE3f Tracking::GrabImageMonocular(const cv::Mat& im, const double& timestamp, - string filename) { + const string &filename, + const Camera_ptr &cam) { mImGray = im; if (mImGray.channels() == 3) { if (mbRGB) @@ -1488,28 +1477,25 @@ Sophus::SE3f Tracking::GrabImageMonocular(const cv::Mat& im, if (mState == Tracker::NOT_INITIALIZED || mState == Tracker::NO_IMAGES_YET || (lastID - initID) < mMaxFrames) mCurrentFrame = - Frame(mImGray, timestamp, mpIniORBextractor, mpORBVocabulary, - mpCamera, mDistCoef, mbf, mThDepth); + Frame(cam, mImGray, timestamp, mpIniORBextractor, mpORBVocabulary, + mpCamera, mDistCoef, mbf, mThDepth, filename, mnNumDataset); else mCurrentFrame = - Frame(mImGray, timestamp, mpORBextractorLeft, mpORBVocabulary, - mpCamera, mDistCoef, mbf, mThDepth); + Frame(cam, mImGray, timestamp, mpORBextractorLeft, mpORBVocabulary, + mpCamera, mDistCoef, mbf, mThDepth, filename, mnNumDataset); } else if (mSensor == CameraType::IMU_MONOCULAR) { if (mState == Tracker::NOT_INITIALIZED || mState == Tracker::NO_IMAGES_YET) { mCurrentFrame = - Frame(mImGray, timestamp, mpIniORBextractor, mpORBVocabulary, - mpCamera, mDistCoef, mbf, mThDepth, &mLastFrame, *mpImuCalib); + Frame(cam, mImGray, timestamp, mpIniORBextractor, mpORBVocabulary, + mpCamera, mDistCoef, mbf, mThDepth, filename, mnNumDataset, &mLastFrame, *mpImuCalib); } else mCurrentFrame = - Frame(mImGray, timestamp, mpORBextractorLeft, mpORBVocabulary, - mpCamera, mDistCoef, mbf, mThDepth, &mLastFrame, *mpImuCalib); + Frame(cam, mImGray, timestamp, mpORBextractorLeft, mpORBVocabulary, + mpCamera, mDistCoef, mbf, mThDepth, filename, mnNumDataset, &mLastFrame, *mpImuCalib); } if (mState == Tracker::NO_IMAGES_YET) t0 = timestamp; - mCurrentFrame.mNameFile = filename; - mCurrentFrame.mnDataset = mnNumDataset; - #ifdef REGISTER_TIMES vdORBExtract_ms.push_back(mCurrentFrame.mTimeORB_Ext); #endif @@ -1541,10 +1527,10 @@ void Tracking::PreintegrateIMU() { return; } - while (true) { + while (true) { + unique_lock lock(mMutexImuQueue); bool bSleep = false; { - unique_lock lock(mMutexImuQueue); if (!mlQueueImuData.empty()) { IMU::Point* m = &mlQueueImuData.front(); cout.precision(17); @@ -1702,7 +1688,7 @@ void Tracking::Track() { return; } - Map* pCurrentMap = mpAtlas->GetCurrentMap(mpSystem); + std::shared_ptr pCurrentMap = mpAtlas->GetCurrentMap(mpSystem); if (!pCurrentMap) { // if (!mpSystem->isShutDown()){ // cout << "ERROR: There is not an active map in the atlas" << endl; @@ -1719,12 +1705,10 @@ void Tracking::Track() { mlQueueImuData.clear(); CreateMapInAtlas(); return; - } + } } - if ((mSensor == CameraType::IMU_MONOCULAR || mSensor == CameraType::IMU_STEREO || - mSensor == CameraType::IMU_RGBD) && - mpLastKeyFrame) + if (CameraType::isInertial(mSensor) && mpLastKeyFrame) mCurrentFrame.SetNewBias(mpLastKeyFrame->GetImuBias()); if (mState == Tracker::NO_IMAGES_YET) { @@ -1733,9 +1717,7 @@ void Tracking::Track() { mLastProcessedState = mState; - if ((mSensor == CameraType::IMU_MONOCULAR || mSensor == CameraType::IMU_STEREO || - mSensor == CameraType::IMU_RGBD) && - !mbCreatedMap) { + if (CameraType::isInertial(mSensor) && !mbCreatedMap) { #ifdef REGISTER_TIMES std::chrono::steady_clock::time_point time_StartPreIMU = std::chrono::steady_clock::now(); @@ -1767,8 +1749,7 @@ void Tracking::Track() { } if (mState == Tracker::NOT_INITIALIZED) { - if (mSensor == CameraType::STEREO || mSensor == CameraType::RGBD || - mSensor == CameraType::IMU_STEREO || mSensor == CameraType::IMU_RGBD) { + if (CameraType::hasMulticam(mSensor)) { StereoInitialization(); } else { MonocularInitialization(); @@ -1818,8 +1799,7 @@ void Tracking::Track() { if (!bOK) { if (mCurrentFrame.mnId <= (mnLastRelocFrameId + mnFramesToResetIMU) && - (mSensor == CameraType::IMU_MONOCULAR || - mSensor == CameraType::IMU_STEREO || mSensor == CameraType::IMU_RGBD)) { + CameraType::isInertial(mSensor)) { mState = Tracker::LOST; } else if (pCurrentMap->KeyFramesInMap() > 10) { // cout << "KF in map: " << pCurrentMap->KeyFramesInMap() << endl; @@ -1835,8 +1815,7 @@ void Tracking::Track() { Verbose::VERBOSITY_NORMAL); bOK = true; - if ((mSensor == CameraType::IMU_MONOCULAR || - mSensor == CameraType::IMU_STEREO || mSensor == CameraType::IMU_RGBD)) { + if (CameraType::isInertial(mSensor)) { if (pCurrentMap->isImuInitialized()) PredictStateIMU(); else @@ -1871,7 +1850,7 @@ void Tracking::Track() { } else CreateMapInAtlas(); - if (mpLastKeyFrame) mpLastKeyFrame = static_cast(NULL); + if (mpLastKeyFrame) mpLastKeyFrame = nullptr; Verbose::PrintMess("done", Verbose::VERBOSITY_NORMAL); @@ -1883,8 +1862,7 @@ void Tracking::Track() { // Localization Mode: Local Mapping is deactivated (TODO Not available in // inertial mode) if (mState == Tracker::LOST) { - if (mSensor == CameraType::IMU_MONOCULAR || mSensor == CameraType::IMU_STEREO || - mSensor == CameraType::IMU_RGBD) + if (CameraType::isInertial(mSensor)) Verbose::PrintMess("IMU. State LOST", Verbose::VERBOSITY_NORMAL); bOK = Relocalization(); } else { @@ -1974,8 +1952,7 @@ void Tracking::Track() { if (bOK) mState = Tracker::OK; else if (mState == Tracker::OK) { - if (mSensor == CameraType::IMU_MONOCULAR || mSensor == CameraType::IMU_STEREO || - mSensor == CameraType::IMU_RGBD) { + if (CameraType::isInertial(mSensor)) { Verbose::PrintMess("Track lost for less than one second...", Verbose::VERBOSITY_NORMAL); if (!pCurrentMap->isImuInitialized() || @@ -2000,8 +1977,7 @@ void Tracking::Track() { // modified) if ((mCurrentFrame.mnId < (mnLastRelocFrameId + mnFramesToResetIMU)) && (static_cast(mCurrentFrame.mnId) > mnFramesToResetIMU) && - (mSensor == CameraType::IMU_MONOCULAR || mSensor == CameraType::IMU_STEREO || - mSensor == CameraType::IMU_RGBD) && + CameraType::isInertial(mSensor) && pCurrentMap->isImuInitialized()) { // TODO check this situation Verbose::PrintMess("Saving pointer to frame. imu needs reset...", @@ -2050,8 +2026,7 @@ void Tracking::Track() { mbVelocity = false; } - // if (mSensor == CameraType::IMU_MONOCULAR || mSensor == CameraType::IMU_STEREO || - // mSensor == CameraType::IMU_RGBD) + // if (CameraType::isInertial(mSensor)) // mpMapDrawer->SetCurrentCameraPose(mCurrentFrame.GetPose()); // Clean VO matches @@ -2060,7 +2035,7 @@ void Tracking::Track() { if (pMP) if (pMP->Observations() < 1) { mCurrentFrame.mvbOutlier[i] = false; - mCurrentFrame.mvpMapPoints[i] = static_cast(NULL); + mCurrentFrame.mvpMapPoints[i] = nullptr; } } @@ -2082,9 +2057,7 @@ void Tracking::Track() { // Check if we need to insert a new keyframe // if(bNeedKF && bOK) if (bNeedKF && (bOK || (mInsertKFsLost && mState == Tracker::RECENTLY_LOST && - (mSensor == CameraType::IMU_MONOCULAR || - mSensor == CameraType::IMU_STEREO || - mSensor == CameraType::IMU_RGBD)))) + CameraType::isInertial(mSensor)))) CreateNewKeyFrame(); #ifdef REGISTER_TIMES @@ -2105,7 +2078,7 @@ void Tracking::Track() { // frame. Only has effect if lastframe is tracked for (int i = 0; i < mCurrentFrame.N; i++) { if (mCurrentFrame.mvpMapPoints[i] && mCurrentFrame.mvbOutlier[i]) - mCurrentFrame.mvpMapPoints[i] = static_cast(NULL); + mCurrentFrame.mvpMapPoints[i] = nullptr; } } @@ -2115,8 +2088,7 @@ void Tracking::Track() { mpSystem->ResetActiveMap(); return; } - if (mSensor == CameraType::IMU_MONOCULAR || mSensor == CameraType::IMU_STEREO || - mSensor == CameraType::IMU_RGBD) + if (CameraType::isInertial(mSensor)) if (!pCurrentMap->isImuInitialized()) { Verbose::PrintMess( "Track lost before IMU initialisation, reseting...", @@ -2276,7 +2248,7 @@ void Tracking::StereoInitialization() { } void Tracking::MonocularInitialization() { - if (!mbReadyToInitializate) { + if (!mbReadyToInitialize) { // Set Reference Frame if (mCurrentFrame.mvKeys.size() > 100) { mInitialFrame = Frame(mCurrentFrame); @@ -2296,7 +2268,7 @@ void Tracking::MonocularInitialization() { mCurrentFrame.mpImuPreintegrated = mpImuPreintegratedFromLastKF; } - mbReadyToInitializate = true; + mbReadyToInitialize = true; return; } @@ -2304,7 +2276,7 @@ void Tracking::MonocularInitialization() { if (((int)mCurrentFrame.mvKeys.size() <= 100) || ((mSensor == CameraType::IMU_MONOCULAR) && (mLastFrame.mTimeStamp - mInitialFrame.mTimeStamp > 1.0))) { - mbReadyToInitializate = false; + mbReadyToInitialize = false; return; } @@ -2316,7 +2288,7 @@ void Tracking::MonocularInitialization() { // Check if there are enough correspondences if (nmatches < 100) { - mbReadyToInitializate = false; + mbReadyToInitialize = false; return; } @@ -2350,7 +2322,7 @@ void Tracking::CreateInitialMapMonocular() { new KeyFrame(mCurrentFrame, mpAtlas->GetCurrentMap(), mpKeyFrameDB); if (mSensor == CameraType::IMU_MONOCULAR) - pKFini->mpImuPreintegrated = (IMU::Preintegrated*)(NULL); + pKFini->mpImuPreintegrated = (IMU::Preintegrated*)(nullptr); pKFini->ComputeBoW(); pKFcur->ComputeBoW(); @@ -2479,8 +2451,7 @@ void Tracking::CreateInitialMapMonocular() { void Tracking::CreateMapInAtlas() { mnLastInitFrameId = mCurrentFrame.mnId; mpAtlas->CreateNewMap(); - if (mSensor == CameraType::IMU_STEREO || mSensor == CameraType::IMU_MONOCULAR || - mSensor == CameraType::IMU_RGBD) + if (CameraType::isInertial(mSensor)) mpAtlas->SetInertialSensor(); mbSetInit = false; @@ -2497,20 +2468,17 @@ void Tracking::CreateMapInAtlas() { mbVO = false; // Init value for know if there are enough MapPoints in the // last KF if (mSensor == CameraType::MONOCULAR || mSensor == CameraType::IMU_MONOCULAR) { - mbReadyToInitializate = false; + mbReadyToInitialize = false; } - if ((mSensor == CameraType::IMU_MONOCULAR || mSensor == CameraType::IMU_STEREO || - mSensor == CameraType::IMU_RGBD) && - mpImuPreintegratedFromLastKF) { + if (CameraType::isInertial(mSensor) && mpImuPreintegratedFromLastKF) { delete mpImuPreintegratedFromLastKF; - mpImuPreintegratedFromLastKF = - new IMU::Preintegrated(IMU::Bias(), *mpImuCalib); + mpImuPreintegratedFromLastKF = new IMU::Preintegrated(IMU::Bias(), *mpImuCalib); } - if (mpLastKeyFrame) mpLastKeyFrame = static_cast(NULL); + if (mpLastKeyFrame) mpLastKeyFrame = nullptr; - if (mpReferenceKF) mpReferenceKF = static_cast(NULL); + if (mpReferenceKF) mpReferenceKF = nullptr; mLastFrame = Frame(); mCurrentFrame = Frame(); @@ -2566,7 +2534,7 @@ bool Tracking::TrackReferenceKeyFrame() { if (mCurrentFrame.mvbOutlier[i]) { MapPoint* pMP = mCurrentFrame.mvpMapPoints[i]; - mCurrentFrame.mvpMapPoints[i] = static_cast(NULL); + mCurrentFrame.mvpMapPoints[i] = nullptr; mCurrentFrame.mvbOutlier[i] = false; if (i < mCurrentFrame.Nleft) { pMP->mbTrackInView = false; @@ -2581,8 +2549,7 @@ bool Tracking::TrackReferenceKeyFrame() { } } - if (mSensor == CameraType::IMU_MONOCULAR || mSensor == CameraType::IMU_STEREO || - mSensor == CameraType::IMU_RGBD) + if (CameraType::isInertial(mSensor)) return true; else return nmatchesMap >= 10; @@ -2669,7 +2636,7 @@ bool Tracking::TrackWithMotionModel() { } fill(mCurrentFrame.mvpMapPoints.begin(), mCurrentFrame.mvpMapPoints.end(), - static_cast(NULL)); + nullptr); // Project points seen in previous frame int th; @@ -2688,7 +2655,7 @@ bool Tracking::TrackWithMotionModel() { Verbose::PrintMess("Not enough matches, wider window search!!", Verbose::VERBOSITY_NORMAL); fill(mCurrentFrame.mvpMapPoints.begin(), mCurrentFrame.mvpMapPoints.end(), - static_cast(NULL)); + nullptr); nmatches = matcher.SearchByProjection( mCurrentFrame, mLastFrame, 2 * th, @@ -2699,8 +2666,7 @@ bool Tracking::TrackWithMotionModel() { if (nmatches < 20) { Verbose::PrintMess("Not enough matches!!", Verbose::VERBOSITY_NORMAL); - if (mSensor == CameraType::IMU_MONOCULAR || mSensor == CameraType::IMU_STEREO || - mSensor == CameraType::IMU_RGBD) + if (CameraType::isInertial(mSensor)) return true; else return false; @@ -2716,7 +2682,7 @@ bool Tracking::TrackWithMotionModel() { if (mCurrentFrame.mvbOutlier[i]) { MapPoint* pMP = mCurrentFrame.mvpMapPoints[i]; - mCurrentFrame.mvpMapPoints[i] = static_cast(NULL); + mCurrentFrame.mvpMapPoints[i] = nullptr; mCurrentFrame.mvbOutlier[i] = false; if (i < mCurrentFrame.Nleft) { pMP->mbTrackInView = false; @@ -2735,8 +2701,7 @@ bool Tracking::TrackWithMotionModel() { return nmatches > 20; } - if (mSensor == CameraType::IMU_MONOCULAR || mSensor == CameraType::IMU_STEREO || - mSensor == CameraType::IMU_RGBD) + if (CameraType::isInertial(mSensor)) return true; else return nmatchesMap >= 10; @@ -2805,7 +2770,7 @@ bool Tracking::TrackLocalMap() { } else mnMatchesInliers++; } else if (mSensor == CameraType::STEREO) - mCurrentFrame.mvpMapPoints[i] = static_cast(NULL); + mCurrentFrame.mvpMapPoints[i] = nullptr; } } @@ -2838,8 +2803,7 @@ bool Tracking::TrackLocalMap() { } bool Tracking::NeedNewKeyFrame() { - if ((mSensor == CameraType::IMU_MONOCULAR || mSensor == CameraType::IMU_STEREO || - mSensor == CameraType::IMU_RGBD) && + if (CameraType::isInertial(mSensor) && !mpAtlas->GetCurrentMap()->isImuInitialized()) { if (mSensor == CameraType::IMU_MONOCULAR && (mCurrentFrame.mTimeStamp - mpLastKeyFrame->mTimeStamp) >= 0.25) @@ -3016,14 +2980,12 @@ void Tracking::CreateNewKeyFrame() { Verbose::VERBOSITY_NORMAL); // Reset preintegration from last KF (Create new object) - if (mSensor == CameraType::IMU_MONOCULAR || mSensor == CameraType::IMU_STEREO || - mSensor == CameraType::IMU_RGBD) { + if (CameraType::isInertial(mSensor)) { mpImuPreintegratedFromLastKF = new IMU::Preintegrated(pKF->GetImuBias(), pKF->mImuCalib); } - if (mSensor != CameraType::MONOCULAR && - mSensor != CameraType::IMU_MONOCULAR) // TODO check if incluide imu_stereo + if (CameraType::hasMulticam(mSensor)) // TODO check if incluide imu_stereo { mCurrentFrame.UpdatePoseMatrices(); // cout << "create new MPs" << endl; @@ -3058,7 +3020,7 @@ void Tracking::CreateNewKeyFrame() { bCreateNew = true; else if (pMP->Observations() < 1) { bCreateNew = true; - mCurrentFrame.mvpMapPoints[i] = static_cast(NULL); + mCurrentFrame.mvpMapPoints[i] = nullptr; } if (bCreateNew) { @@ -3122,7 +3084,7 @@ void Tracking::SearchLocalPoints() { MapPoint* pMP = *vit; if (pMP) { if (pMP->isBad()) { - *vit = static_cast(NULL); + *vit = nullptr; } else { pMP->IncreaseVisible(); pMP->mnLastFrameSeen = mCurrentFrame.mnId; @@ -3162,9 +3124,7 @@ void Tracking::SearchLocalPoints() { th = 2; else th = 6; - } else if (!mpAtlas->isImuInitialized() && - (mSensor == CameraType::IMU_MONOCULAR || - mSensor == CameraType::IMU_STEREO || mSensor == CameraType::IMU_RGBD)) { + } else if (!mpAtlas->isImuInitialized() && CameraType::isInertial(mSensor)) { th = 10; } @@ -3234,7 +3194,7 @@ void Tracking::UpdateLocalKeyFrames() { it != itend; it++) keyframeCounter[it->first]++; } else { - mCurrentFrame.mvpMapPoints[i] = NULL; + mCurrentFrame.mvpMapPoints[i] = nullptr; } } } @@ -3254,14 +3214,14 @@ void Tracking::UpdateLocalKeyFrames() { keyframeCounter[it->first]++; } else { // MODIFICATION - mLastFrame.mvpMapPoints[i] = NULL; + mLastFrame.mvpMapPoints[i] = nullptr; } } } } int max = 0; - KeyFrame* pKFmax = static_cast(NULL); + KeyFrame* pKFmax = nullptr; mvpLocalKeyFrames.clear(); mvpLocalKeyFrames.reserve(3 * keyframeCounter.size()); @@ -3335,8 +3295,7 @@ void Tracking::UpdateLocalKeyFrames() { } // Add 10 last temporal KFs (mainly for IMU) - if ((mSensor == CameraType::IMU_MONOCULAR || mSensor == CameraType::IMU_STEREO || - mSensor == CameraType::IMU_RGBD) && + if (CameraType::isInertial(mSensor) && mvpLocalKeyFrames.size() < 80) { KeyFrame* tempKeyFrame = mCurrentFrame.mpLastKeyFrame; @@ -3452,7 +3411,7 @@ bool Tracking::Relocalization() { mCurrentFrame.mvpMapPoints[j] = vvpMapPointMatches[i][j]; sFound.insert(vvpMapPointMatches[i][j]); } else - mCurrentFrame.mvpMapPoints[j] = NULL; + mCurrentFrame.mvpMapPoints[j] = nullptr; } int nGood = Optimizer::PoseOptimization(&mCurrentFrame); @@ -3461,7 +3420,7 @@ bool Tracking::Relocalization() { for (int io = 0; io < mCurrentFrame.N; io++) if (mCurrentFrame.mvbOutlier[io]) - mCurrentFrame.mvpMapPoints[io] = static_cast(NULL); + mCurrentFrame.mvpMapPoints[io] = nullptr; // If few inliers, search by projection in a coarse window and optimize // again @@ -3489,7 +3448,7 @@ bool Tracking::Relocalization() { for (int io = 0; io < mCurrentFrame.N; io++) if (mCurrentFrame.mvbOutlier[io]) - mCurrentFrame.mvpMapPoints[io] = NULL; + mCurrentFrame.mvpMapPoints[io] = nullptr; } } } @@ -3536,8 +3495,7 @@ void Tracking::Reset(bool bLocMap) { // Clear Map (this erase MapPoints and KeyFrames) mpAtlas->clearAtlas(); mpAtlas->CreateNewMap(); - if (mSensor == CameraType::IMU_STEREO || mSensor == CameraType::IMU_MONOCULAR || - mSensor == CameraType::IMU_RGBD) + if (CameraType::isInertial(mSensor)) mpAtlas->SetInertialSensor(); mnInitialFrameId = 0; @@ -3545,7 +3503,7 @@ void Tracking::Reset(bool bLocMap) { Frame::nNextId = 0; mState = Tracker::NO_IMAGES_YET; - mbReadyToInitializate = false; + mbReadyToInitialize = false; mbSetInit = false; mlRelativeFramePoses.clear(); @@ -3555,8 +3513,8 @@ void Tracking::Reset(bool bLocMap) { mCurrentFrame = Frame(); mnLastRelocFrameId = 0; mLastFrame = Frame(); - mpReferenceKF = static_cast(NULL); - mpLastKeyFrame = static_cast(NULL); + mpReferenceKF = nullptr; + mpLastKeyFrame = nullptr; mvIniMatches.clear(); Verbose::PrintMess(" End reseting! ", Verbose::VERBOSITY_NORMAL); @@ -3565,7 +3523,7 @@ void Tracking::Reset(bool bLocMap) { void Tracking::ResetActiveMap(bool bLocMap) { Verbose::PrintMess("Active map Reseting", Verbose::VERBOSITY_NORMAL); - Map* pMap = mpAtlas->GetCurrentMap(); + std::shared_ptr pMap = mpAtlas->GetCurrentMap(); if (!bLocMap) { Verbose::PrintMess("Reseting Local Mapper...", @@ -3593,13 +3551,13 @@ void Tracking::ResetActiveMap(bool bLocMap) { // mnLastRelocFrameId = mnLastInitFrameId; mState = Tracker::NO_IMAGES_YET; // Tracker::NOT_INITIALIZED; - mbReadyToInitializate = false; + mbReadyToInitialize = false; list lbLost; // lbLost.reserve(mlbLost.size()); unsigned int index = mnFirstFrameId; cout << "mnFirstFrameId = " << mnFirstFrameId << endl; - for (Map* pMap : mpAtlas->GetAllMaps()) { + for (std::shared_ptr pMap : mpAtlas->GetAllMaps()) { if (pMap->GetAllKeyFrames().size() > 0) { if (index > pMap->GetLowerKFID()) index = pMap->GetLowerKFID(); } @@ -3629,8 +3587,8 @@ void Tracking::ResetActiveMap(bool bLocMap) { mCurrentFrame = Frame(); mLastFrame = Frame(); - mpReferenceKF = static_cast(NULL); - mpLastKeyFrame = static_cast(NULL); + mpReferenceKF = nullptr; + mpLastKeyFrame = nullptr; mvIniMatches.clear(); mbVelocity = false; @@ -3681,9 +3639,9 @@ void Tracking::InformOnlyTracking(const bool& flag) { mbOnlyTracking = flag; } void Tracking::UpdateFrameIMU(const float s, const IMU::Bias& b, KeyFrame* pCurrentKeyFrame) { - Map* pMap = pCurrentKeyFrame->GetMap(); + std::shared_ptr pMap = pCurrentKeyFrame->GetMap(); // unsigned int index = mnFirstFrameId; // UNUSED - list::iterator lRit = mlpReferences.begin(); + list::iterator lRit = mlpReferences.begin(); list::iterator lbL = mlbLost.begin(); for (auto lit = mlRelativeFramePoses.begin(), lend = mlRelativeFramePoses.end(); @@ -3765,7 +3723,7 @@ void Tracking::SaveSubTrajectory(string strNameFile_frames, } void Tracking::SaveSubTrajectory(string strNameFile_frames, - string strNameFile_kf, Map* pMap) { + string strNameFile_kf, std::shared_ptr pMap) { mpSystem->SaveTrajectoryEuRoC(strNameFile_frames, pMap); if (!strNameFile_kf.empty()) mpSystem->SaveKeyFrameTrajectoryEuRoC(strNameFile_kf, pMap); @@ -3807,4 +3765,4 @@ void Tracking::Release() { } #endif -} // namespace ORB_SLAM3 +} // namespace MORB_SLAM diff --git a/src/TwoViewReconstruction.cc b/src/TwoViewReconstruction.cc index 36f603ae..26f97968 100644 --- a/src/TwoViewReconstruction.cc +++ b/src/TwoViewReconstruction.cc @@ -19,16 +19,16 @@ * ORB-SLAM3. If not, see . */ -#include "TwoViewReconstruction.h" +#include "MORB_SLAM/TwoViewReconstruction.h" #include -#include "Converter.h" +#include "MORB_SLAM/Converter.h" #include "DUtils/Random.h" -#include "GeometricTools.h" +#include "MORB_SLAM/GeometricTools.h" using namespace std; -namespace ORB_SLAM3 { +namespace MORB_SLAM { TwoViewReconstruction::TwoViewReconstruction(const Eigen::Matrix3f &k, float sigma, int iterations) { mK = k; @@ -904,4 +904,4 @@ void TwoViewReconstruction::DecomposeE(const Eigen::Matrix3f &E, if (R2.determinant() < 0) R2 = -R2; } -} // namespace ORB_SLAM3 +} // namespace MORB_SLAM diff --git a/src/Viewer.cc b/src/Viewer.cc index d7a1fa55..76c09cf5 100644 --- a/src/Viewer.cc +++ b/src/Viewer.cc @@ -19,22 +19,22 @@ * ORB-SLAM3. If not, see . */ -#include "Viewer.h" +#include "MORB_SLAM/Viewer.h" #include -#include "ImprovedTypes.hpp" +#include "MORB_SLAM/ImprovedTypes.hpp" #include #include #include #include #include #include -#include "System.h" -#include "Atlas.h" -#include "Tracking.h" +#include "MORB_SLAM/System.h" +#include "MORB_SLAM/Atlas.h" +#include "MORB_SLAM/Tracking.h" -namespace ORB_SLAM3 { +namespace MORB_SLAM { void Viewer::setBoth(const bool b){ both = true; @@ -367,4 +367,4 @@ void Viewer::close(){ mbClosed = true; } -} // namespace ORB_SLAM3 +} // namespace MORB_SLAM