i/rtabmap
1
0
Fork 0
mirror of https://github.com/introlab/rtabmap.git synced 2026-09-03 01:50:24 +08:00
Code Issues Packages Projects Releases Wiki Activity

Fixed superglue deadlock on standalone (#896). Fixed some elemSize opencv asserts in debug build (876)

Browse Source
This commit is contained in:
matlabbe
2023-09-17 01:19:59 -07:00
parent fb466a6a96
commit f1cd819673
11 changed files with 68 additions and 129 deletions
Download Patch File Download Diff File Expand all files Collapse all files

1
CMakeLists.txt
View File

@@ -392,6 +392,7 @@ IF(WITH_PYTHON)
FIND_PACKAGE(Python3 COMPONENTS Interpreter Development NumPy) FIND_PACKAGE(Python3 COMPONENTS Interpreter Development NumPy)
IF(Python3_FOUND) IF(Python3_FOUND)
MESSAGE(STATUS "Found Python3") MESSAGE(STATUS "Found Python3")
FIND_PACKAGE(pybind11 REQUIRED)
ENDIF(Python3_FOUND) ENDIF(Python3_FOUND)
ENDIF(WITH_PYTHON) ENDIF(WITH_PYTHON)

26
corelib/include/rtabmap/core/PythonInterface.h
View File

@@ -11,31 +11,31 @@
#include <string> #include <string>
#include <rtabmap/utilite/UMutex.h> #include <rtabmap/utilite/UMutex.h>
#include <Python.h>
namespace pybind11 {
class scoped_interpreter;
class gil_scoped_release;
}
namespace rtabmap { namespace rtabmap {
/**
* Create a single PythonInterface on main thread at
* global scope before any Python classes.
*/
class PythonInterface class PythonInterface
{ {
public: public:
PythonInterface(); PythonInterface();
virtual ~PythonInterface(); virtual ~PythonInterface();
protected:
std::string getTraceback(); // should be called between lock() and unlock()
void lock();
void unlock();
private: private:
static UMutex mutex_; pybind11::scoped_interpreter* guard_;
static int refCount_; pybind11::gil_scoped_release* release_;
protected:
static PyThreadState * mainThreadState_;
static unsigned long mainThreadID_;
PyThreadState * threadState_;
}; };
std::string getPythonTraceback();
} }
#endif /* CORELIB_SRC_PYTHON_PYTHONINTERFACE_H_ */ #endif /* CORELIB_SRC_PYTHON_PYTHONINTERFACE_H_ */

1
corelib/src/CMakeLists.txt
View File

@@ -209,6 +209,7 @@ IF(WITH_PYTHON AND Python3_FOUND)
${PUBLIC_LIBRARIES} ${PUBLIC_LIBRARIES}
Python3::Python Python3::Python
Python3::NumPy Python3::NumPy
pybind11::embed
) )
SET(SRC_FILES SET(SRC_FILES
${SRC_FILES} ${SRC_FILES}

6
corelib/src/DBDriverSqlite3.cpp
View File

@@ -4298,9 +4298,9 @@ void DBDriverSqlite3::saveQuery(const std::list<Signature *> & signatures)
{ {
_memoryUsedEstimate += (*i)->getMemoryUsed(); _memoryUsedEstimate += (*i)->getMemoryUsed();
// raw data are not kept in database // raw data are not kept in database
_memoryUsedEstimate -= (*i)->sensorData().imageRaw().total() * (*i)->sensorData().imageRaw().elemSize(); _memoryUsedEstimate -= (*i)->sensorData().imageRaw().empty()?0:(*i)->sensorData().imageRaw().total() * (*i)->sensorData().imageRaw().elemSize();
_memoryUsedEstimate -= (*i)->sensorData().depthOrRightRaw().total() * (*i)->sensorData().depthOrRightRaw().elemSize(); _memoryUsedEstimate -= (*i)->sensorData().depthOrRightRaw().empty()?0:(*i)->sensorData().depthOrRightRaw().total() * (*i)->sensorData().depthOrRightRaw().elemSize();
_memoryUsedEstimate -= (*i)->sensorData().laserScanRaw().data().total() * (*i)->sensorData().laserScanRaw().data().elemSize(); _memoryUsedEstimate -= (*i)->sensorData().laserScanRaw().empty()?0:(*i)->sensorData().laserScanRaw().data().total() * (*i)->sensorData().laserScanRaw().data().elemSize();
stepNode(ppStmt, *i); stepNode(ppStmt, *i);
} }

30
corelib/src/SensorData.cpp
View File

@@ -810,23 +810,23 @@ void SensorData::setFeatures(const std::vector<cv::KeyPoint> & keypoints, const
unsigned long SensorData::getMemoryUsed() const // Return memory usage in Bytes unsigned long SensorData::getMemoryUsed() const // Return memory usage in Bytes
{ {
return sizeof(SensorData) + return sizeof(SensorData) +
_imageCompressed.total()*_imageCompressed.elemSize() + (_imageCompressed.empty()?0:_imageCompressed.total()*_imageCompressed.elemSize()) +
_imageRaw.total()*_imageRaw.elemSize() + (_imageRaw.empty()?0:_imageRaw.total()*_imageRaw.elemSize()) +
_depthOrRightCompressed.total()*_depthOrRightCompressed.elemSize() + (_depthOrRightCompressed.empty()?0:_depthOrRightCompressed.total()*_depthOrRightCompressed.elemSize()) +
_depthOrRightRaw.total()*_depthOrRightRaw.elemSize() + (_depthOrRightRaw.empty()?0:_depthOrRightRaw.total()*_depthOrRightRaw.elemSize()) +
_userDataCompressed.total()*_userDataCompressed.elemSize() + (_userDataCompressed.empty()?0:_userDataCompressed.total()*_userDataCompressed.elemSize()) +
_userDataRaw.total()*_userDataRaw.elemSize() + (_userDataRaw.empty()?0:_userDataRaw.total()*_userDataRaw.elemSize()) +
_laserScanCompressed.data().total()*_laserScanCompressed.data().elemSize() + (_laserScanCompressed.empty()?0:_laserScanCompressed.data().total()*_laserScanCompressed.data().elemSize()) +
_laserScanRaw.data().total()*_laserScanRaw.data().elemSize() + (_laserScanRaw.empty()?0:_laserScanRaw.data().total()*_laserScanRaw.data().elemSize()) +
_groundCellsCompressed.total()*_groundCellsCompressed.elemSize() + (_groundCellsCompressed.empty()?0:_groundCellsCompressed.total()*_groundCellsCompressed.elemSize()) +
_groundCellsRaw.total()*_groundCellsRaw.elemSize() + (_groundCellsRaw.empty()?0:_groundCellsRaw.total()*_groundCellsRaw.elemSize()) +
_obstacleCellsCompressed.total()*_obstacleCellsCompressed.elemSize() + (_obstacleCellsCompressed.empty()?0:_obstacleCellsCompressed.total()*_obstacleCellsCompressed.elemSize()) +
_obstacleCellsRaw.total()*_obstacleCellsRaw.elemSize()+ (_obstacleCellsRaw.empty()?0:_obstacleCellsRaw.total()*_obstacleCellsRaw.elemSize())+
_emptyCellsCompressed.total()*_emptyCellsCompressed.elemSize() + (_emptyCellsCompressed.empty()?0:_emptyCellsCompressed.total()*_emptyCellsCompressed.elemSize()) +
_emptyCellsRaw.total()*_emptyCellsRaw.elemSize()+ (_emptyCellsRaw.empty()?0:_emptyCellsRaw.total()*_emptyCellsRaw.elemSize())+
_keypoints.size() * sizeof(cv::KeyPoint) + _keypoints.size() * sizeof(cv::KeyPoint) +
_keypoints3D.size() * sizeof(cv::Point3f) + _keypoints3D.size() * sizeof(cv::Point3f) +
_descriptors.total()*_descriptors.elemSize(); (_descriptors.empty()?0:_descriptors.total()*_descriptors.elemSize());
} }
void SensorData::clearCompressedData(bool images, bool scan, bool userData) void SensorData::clearCompressedData(bool images, bool scan, bool userData)

2
corelib/src/Signature.cpp
View File

@@ -348,7 +348,7 @@ unsigned long Signature::getMemoryUsed(bool withSensorData) const // Return memo
total += _words.size() * (sizeof(int)*2+sizeof(std::multimap<int, cv::KeyPoint>::iterator)) + sizeof(std::multimap<int, cv::KeyPoint>); total += _words.size() * (sizeof(int)*2+sizeof(std::multimap<int, cv::KeyPoint>::iterator)) + sizeof(std::multimap<int, cv::KeyPoint>);
total += _wordsKpts.size() * sizeof(cv::KeyPoint) + sizeof(std::vector<cv::KeyPoint>); total += _wordsKpts.size() * sizeof(cv::KeyPoint) + sizeof(std::vector<cv::KeyPoint>);
total += _words3.size() * sizeof(cv::Point3f) + sizeof(std::vector<cv::Point3f>); total += _words3.size() * sizeof(cv::Point3f) + sizeof(std::vector<cv::Point3f>);
total += _wordsDescriptors.total() * _wordsDescriptors.elemSize() + sizeof(cv::Mat); total += _wordsDescriptors.empty()?0:_wordsDescriptors.total() * _wordsDescriptors.elemSize() + sizeof(cv::Mat);
total += _wordsChanged.size() * (sizeof(int)*2+sizeof(std::map<int, int>::iterator)) + sizeof(std::map<int, int>); total += _wordsChanged.size() * (sizeof(int)*2+sizeof(std::map<int, int>::iterator)) + sizeof(std::map<int, int>);
if(withSensorData) if(withSensorData)
{ {

26
corelib/src/python/PyDetector.cpp
View File

@@ -10,6 +10,8 @@
#include <rtabmap/utilite/UConversion.h> #include <rtabmap/utilite/UConversion.h>
#include <rtabmap/utilite/UTimer.h> #include <rtabmap/utilite/UTimer.h>
#include <pybind11/embed.h>
#define NPY_NO_DEPRECATED_API NPY_API_VERSION #define NPY_NO_DEPRECATED_API NPY_API_VERSION
#include <numpy/arrayobject.h> #include <numpy/arrayobject.h>
@@ -32,7 +34,7 @@ PyDetector::PyDetector(const ParametersMap & parameters) :
return; return;
} }
lock(); pybind11::gil_scoped_acquire acquire;
std::string matcherPythonDir = UDirectory::getDir(path_); std::string matcherPythonDir = UDirectory::getDir(path_);
if(!matcherPythonDir.empty()) if(!matcherPythonDir.empty())
@@ -54,15 +56,13 @@ PyDetector::PyDetector(const ParametersMap & parameters) :
if(!pModule_) if(!pModule_)
{ {
UERROR("Module \"%s\" could not be imported! (File=\"%s\")", scriptName.c_str(), path_.c_str()); UERROR("Module \"%s\" could not be imported! (File=\"%s\")", scriptName.c_str(), path_.c_str());
UERROR("%s", getTraceback().c_str()); UERROR("%s", getPythonTraceback().c_str());
} }
unlock();
} }
PyDetector::~PyDetector() PyDetector::~PyDetector()
{ {
lock(); pybind11::gil_scoped_acquire acquire;
if(pFunc_) if(pFunc_)
{ {
@@ -72,8 +72,6 @@ PyDetector::~PyDetector()
{ {
Py_DECREF(pModule_); Py_DECREF(pModule_);
} }
unlock();
} }
void PyDetector::parseParameters(const ParametersMap & parameters) void PyDetector::parseParameters(const ParametersMap & parameters)
@@ -102,7 +100,7 @@ std::vector<cv::KeyPoint> PyDetector::generateKeypointsImpl(const cv::Mat & imag
return keypoints; return keypoints;
} }
lock(); pybind11::gil_scoped_acquire acquire;
if(!pFunc_) if(!pFunc_)
{ {
@@ -116,7 +114,7 @@ std::vector<cv::KeyPoint> PyDetector::generateKeypointsImpl(const cv::Mat & imag
if(result == NULL) if(result == NULL)
{ {
UERROR("Call to \"init(...)\" in \"%s\" failed!", path_.c_str()); UERROR("Call to \"init(...)\" in \"%s\" failed!", path_.c_str());
UERROR("%s", getTraceback().c_str()); UERROR("%s", getPythonTraceback().c_str());
return keypoints; return keypoints;
} }
Py_DECREF(result); Py_DECREF(result);
@@ -129,7 +127,7 @@ std::vector<cv::KeyPoint> PyDetector::generateKeypointsImpl(const cv::Mat & imag
else else
{ {
UERROR("Cannot find method \"detect(...)\" in %s", path_.c_str()); UERROR("Cannot find method \"detect(...)\" in %s", path_.c_str());
UERROR("%s", getTraceback().c_str()); UERROR("%s", getPythonTraceback().c_str());
if(pFunc_) if(pFunc_)
{ {
Py_DECREF(pFunc_); Py_DECREF(pFunc_);
@@ -141,7 +139,7 @@ std::vector<cv::KeyPoint> PyDetector::generateKeypointsImpl(const cv::Mat & imag
else else
{ {
UERROR("Cannot call method \"init(...)\" in %s", path_.c_str()); UERROR("Cannot call method \"init(...)\" in %s", path_.c_str());
UERROR("%s", getTraceback().c_str()); UERROR("%s", getPythonTraceback().c_str());
return keypoints; return keypoints;
} }
Py_DECREF(pFunc); Py_DECREF(pFunc);
@@ -149,7 +147,7 @@ std::vector<cv::KeyPoint> PyDetector::generateKeypointsImpl(const cv::Mat & imag
else else
{ {
UERROR("Cannot find method \"init(...)\""); UERROR("Cannot find method \"init(...)\"");
UERROR("%s", getTraceback().c_str()); UERROR("%s", getPythonTraceback().c_str());
return keypoints; return keypoints;
} }
UDEBUG("init time = %fs", timer.ticks()); UDEBUG("init time = %fs", timer.ticks());
@@ -167,7 +165,7 @@ std::vector<cv::KeyPoint> PyDetector::generateKeypointsImpl(const cv::Mat & imag
if(pReturn == NULL) if(pReturn == NULL)
{ {
UERROR("Failed to call match() function!"); UERROR("Failed to call match() function!");
UERROR("%s", getTraceback().c_str()); UERROR("%s", getPythonTraceback().c_str());
} }
else else
{ {
@@ -220,8 +218,6 @@ std::vector<cv::KeyPoint> PyDetector::generateKeypointsImpl(const cv::Mat & imag
Py_DECREF(pImageBuffer); Py_DECREF(pImageBuffer);
} }
unlock();
return keypoints; return keypoints;
} }

2
corelib/src/python/PyDetector.h
View File

@@ -18,7 +18,7 @@
namespace rtabmap namespace rtabmap
{ {
class PyDetector : public Feature2D, public PythonInterface class PyDetector : public Feature2D
{ {
public: public:
PyDetector(const ParametersMap & parameters = ParametersMap()); PyDetector(const ParametersMap & parameters = ParametersMap());

24
corelib/src/python/PyMatcher.cpp
View File

@@ -10,6 +10,8 @@
#include <rtabmap/utilite/UConversion.h> #include <rtabmap/utilite/UConversion.h>
#include <rtabmap/utilite/UTimer.h> #include <rtabmap/utilite/UTimer.h>
#include <pybind11/embed.h>
#define NPY_NO_DEPRECATED_API NPY_API_VERSION #define NPY_NO_DEPRECATED_API NPY_API_VERSION
#include <numpy/arrayobject.h> #include <numpy/arrayobject.h>
@@ -39,7 +41,7 @@ PyMatcher::PyMatcher(
return; return;
} }
lock(); pybind11::gil_scoped_acquire acquire;
std::string matcherPythonDir = UDirectory::getDir(path_); std::string matcherPythonDir = UDirectory::getDir(path_);
if(!matcherPythonDir.empty()) if(!matcherPythonDir.empty())
@@ -59,15 +61,13 @@ PyMatcher::PyMatcher(
if(!pModule_) if(!pModule_)
{ {
UERROR("Module \"%s\" could not be imported! (File=\"%s\")", scriptName.c_str(), path_.c_str()); UERROR("Module \"%s\" could not be imported! (File=\"%s\")", scriptName.c_str(), path_.c_str());
UERROR("%s", getTraceback().c_str()); UERROR("%s", getPythonTraceback().c_str());
} }
unlock();
} }
PyMatcher::~PyMatcher() PyMatcher::~PyMatcher()
{ {
lock(); pybind11::gil_scoped_acquire acquire;
if(pFunc_) if(pFunc_)
{ {
Py_DECREF(pFunc_); Py_DECREF(pFunc_);
@@ -76,7 +76,6 @@ PyMatcher::~PyMatcher()
{ {
Py_DECREF(pModule_); Py_DECREF(pModule_);
} }
unlock();
} }
std::vector<cv::DMatch> PyMatcher::match( std::vector<cv::DMatch> PyMatcher::match(
@@ -104,7 +103,7 @@ std::vector<cv::DMatch> PyMatcher::match(
imageSize.width>0 && imageSize.height>0) imageSize.width>0 && imageSize.height>0)
{ {
lock(); pybind11::gil_scoped_acquire acquire;
UDEBUG("matchThreshold=%f, iterations=%d, cuda=%d", matchThreshold_, iterations_, cuda_?1:0); UDEBUG("matchThreshold=%f, iterations=%d, cuda=%d", matchThreshold_, iterations_, cuda_?1:0);
@@ -120,7 +119,7 @@ std::vector<cv::DMatch> PyMatcher::match(
if(result == NULL) if(result == NULL)
{ {
UERROR("Call to \"init(...)\" in \"%s\" failed!", path_.c_str()); UERROR("Call to \"init(...)\" in \"%s\" failed!", path_.c_str());
UERROR("%s", getTraceback().c_str()); UERROR("%s", getPythonTraceback().c_str());
return matches; return matches;
} }
Py_DECREF(result); Py_DECREF(result);
@@ -133,7 +132,7 @@ std::vector<cv::DMatch> PyMatcher::match(
else else
{ {
UERROR("Cannot find method \"match(...)\" in %s", path_.c_str()); UERROR("Cannot find method \"match(...)\" in %s", path_.c_str());
UERROR("%s", getTraceback().c_str()); UERROR("%s", getPythonTraceback().c_str());
if(pFunc_) if(pFunc_)
{ {
Py_DECREF(pFunc_); Py_DECREF(pFunc_);
@@ -145,7 +144,7 @@ std::vector<cv::DMatch> PyMatcher::match(
else else
{ {
UERROR("Cannot call method \"init(...)\" in %s", path_.c_str()); UERROR("Cannot call method \"init(...)\" in %s", path_.c_str());
UERROR("%s", getTraceback().c_str()); UERROR("%s", getPythonTraceback().c_str());
return matches; return matches;
} }
Py_DECREF(pFunc); Py_DECREF(pFunc);
@@ -153,7 +152,7 @@ std::vector<cv::DMatch> PyMatcher::match(
else else
{ {
UERROR("Cannot find method \"init(...)\""); UERROR("Cannot find method \"init(...)\"");
UERROR("%s", getTraceback().c_str()); UERROR("%s", getPythonTraceback().c_str());
return matches; return matches;
} }
UDEBUG("init time = %fs", timer.ticks()); UDEBUG("init time = %fs", timer.ticks());
@@ -216,7 +215,7 @@ std::vector<cv::DMatch> PyMatcher::match(
if(pReturn == NULL) if(pReturn == NULL)
{ {
UERROR("Failed to call match() function!"); UERROR("Failed to call match() function!");
UERROR("%s", getTraceback().c_str()); UERROR("%s", getPythonTraceback().c_str());
} }
else else
{ {
@@ -260,7 +259,6 @@ std::vector<cv::DMatch> PyMatcher::match(
UDEBUG("Fill matches (%d/%d) and cleanup time = %fs", matches.size(), std::min(descriptorsQuery.rows, descriptorsTrain.rows), timer.ticks()); UDEBUG("Fill matches (%d/%d) and cleanup time = %fs", matches.size(), std::min(descriptorsQuery.rows, descriptorsTrain.rows), timer.ticks());
} }
unlock();
} }
else else
{ {

2
corelib/src/python/PyMatcher.h
View File

@@ -16,7 +16,7 @@
namespace rtabmap namespace rtabmap
{ {
class PyMatcher : public PythonInterface class PyMatcher
{ {
public: public:
PyMatcher(const std::string & pythonMatcherPath, PyMatcher(const std::string & pythonMatcherPath,

77
corelib/src/python/PythonInterface.cpp
View File

@@ -8,83 +8,26 @@
#include <rtabmap/core/PythonInterface.h> #include <rtabmap/core/PythonInterface.h>
#include <rtabmap/utilite/ULogger.h> #include <rtabmap/utilite/ULogger.h>
#include <rtabmap/utilite/UThread.h> #include <rtabmap/utilite/UThread.h>
#include <pybind11/embed.h>
namespace rtabmap { namespace rtabmap {
UMutex PythonInterface::mutex_; PythonInterface::PythonInterface()
int PythonInterface::refCount_ = 0;
PyThreadState * PythonInterface::mainThreadState_ = 0;
unsigned long PythonInterface::mainThreadID_ = 0;
PythonInterface::PythonInterface() :
threadState_(0)
{ {
UScopeMutex lockM(mutex_); UINFO("Initialize python interpreter");
if(refCount_ == 0) guard_ = new pybind11::scoped_interpreter();
{ pybind11::module::import("threading");
UINFO("Py_Initialize() with thread = %d", UThread::currentThreadId()); release_ = new pybind11::gil_scoped_release();
// initialize Python
Py_Initialize();
// initialize thread support
PyEval_InitThreads();
Py_DECREF(PyImport_ImportModule("threading"));
//release the GIL, store thread state, set the current thread state to NULL
mainThreadState_ = PyEval_SaveThread();
UASSERT(mainThreadState_);
mainThreadID_ = UThread::currentThreadId();
}
++refCount_;
} }
PythonInterface::~PythonInterface() PythonInterface::~PythonInterface()
{ {
UScopeMutex lock(mutex_); UINFO("Finalize python interpreter");
if(refCount_>0 && --refCount_==0) delete release_;
{ delete guard_;
// shut down the interpreter
UINFO("Py_Finalize() with thread = %d", UThread::currentThreadId());
PyEval_RestoreThread(mainThreadState_);
Py_Finalize();
}
} }
void PythonInterface::lock() std::string getPythonTraceback()
{
mutex_.lock();
UDEBUG("Lock: Current thread=%d (main=%d)", UThread::currentThreadId(), mainThreadID_);
if(UThread::currentThreadId() == mainThreadID_)
{
PyEval_RestoreThread(mainThreadState_);
}
else
{
// create a thread state object for this thread
threadState_ = PyThreadState_New(mainThreadState_->interp);
UASSERT(threadState_);
PyEval_RestoreThread(threadState_);
}
}
void PythonInterface::unlock()
{
if(UThread::currentThreadId() == mainThreadID_)
{
mainThreadState_ = PyEval_SaveThread();
}
else
{
PyThreadState_Clear(threadState_);
PyThreadState_DeleteCurrent();
}
UDEBUG("Unlock: Current thread=%d (main=%d)", UThread::currentThreadId(), mainThreadID_);
mutex_.unlock();
}
std::string PythonInterface::getTraceback()
{ {
// Author: https://stackoverflow.com/questions/41268061/c-c-python-exception-traceback-not-being-generated // Author: https://stackoverflow.com/questions/41268061/c-c-python-exception-traceback-not-being-generated