i/rtabmap
1
0
Fork 0
mirror of https://github.com/introlab/rtabmap.git synced 2026-09-02 17:40:23 +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
corelib/src/CMakeLists.txt
View File

@@ -209,6 +209,7 @@ IF(WITH_PYTHON AND Python3_FOUND)
${PUBLIC_LIBRARIES}
Python3::Python
Python3::NumPy
pybind11::embed
)
SET(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();
// raw data are not kept in database
_memoryUsedEstimate -= (*i)->sensorData().imageRaw().total() * (*i)->sensorData().imageRaw().elemSize();
_memoryUsedEstimate -= (*i)->sensorData().depthOrRightRaw().total() * (*i)->sensorData().depthOrRightRaw().elemSize();
_memoryUsedEstimate -= (*i)->sensorData().laserScanRaw().data().total() * (*i)->sensorData().laserScanRaw().data().elemSize();
_memoryUsedEstimate -= (*i)->sensorData().imageRaw().empty()?0:(*i)->sensorData().imageRaw().total() * (*i)->sensorData().imageRaw().elemSize();
_memoryUsedEstimate -= (*i)->sensorData().depthOrRightRaw().empty()?0:(*i)->sensorData().depthOrRightRaw().total() * (*i)->sensorData().depthOrRightRaw().elemSize();
_memoryUsedEstimate -= (*i)->sensorData().laserScanRaw().empty()?0:(*i)->sensorData().laserScanRaw().data().total() * (*i)->sensorData().laserScanRaw().data().elemSize();
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
{
return sizeof(SensorData) +
_imageCompressed.total()*_imageCompressed.elemSize() +
_imageRaw.total()*_imageRaw.elemSize() +
_depthOrRightCompressed.total()*_depthOrRightCompressed.elemSize() +
_depthOrRightRaw.total()*_depthOrRightRaw.elemSize() +
_userDataCompressed.total()*_userDataCompressed.elemSize() +
_userDataRaw.total()*_userDataRaw.elemSize() +
_laserScanCompressed.data().total()*_laserScanCompressed.data().elemSize() +
_laserScanRaw.data().total()*_laserScanRaw.data().elemSize() +
_groundCellsCompressed.total()*_groundCellsCompressed.elemSize() +
_groundCellsRaw.total()*_groundCellsRaw.elemSize() +
_obstacleCellsCompressed.total()*_obstacleCellsCompressed.elemSize() +
_obstacleCellsRaw.total()*_obstacleCellsRaw.elemSize()+
_emptyCellsCompressed.total()*_emptyCellsCompressed.elemSize() +
_emptyCellsRaw.total()*_emptyCellsRaw.elemSize()+
(_imageCompressed.empty()?0:_imageCompressed.total()*_imageCompressed.elemSize()) +
(_imageRaw.empty()?0:_imageRaw.total()*_imageRaw.elemSize()) +
(_depthOrRightCompressed.empty()?0:_depthOrRightCompressed.total()*_depthOrRightCompressed.elemSize()) +
(_depthOrRightRaw.empty()?0:_depthOrRightRaw.total()*_depthOrRightRaw.elemSize()) +
(_userDataCompressed.empty()?0:_userDataCompressed.total()*_userDataCompressed.elemSize()) +
(_userDataRaw.empty()?0:_userDataRaw.total()*_userDataRaw.elemSize()) +
(_laserScanCompressed.empty()?0:_laserScanCompressed.data().total()*_laserScanCompressed.data().elemSize()) +
(_laserScanRaw.empty()?0:_laserScanRaw.data().total()*_laserScanRaw.data().elemSize()) +
(_groundCellsCompressed.empty()?0:_groundCellsCompressed.total()*_groundCellsCompressed.elemSize()) +
(_groundCellsRaw.empty()?0:_groundCellsRaw.total()*_groundCellsRaw.elemSize()) +
(_obstacleCellsCompressed.empty()?0:_obstacleCellsCompressed.total()*_obstacleCellsCompressed.elemSize()) +
(_obstacleCellsRaw.empty()?0:_obstacleCellsRaw.total()*_obstacleCellsRaw.elemSize())+
(_emptyCellsCompressed.empty()?0:_emptyCellsCompressed.total()*_emptyCellsCompressed.elemSize()) +
(_emptyCellsRaw.empty()?0:_emptyCellsRaw.total()*_emptyCellsRaw.elemSize())+
_keypoints.size() * sizeof(cv::KeyPoint) +
_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)

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 += _wordsKpts.size() * sizeof(cv::KeyPoint) + sizeof(std::vector<cv::KeyPoint>);
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>);
if(withSensorData)
{

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

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

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

@@ -18,7 +18,7 @@
namespace rtabmap
{
class PyDetector : public Feature2D, public PythonInterface
class PyDetector : public Feature2D
{
public:
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/UTimer.h>
#include <pybind11/embed.h>
#define NPY_NO_DEPRECATED_API NPY_API_VERSION
#include <numpy/arrayobject.h>
@@ -39,7 +41,7 @@ PyMatcher::PyMatcher(
return;
}
lock();
pybind11::gil_scoped_acquire acquire;
std::string matcherPythonDir = UDirectory::getDir(path_);
if(!matcherPythonDir.empty())
@@ -59,15 +61,13 @@ PyMatcher::PyMatcher(
if(!pModule_)
{
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()
{
lock();
pybind11::gil_scoped_acquire acquire;
if(pFunc_)
{
Py_DECREF(pFunc_);
@@ -76,7 +76,6 @@ PyMatcher::~PyMatcher()
{
Py_DECREF(pModule_);
}
unlock();
}
std::vector<cv::DMatch> PyMatcher::match(
@@ -104,7 +103,7 @@ std::vector<cv::DMatch> PyMatcher::match(
imageSize.width>0 && imageSize.height>0)
{
lock();
pybind11::gil_scoped_acquire acquire;
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)
{
UERROR("Call to \"init(...)\" in \"%s\" failed!", path_.c_str());
UERROR("%s", getTraceback().c_str());
UERROR("%s", getPythonTraceback().c_str());
return matches;
}
Py_DECREF(result);
@@ -133,7 +132,7 @@ std::vector<cv::DMatch> PyMatcher::match(
else
{
UERROR("Cannot find method \"match(...)\" in %s", path_.c_str());
UERROR("%s", getTraceback().c_str());
UERROR("%s", getPythonTraceback().c_str());
if(pFunc_)
{
Py_DECREF(pFunc_);
@@ -145,7 +144,7 @@ std::vector<cv::DMatch> PyMatcher::match(
else
{
UERROR("Cannot call method \"init(...)\" in %s", path_.c_str());
UERROR("%s", getTraceback().c_str());
UERROR("%s", getPythonTraceback().c_str());
return matches;
}
Py_DECREF(pFunc);
@@ -153,7 +152,7 @@ std::vector<cv::DMatch> PyMatcher::match(
else
{
UERROR("Cannot find method \"init(...)\"");
UERROR("%s", getTraceback().c_str());
UERROR("%s", getPythonTraceback().c_str());
return matches;
}
UDEBUG("init time = %fs", timer.ticks());
@@ -216,7 +215,7 @@ std::vector<cv::DMatch> PyMatcher::match(
if(pReturn == NULL)
{
UERROR("Failed to call match() function!");
UERROR("%s", getTraceback().c_str());
UERROR("%s", getPythonTraceback().c_str());
}
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());
}
unlock();
}
else
{

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

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

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

@@ -8,83 +8,26 @@
#include <rtabmap/core/PythonInterface.h>
#include <rtabmap/utilite/ULogger.h>
#include <rtabmap/utilite/UThread.h>
#include <pybind11/embed.h>
namespace rtabmap {
UMutex PythonInterface::mutex_;
int PythonInterface::refCount_ = 0;
PyThreadState * PythonInterface::mainThreadState_ = 0;
unsigned long PythonInterface::mainThreadID_ = 0;
PythonInterface::PythonInterface() :
threadState_(0)
PythonInterface::PythonInterface()
{
UScopeMutex lockM(mutex_);
if(refCount_ == 0)
{
UINFO("Py_Initialize() with thread = %d", UThread::currentThreadId());
// 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_;
UINFO("Initialize python interpreter");
guard_ = new pybind11::scoped_interpreter();
pybind11::module::import("threading");
release_ = new pybind11::gil_scoped_release();
}
PythonInterface::~PythonInterface()
{
UScopeMutex lock(mutex_);
if(refCount_>0 && --refCount_==0)
{
// shut down the interpreter
UINFO("Py_Finalize() with thread = %d", UThread::currentThreadId());
PyEval_RestoreThread(mainThreadState_);
Py_Finalize();
}
UINFO("Finalize python interpreter");
delete release_;
delete guard_;
}
void PythonInterface::lock()
{
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()
std::string getPythonTraceback()
{
// Author: https://stackoverflow.com/questions/41268061/c-c-python-exception-traceback-not-being-generated