PyDetector.cpp
Go to the documentation of this file.
1 
5 #include "PyDetector.h"
6 #include <rtabmap/utilite/ULogger.h>
7 #include <rtabmap/utilite/UDirectory.h>
8 #include <rtabmap/utilite/UFile.h>
9 #include <rtabmap/utilite/UStl.h>
10 #include <rtabmap/utilite/UConversion.h>
11 #include <rtabmap/utilite/UTimer.h>
12 
13 #define NPY_NO_DEPRECATED_API NPY_API_VERSION
14 #include <numpy/arrayobject.h>
15 
16 namespace rtabmap
17 {
18 
19 PyDetector::PyDetector(const ParametersMap & parameters) :
20  pModule_(0),
21  pFunc_(0),
22  path_(Parameters::defaultPyDetectorPath()),
23  cuda_(Parameters::defaultPyDetectorCuda())
24 {
25  this->parseParameters(parameters);
26 
27  UDEBUG("path = %s", path_.c_str());
28  if(!UFile::exists(path_) || UFile::getExtension(path_).compare("py") != 0)
29  {
30  UERROR("Cannot initialize Python detector, the path is not valid: \"%s\"=\"%s\"",
31  Parameters::kPyDetectorPath().c_str(), path_.c_str());
32  return;
33  }
34 
35  lock();
36 
37  std::string matcherPythonDir = UDirectory::getDir(path_);
38  if(!matcherPythonDir.empty())
39  {
40  PyRun_SimpleString("import sys");
41  PyRun_SimpleString(uFormat("sys.path.append(\"%s\")", matcherPythonDir.c_str()).c_str());
42  }
43 
44  _import_array();
45 
46  std::string scriptName = uSplit(UFile::getName(path_), '.').front();
47  PyObject * pName = PyUnicode_FromString(scriptName.c_str());
48  UDEBUG("PyImport_Import() beg");
49  pModule_ = PyImport_Import(pName);
50  UDEBUG("PyImport_Import() end");
51 
52  Py_DECREF(pName);
53 
54  if(!pModule_)
55  {
56  UERROR("Module \"%s\" could not be imported! (File=\"%s\")", scriptName.c_str(), path_.c_str());
57  UERROR("%s", getTraceback().c_str());
58  }
59 
60  unlock();
61 }
62 
63 PyDetector::~PyDetector()
64 {
65  lock();
66 
67  if(pFunc_)
68  {
69  Py_DECREF(pFunc_);
70  }
71  if(pModule_)
72  {
73  Py_DECREF(pModule_);
74  }
75 
76  unlock();
77 }
78 
79 void PyDetector::parseParameters(const ParametersMap & parameters)
80 {
81  Feature2D::parseParameters(parameters);
82 
83  Parameters::parse(parameters, Parameters::kPyDetectorPath(), path_);
84  Parameters::parse(parameters, Parameters::kPyDetectorCuda(), cuda_);
85 
86  path_ = uReplaceChar(path_, '~', UDirectory::homeDir());
87 }
88 
89 std::vector<cv::KeyPoint> PyDetector::generateKeypointsImpl(const cv::Mat & image, const cv::Rect & roi, const cv::Mat & mask)
90 {
91  UDEBUG("");
92  descriptors_ = cv::Mat();
93  UASSERT(!image.empty() && image.channels() == 1 && image.depth() == CV_8U);
94  std::vector<cv::KeyPoint> keypoints;
95  cv::Mat imgRoi(image, roi);
96 
97  UTimer timer;
98 
99  if(!pModule_)
100  {
101  UERROR("Python detector module not loaded!");
102  return keypoints;
103  }
104 
105  lock();
106 
107  if(!pFunc_)
108  {
109  PyObject * pFunc = PyObject_GetAttrString(pModule_, "init");
110  if(pFunc)
111  {
112  if(PyCallable_Check(pFunc))
113  {
114  PyObject * result = PyObject_CallFunction(pFunc, "i", cuda_?1:0);
115 
116  if(result == NULL)
117  {
118  UERROR("Call to \"init(...)\" in \"%s\" failed!", path_.c_str());
119  UERROR("%s", getTraceback().c_str());
120  return keypoints;
121  }
122  Py_DECREF(result);
123 
124  pFunc_ = PyObject_GetAttrString(pModule_, "detect");
125  if(pFunc_ && PyCallable_Check(pFunc_))
126  {
127  // we are ready!
128  }
129  else
130  {
131  UERROR("Cannot find method \"detect(...)\" in %s", path_.c_str());
132  UERROR("%s", getTraceback().c_str());
133  if(pFunc_)
134  {
135  Py_DECREF(pFunc_);
136  pFunc_ = 0;
137  }
138  return keypoints;
139  }
140  }
141  else
142  {
143  UERROR("Cannot call method \"init(...)\" in %s", path_.c_str());
144  UERROR("%s", getTraceback().c_str());
145  return keypoints;
146  }
147  Py_DECREF(pFunc);
148  }
149  else
150  {
151  UERROR("Cannot find method \"init(...)\"");
152  UERROR("%s", getTraceback().c_str());
153  return keypoints;
154  }
155  UDEBUG("init time = %fs", timer.ticks());
156  }
157 
158  if(pFunc_)
159  {
160  npy_intp dims[2] = {imgRoi.rows, imgRoi.cols};
161  PyObject* pImageBuffer = PyArray_SimpleNewFromData(2, dims, NPY_UBYTE, (void*)imgRoi.data);
162  UASSERT(pImageBuffer);
163 
164  UDEBUG("Preparing data time = %fs", timer.ticks());
165 
166  PyObject *pReturn = PyObject_CallFunctionObjArgs(pFunc_, pImageBuffer, NULL);
167  if(pReturn == NULL)
168  {
169  UERROR("Failed to call match() function!");
170  UERROR("%s", getTraceback().c_str());
171  }
172  else
173  {
174  UDEBUG("Python detector time = %fs", timer.ticks());
175 
176  if (PyTuple_Check(pReturn) && PyTuple_GET_SIZE(pReturn) == 2)
177  {
178  PyObject *kptsPtr = PyTuple_GET_ITEM(pReturn, 0);
179  PyObject *descPtr = PyTuple_GET_ITEM(pReturn, 1);
180  if(PyArray_Check(kptsPtr) && PyArray_Check(descPtr))
181  {
182  PyArrayObject *arrayPtr = reinterpret_cast<PyArrayObject*>(kptsPtr);
183  int nKpts = PyArray_SHAPE(arrayPtr)[0];
184  int kptSize = PyArray_SHAPE(arrayPtr)[1];
185  int type = PyArray_TYPE(arrayPtr);
186  UDEBUG("Kpts array %dx%d (type=%d)", nKpts, kptSize, type);
187  UASSERT(kptSize == 3);
188  UASSERT_MSG(type == NPY_FLOAT, uFormat("Returned matches should type FLOAT=11, received type=%d", type).c_str());
189 
190  float* c_out = reinterpret_cast<float*>(PyArray_DATA(arrayPtr));
191  keypoints.reserve(nKpts);
192  for (int i = 0; i < nKpts*kptSize; i+=kptSize)
193  {
194  cv::KeyPoint kpt(c_out[i], c_out[i+1], 8, -1, c_out[i+2]);
195  keypoints.push_back(kpt);
196  }
197 
198  arrayPtr = reinterpret_cast<PyArrayObject*>(descPtr);
199  int nDesc = PyArray_SHAPE(arrayPtr)[0];
200  UASSERT(nDesc = nKpts);
201  int dim = PyArray_SHAPE(arrayPtr)[1];
202  type = PyArray_TYPE(arrayPtr);
203  UDEBUG("Desc array %dx%d (type=%d)", nDesc, dim, type);
204  UASSERT_MSG(type == NPY_FLOAT, uFormat("Returned matches should type FLOAT=11, received type=%d", type).c_str());
205 
206  c_out = reinterpret_cast<float*>(PyArray_DATA(arrayPtr));
207  for (int i = 0; i < nDesc*dim; i+=dim)
208  {
209  cv::Mat descriptor = cv::Mat(1, dim, CV_32FC1, &c_out[i]).clone();
210  descriptors_.push_back(descriptor);
211  }
212  }
213  }
214  else
215  {
216  UWARN("Expected tuple (Kpts 3 x N, Descriptors dim x N), returning empty features.");
217  }
218  Py_DECREF(pReturn);
219  }
220  Py_DECREF(pImageBuffer);
221  }
222 
223  unlock();
224 
225  return keypoints;
226 }
227 
228 cv::Mat PyDetector::generateDescriptorsImpl(const cv::Mat & image, std::vector<cv::KeyPoint> & keypoints) const
229 {
230  UASSERT((int)keypoints.size() == descriptors_.rows);
231  return descriptors_;
232 }
233 
234 }
glm::mask
GLM_FUNC_DECL genIType mask(genIType const &count)
UDirectory::homeDir
static std::string homeDir()
Definition: UDirectory.cpp:355
NULL
#define NULL
rtabmap::Parameters::parse
static bool parse(const ParametersMap &parameters, const std::string &key, bool &value)
Definition: Parameters.cpp:497
UTimer
Definition: UTimer.h:46
UFile::getName
std::string getName()
Definition: UFile.h:135
rtabmap::PyDetector::PyDetector
PyDetector(const ParametersMap &parameters=ParametersMap())
Definition: PyDetector.cpp:19
rtabmap::Feature2D::parseParameters
virtual void parseParameters(const ParametersMap &parameters)
Definition: Features2d.cpp:451
UDirectory::getDir
static std::string getDir(const std::string &filePath)
Definition: UDirectory.cpp:273
rtabmap::PyDetector::pFunc_
PyObject * pFunc_
Definition: PyDetector.h:36
rtabmap::ParametersMap
std::map< std::string, std::string > ParametersMap
Definition: Parameters.h:43
UConversion.h
Some conversion functions.
UFile::getExtension
std::string getExtension()
Definition: UFile.h:140
rtabmap::PyDetector::~PyDetector
virtual ~PyDetector()
Definition: PyDetector.cpp:63
UASSERT
#define UASSERT(condition)
UStl.h
Wrappers of STL for convenient functions.
uSplit
std::list< std::string > uSplit(const std::string &str, char separator=' ')
Definition: UStl.h:566
UASSERT_MSG
#define UASSERT_MSG(condition, msg_str)
Definition: ULogger.h:67
rtabmap::PyDetector::path_
std::string path_
Definition: PyDetector.h:37
uReplaceChar
std::string UTILITE_EXP uReplaceChar(const std::string &str, char before, char after)
Definition: UConversion.cpp:33
rtabmap::PyDetector::generateKeypointsImpl
virtual std::vector< cv::KeyPoint > generateKeypointsImpl(const cv::Mat &image, const cv::Rect &roi, const cv::Mat &mask=cv::Mat())
Definition: PyDetector.cpp:89
UDEBUG
#define UDEBUG(...)
UFile::exists
bool exists()
Definition: UFile.h:104
UERROR
#define UERROR(...)
ULogger.h
ULogger class and convenient macros.
UWARN
#define UWARN(...)
UTimer::ticks
double ticks()
Definition: UTimer.cpp:117
PyUnicode_FromString
#define PyUnicode_FromString
rtabmap::PyDetector::descriptors_
cv::Mat descriptors_
Definition: PyDetector.h:39
dim
dim
rtabmap::PyDetector::pModule_
PyObject * pModule_
Definition: PyDetector.h:35
uFormat
std::string UTILITE_EXP uFormat(const char *fmt,...)
Definition: UConversion.cpp:365
rtabmap::PyDetector::parseParameters
virtual void parseParameters(const ParametersMap &parameters)
Definition: PyDetector.cpp:79
rtabmap::PyDetector::generateDescriptorsImpl
virtual cv::Mat generateDescriptorsImpl(const cv::Mat &image, std::vector< cv::KeyPoint > &keypoints) const
Definition: PyDetector.cpp:228

rtabmap
Author(s): Mathieu Labbe
autogenerated on Mon Jan 23 2023 03:37:29