From 83ffd3a95fc50313c3525c34cd5021b9050412e6 Mon Sep 17 00:00:00 2001 From: Muhammad0312 Date: Mon, 1 Apr 2024 23:53:02 +0000 Subject: [PATCH 1/4] Added funtion to get trajectory and updated CMakeLists.txt --- CMakeLists.txt | 40 +++++++++++++++++++++------------------- include/System.h | 2 ++ src/System.cc | 47 +++++++++++++++++++++++++++++++++++++++++++++++ 3 files changed, 70 insertions(+), 19 deletions(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index 016e74354d..90f63f1f90 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -12,21 +12,10 @@ set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -Wall -O3") set(CMAKE_C_FLAGS_RELEASE "${CMAKE_C_FLAGS_RELEASE} -march=native") set(CMAKE_CXX_FLAGS_RELEASE "${CMAKE_CXX_FLAGS_RELEASE} -march=native") -# Check C++11 or C++0x support -include(CheckCXXCompilerFlag) -CHECK_CXX_COMPILER_FLAG("-std=c++11" COMPILER_SUPPORTS_CXX11) -CHECK_CXX_COMPILER_FLAG("-std=c++0x" COMPILER_SUPPORTS_CXX0X) -if(COMPILER_SUPPORTS_CXX11) - set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++11") - add_definitions(-DCOMPILEDWITHC11) - message(STATUS "Using flag -std=c++11.") -elseif(COMPILER_SUPPORTS_CXX0X) - set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++0x") - add_definitions(-DCOMPILEDWITHC0X) - message(STATUS "Using flag -std=c++0x.") -else() - message(FATAL_ERROR "The compiler ${CMAKE_CXX_COMPILER} has no C++11 support. Please use a different C++ compiler.") -endif() +# Compile with C++14 +set(CMAKE_CXX_STANDARD 14) +set(CMAKE_CXX_EXTENSIONS OFF) +add_definitions(-DCOMPILEDWITHC11) LIST(APPEND CMAKE_MODULE_PATH ${PROJECT_SOURCE_DIR}/cmake_modules) @@ -114,13 +103,26 @@ include/Config.h include/Settings.h) add_subdirectory(Thirdparty/g2o) +add_subdirectory(Thirdparty/DBoW2) +# add_subdirectory(Thirdparty/Sophus) + +target_include_directories(${PROJECT_NAME} PUBLIC +${PROJECT_SOURCE_DIR} +${PROJECT_SOURCE_DIR}/include +${PROJECT_SOURCE_DIR}/include/CameraModels +${PROJECT_SOURCE_DIR}/Thirdparty/Sophus +${EIGEN3_INCLUDE_DIR} +${Pangolin_INCLUDE_DIRS} +) -target_link_libraries(${PROJECT_NAME} +target_link_libraries(${PROJECT_NAME} PUBLIC ${OpenCV_LIBS} ${EIGEN3_LIBS} ${Pangolin_LIBRARIES} -${PROJECT_SOURCE_DIR}/Thirdparty/DBoW2/lib/libDBoW2.so -${PROJECT_SOURCE_DIR}/Thirdparty/g2o/lib/libg2o.so +DBoW2 +g2o +# ${PROJECT_SOURCE_DIR}/Thirdparty/DBoW2/lib/libDBoW2.so +# ${PROJECT_SOURCE_DIR}/Thirdparty/g2o/lib/libg2o.so -lboost_serialization -lcrypto ) @@ -130,7 +132,7 @@ if(realsense2_FOUND) include_directories(${PROJECT_NAME} ${realsense_INCLUDE_DIR} ) - target_link_libraries(${PROJECT_NAME} + target_link_libraries(${PROJECT_NAME} PUBLIC ${realsense2_LIBRARY} ) endif() diff --git a/include/System.h b/include/System.h index 872c86e734..3bcbf31c2a 100644 --- a/include/System.h +++ b/include/System.h @@ -140,6 +140,8 @@ class System void Shutdown(); bool isShutDown(); + std::vector GetCameraTrajectory(); + // Save camera trajectory in the TUM RGB-D dataset format. // Only for stereo and RGB-D. This method does not work for monocular. // Call first Shutdown() diff --git a/src/System.cc b/src/System.cc index 60d9c5185a..18090b6aae 100644 --- a/src/System.cc +++ b/src/System.cc @@ -566,6 +566,53 @@ bool System::isShutDown() { return mbShutDown; } + +vector System::GetCameraTrajectory() +{ + vector vpKFs = mpAtlas->GetAllKeyFrames(); + sort(vpKFs.begin(),vpKFs.end(),KeyFrame::lId); + + // Transform all keyframes so that the first keyframe is at the origin. + // After a loop closure the first keyframe might not be at the origin. + Sophus::SE3f Two = vpKFs[0]->GetPoseInverse(); + vector trajectory; + // Frame pose is stored relative to its reference keyframe (which is optimized by BA and pose graph). + // We need to get first the keyframe pose and then concatenate the relative transformation. + // Frames not localized (tracking failure) are not saved. + + // 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 lT = mpTracker->mlFrameTimes.begin(); + list::iterator lbL = mpTracker->mlbLost.begin(); + for(list::iterator lit=mpTracker->mlRelativeFramePoses.begin(), + lend=mpTracker->mlRelativeFramePoses.end();lit!=lend;lit++, lRit++, lT++, lbL++) + { + if(*lbL) + continue; + + KeyFrame* pKF = *lRit; + + Sophus::SE3f Trw; + + // If the reference keyframe was culled, traverse the spanning tree to get a suitable keyframe. + while(pKF->isBad()) + { + Trw = Trw * pKF->mTcp; + pKF = pKF->GetParent(); + } + + Trw = Trw * pKF->GetPose() * Two; + + Sophus::SE3f Tcw = (*lit) * Trw; + Sophus::SE3f Twc = Tcw.inverse(); + + trajectory.push_back(Twc.matrix()); + } + + return trajectory; +} + void System::SaveTrajectoryTUM(const string &filename) { cout << endl << "Saving camera trajectory to " << filename << " ..." << endl; From 7093bc2177270e8d4e2200187c2bd18094acc69e Mon Sep 17 00:00:00 2001 From: Muhammad0312 Date: Thu, 4 Apr 2024 11:24:27 +0000 Subject: [PATCH 2/4] Nothing Significant --- Examples/Monocular/mono_euroc.cc | 1 - 1 file changed, 1 deletion(-) diff --git a/Examples/Monocular/mono_euroc.cc b/Examples/Monocular/mono_euroc.cc index 3a233129be..bfb6dc2881 100644 --- a/Examples/Monocular/mono_euroc.cc +++ b/Examples/Monocular/mono_euroc.cc @@ -94,7 +94,6 @@ int main(int argc, char **argv) int proccIm = 0; for(int ni=0; ni Date: Thu, 4 Apr 2024 16:09:46 +0000 Subject: [PATCH 3/4] Fixed Segmentation fault error --- Examples/Stereo/KITTI00-02.yaml | 2 +- src/Settings.cc | 10 +++++++--- 2 files changed, 8 insertions(+), 4 deletions(-) diff --git a/Examples/Stereo/KITTI00-02.yaml b/Examples/Stereo/KITTI00-02.yaml index 64d5141791..d523a7f7b4 100644 --- a/Examples/Stereo/KITTI00-02.yaml +++ b/Examples/Stereo/KITTI00-02.yaml @@ -57,7 +57,7 @@ Viewer.PointSize: 2.0 Viewer.CameraSize: 0.7 Viewer.CameraLineWidth: 3.0 Viewer.ViewpointX: 0.0 -Viewer.ViewpointY: -100 +Viewer.ViewpointY: -100.0 Viewer.ViewpointZ: -0.1 Viewer.ViewpointF: 2000.0 diff --git a/src/Settings.cc b/src/Settings.cc index 93a50be626..a7c9f47adb 100644 --- a/src/Settings.cc +++ b/src/Settings.cc @@ -558,11 +558,15 @@ namespace ORB_SLAM3 { else{ output << "Kannala-Brandt"; } - output << "" << ": ["; - for(size_t i = 0; i < settings.originalCalib2_->size(); i++){ + + if (settings.cameraType_ != Settings::Rectified){ + output << "" << ": ["; + for(size_t i = 0; i < settings.originalCalib2_->size(); i++){ output << " " << settings.originalCalib2_->getParameter(i); + } + output << " ]" << endl; } - output << " ]" << endl; + if(!settings.vPinHoleDistorsion2_.empty()){ output << "\t-Camera 1 distortion parameters: [ "; From 73af8d039556bccaf30549f163cfea80cefd319f Mon Sep 17 00:00:00 2001 From: Muhammad0312 Date: Thu, 4 Apr 2024 16:21:03 +0000 Subject: [PATCH 4/4] Fixed Segmentation fault for KITTI dataset --- CMakeLists.txt | 42 ++++++++++++++++++++---------------------- include/System.h | 4 +--- src/System.cc | 48 ------------------------------------------------ 3 files changed, 21 insertions(+), 73 deletions(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index 90f63f1f90..ea3ce2d48c 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -12,10 +12,21 @@ set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -Wall -O3") set(CMAKE_C_FLAGS_RELEASE "${CMAKE_C_FLAGS_RELEASE} -march=native") set(CMAKE_CXX_FLAGS_RELEASE "${CMAKE_CXX_FLAGS_RELEASE} -march=native") -# Compile with C++14 -set(CMAKE_CXX_STANDARD 14) -set(CMAKE_CXX_EXTENSIONS OFF) -add_definitions(-DCOMPILEDWITHC11) +# Check C++11 or C++0x support +include(CheckCXXCompilerFlag) +CHECK_CXX_COMPILER_FLAG("-std=c++11" COMPILER_SUPPORTS_CXX11) +CHECK_CXX_COMPILER_FLAG("-std=c++0x" COMPILER_SUPPORTS_CXX0X) +if(COMPILER_SUPPORTS_CXX11) + set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++11") + add_definitions(-DCOMPILEDWITHC11) + message(STATUS "Using flag -std=c++11.") +elseif(COMPILER_SUPPORTS_CXX0X) + set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++0x") + add_definitions(-DCOMPILEDWITHC0X) + message(STATUS "Using flag -std=c++0x.") +else() + message(FATAL_ERROR "The compiler ${CMAKE_CXX_COMPILER} has no C++11 support. Please use a different C++ compiler.") +endif() LIST(APPEND CMAKE_MODULE_PATH ${PROJECT_SOURCE_DIR}/cmake_modules) @@ -103,26 +114,13 @@ include/Config.h include/Settings.h) add_subdirectory(Thirdparty/g2o) -add_subdirectory(Thirdparty/DBoW2) -# add_subdirectory(Thirdparty/Sophus) - -target_include_directories(${PROJECT_NAME} PUBLIC -${PROJECT_SOURCE_DIR} -${PROJECT_SOURCE_DIR}/include -${PROJECT_SOURCE_DIR}/include/CameraModels -${PROJECT_SOURCE_DIR}/Thirdparty/Sophus -${EIGEN3_INCLUDE_DIR} -${Pangolin_INCLUDE_DIRS} -) -target_link_libraries(${PROJECT_NAME} PUBLIC +target_link_libraries(${PROJECT_NAME} ${OpenCV_LIBS} ${EIGEN3_LIBS} ${Pangolin_LIBRARIES} -DBoW2 -g2o -# ${PROJECT_SOURCE_DIR}/Thirdparty/DBoW2/lib/libDBoW2.so -# ${PROJECT_SOURCE_DIR}/Thirdparty/g2o/lib/libg2o.so +${PROJECT_SOURCE_DIR}/Thirdparty/DBoW2/lib/libDBoW2.so +${PROJECT_SOURCE_DIR}/Thirdparty/g2o/lib/libg2o.so -lboost_serialization -lcrypto ) @@ -132,7 +130,7 @@ if(realsense2_FOUND) include_directories(${PROJECT_NAME} ${realsense_INCLUDE_DIR} ) - target_link_libraries(${PROJECT_NAME} PUBLIC + target_link_libraries(${PROJECT_NAME} ${realsense2_LIBRARY} ) endif() @@ -389,4 +387,4 @@ if(realsense2_FOUND) add_executable(stereo_inertial_realsense_D435i_old Examples_old/Stereo-Inertial/stereo_inertial_realsense_D435i.cc) target_link_libraries(stereo_inertial_realsense_D435i_old ${PROJECT_NAME}) -endif() +endif() \ No newline at end of file diff --git a/include/System.h b/include/System.h index 3bcbf31c2a..31797f4ba4 100644 --- a/include/System.h +++ b/include/System.h @@ -140,8 +140,6 @@ class System void Shutdown(); bool isShutDown(); - std::vector GetCameraTrajectory(); - // Save camera trajectory in the TUM RGB-D dataset format. // Only for stereo and RGB-D. This method does not work for monocular. // Call first Shutdown() @@ -268,4 +266,4 @@ class System }// namespace ORB_SLAM -#endif // SYSTEM_H +#endif // SYSTEM_H \ No newline at end of file diff --git a/src/System.cc b/src/System.cc index 18090b6aae..dafb4cb6aa 100644 --- a/src/System.cc +++ b/src/System.cc @@ -566,53 +566,6 @@ bool System::isShutDown() { return mbShutDown; } - -vector System::GetCameraTrajectory() -{ - vector vpKFs = mpAtlas->GetAllKeyFrames(); - sort(vpKFs.begin(),vpKFs.end(),KeyFrame::lId); - - // Transform all keyframes so that the first keyframe is at the origin. - // After a loop closure the first keyframe might not be at the origin. - Sophus::SE3f Two = vpKFs[0]->GetPoseInverse(); - vector trajectory; - // Frame pose is stored relative to its reference keyframe (which is optimized by BA and pose graph). - // We need to get first the keyframe pose and then concatenate the relative transformation. - // Frames not localized (tracking failure) are not saved. - - // 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 lT = mpTracker->mlFrameTimes.begin(); - list::iterator lbL = mpTracker->mlbLost.begin(); - for(list::iterator lit=mpTracker->mlRelativeFramePoses.begin(), - lend=mpTracker->mlRelativeFramePoses.end();lit!=lend;lit++, lRit++, lT++, lbL++) - { - if(*lbL) - continue; - - KeyFrame* pKF = *lRit; - - Sophus::SE3f Trw; - - // If the reference keyframe was culled, traverse the spanning tree to get a suitable keyframe. - while(pKF->isBad()) - { - Trw = Trw * pKF->mTcp; - pKF = pKF->GetParent(); - } - - Trw = Trw * pKF->GetPose() * Two; - - Sophus::SE3f Tcw = (*lit) * Trw; - Sophus::SE3f Twc = Tcw.inverse(); - - trajectory.push_back(Twc.matrix()); - } - - return trajectory; -} - void System::SaveTrajectoryTUM(const string &filename) { cout << endl << "Saving camera trajectory to " << filename << " ..." << endl; @@ -1593,4 +1546,3 @@ string System::CalculateCheckSum(string filename, int type) } } //namespace ORB_SLAM -