diff --git a/orb_slam2/include/System.h b/orb_slam2/include/System.h index 37148b4f..3eff8969 100755 --- a/orb_slam2/include/System.h +++ b/orb_slam2/include/System.h @@ -26,6 +26,7 @@ #include #include #include +#include #include "Tracking.h" #include "FrameDrawer.h" @@ -129,6 +130,10 @@ class System std::vector GetAllMapPoints(); private: + bool SetCallStackSize (const rlim_t kNewStackSize); + + rlim_t GetCurrentCallStackSize (); + // This stops local mapping thread (map building) and performs only camera tracking. void ActivateLocalizationMode(); diff --git a/orb_slam2/src/System.cc b/orb_slam2/src/System.cc index 38f6f580..1eb30a5f 100644 --- a/orb_slam2/src/System.cc +++ b/orb_slam2/src/System.cc @@ -497,6 +497,49 @@ std::vector System::GetAllMapPoints() { return mpMap->GetAllMapPoints(); } + +bool System::SetCallStackSize (const rlim_t kNewStackSize) { + struct rlimit rlimit; + int operation_result; + + operation_result = getrlimit(RLIMIT_STACK, &rlimit); + if (operation_result != 0) { + std::cerr << "Error getting the call stack struct" << std::endl; + return false; + } + + if (kNewStackSize > rlimit.rlim_max) { + std::cerr << "Requested call stack size too large" << std::endl; + return false; + } + + if (rlimit.rlim_cur < kNewStackSize) { + rlimit.rlim_cur = kNewStackSize; + operation_result = setrlimit(RLIMIT_STACK, &rlimit); + if (operation_result != 0) { + std::cerr << "Setrlimit returned result: " << operation_result << std::endl; + return false; + } + return true; + } + return false; +} + + +rlim_t System::GetCurrentCallStackSize () { + struct rlimit rlimit; + int operation_result; + + operation_result = getrlimit(RLIMIT_STACK, &rlimit); + if (operation_result != 0) { + std::cerr << "Error getting the call stack struct" << std::endl; + return 16 * 1024L * 1024L; //default + } + + return rlimit.rlim_cur; +} + + void System::ActivateLocalizationMode() { currently_localizing_only_ = true; @@ -527,7 +570,6 @@ void System::EnableLocalizationOnly (bool localize_only) { // map serialization addition bool System::SaveMap(const string &filename) { - unique_lockMapPointGlobal(MapPoint::mGlobalMutex); std::ofstream out(filename, std::ios_base::binary); if (!out) { @@ -535,6 +577,13 @@ bool System::SaveMap(const string &filename) { return false; } + const rlim_t kNewStackSize = 64L * 1024L * 1024L; // min stack size = 64 Mb + const rlim_t kDefaultCallStackSize = GetCurrentCallStackSize(); + if (!SetCallStackSize(kNewStackSize)) { + std::cerr << "Error changing the call stack size; Aborting" << std::endl; + return false; + } + try { std::cout << "saving map file: " << map_file << std::flush; boost::archive::binary_oarchive oa(out, boost::archive::no_header); @@ -542,15 +591,17 @@ bool System::SaveMap(const string &filename) { oa << mpKeyFrameDatabase; std::cout << " ... done" << std::endl; out.close(); - } catch (const std::exception &e) { std::cerr << e.what() << std::endl; + SetCallStackSize(kDefaultCallStackSize); return false; } catch (...) { std::cerr << "Unknows exeption" << std::endl; + SetCallStackSize(kDefaultCallStackSize); return false; } + SetCallStackSize(kDefaultCallStackSize); return true; } diff --git a/orb_slam2/src/Tracking.cc b/orb_slam2/src/Tracking.cc index 883fecb5..9d8f0dcc 100755 --- a/orb_slam2/src/Tracking.cc +++ b/orb_slam2/src/Tracking.cc @@ -1486,6 +1486,7 @@ bool Tracking::Relocalization() if(!bMatch) { + mCurrentFrame.mTcw = cv::Mat::zeros(0, 0, CV_32F); // this prevents a segfault later (https://github.com/raulmur/ORB_SLAM2/pull/381#issuecomment-337312336) return false; } else