PyMatcher.cpp
Go to the documentation of this file.
1 
5 #include <pymatcher/PyMatcher.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 class PythonSingleTon
20 {
21 public:
22  PythonSingleTon() : initialized_(false) {}
23  void init() {UScopeMutex lock(mutex_); if(!initialized_)Py_Initialize(); initialized_=true;}
24  bool initialized() const {return initialized_;}
25  virtual ~PythonSingleTon() {if(initialized_) Py_Finalize();}
26 private:
27  bool initialized_;
28  UMutex mutex_;
29 };
30 
31 static PythonSingleTon g_python;
32 
33 std::string getTraceback()
34 {
35  // Author: https://stackoverflow.com/questions/41268061/c-c-python-exception-traceback-not-being-generated
36 
37  PyObject* type;
38  PyObject* value;
39  PyObject* traceback;
40 
41  PyErr_Fetch(&type, &value, &traceback);
42  PyErr_NormalizeException(&type, &value, &traceback);
43 
44  std::string fcn = "";
45  fcn += "def get_pretty_traceback(exc_type, exc_value, exc_tb):\n";
46  fcn += " import sys, traceback\n";
47  fcn += " lines = []\n";
48  fcn += " lines = traceback.format_exception(exc_type, exc_value, exc_tb)\n";
49  fcn += " output = '\\n'.join(lines)\n";
50  fcn += " return output\n";
51 
52  PyRun_SimpleString(fcn.c_str());
53  PyObject* mod = PyImport_ImportModule("__main__");
54  PyObject* method = PyObject_GetAttrString(mod, "get_pretty_traceback");
55  PyObject* outStr = PyObject_CallObject(method, Py_BuildValue("OOO", type, value, traceback));
56  std::string pretty = PyBytes_AsString(PyUnicode_AsASCIIString(outStr));
57 
58  Py_DECREF(method);
59  Py_DECREF(outStr);
60  Py_DECREF(mod);
61 
62  return pretty;
63 }
64 
65 PyMatcher::PyMatcher(
66  const std::string & pythonMatcherPath,
67  float matchThreshold,
68  int iterations,
69  bool cuda,
70  const std::string & model) :
71  pModule_(0),
72  pFunc_(0),
73  matchThreshold_(matchThreshold),
74  iterations_(iterations),
75  cuda_(cuda)
76 {
77  path_ = uReplaceChar(pythonMatcherPath, '~', UDirectory::homeDir());
78  model_ = uReplaceChar(model, '~', UDirectory::homeDir());
79  UINFO("path = %s", path_.c_str());
80  UINFO("model = %s", model_.c_str());
81 
82  if(!UFile::exists(path_) || UFile::getExtension(path_).compare("py") != 0)
83  {
84  UERROR("Cannot initialize Python matcher, the path is not valid: \"%s\"", path_.c_str());
85  return;
86  }
87 
88  if(!g_python.initialized())
89  {
90  g_python.init();
91  }
92 
93  std::string matcherPythonDir = UDirectory::getDir(path_);
94  if(!matcherPythonDir.empty())
95  {
96  PyRun_SimpleString("import sys");
97  PyRun_SimpleString(uFormat("sys.path.append(\"%s\")", matcherPythonDir.c_str()).c_str());
98  }
99 
100  _import_array();
101 
102  std::string scriptName = uSplit(UFile::getName(path_), '.').front();
103  PyObject * pName = PyUnicode_FromString(scriptName.c_str());
104  pModule_ = PyImport_Import(pName);
105  Py_DECREF(pName);
106 
107  if(!pModule_)
108  {
109  UERROR("Module \"%s\" could not be imported! (File=\"%s\")", scriptName.c_str(), path_.c_str());
110  UERROR("%s", getTraceback().c_str());
111  }
112 }
113 
114 PyMatcher::~PyMatcher()
115 {
116  if(pFunc_)
117  {
118  Py_DECREF(pFunc_);
119  }
120  if(pModule_)
121  {
122  Py_DECREF(pModule_);
123  }
124 }
125 
126 std::vector<cv::DMatch> PyMatcher::match(
127  const cv::Mat & descriptorsQuery,
128  const cv::Mat & descriptorsTrain,
129  const std::vector<cv::KeyPoint> & keypointsQuery,
130  const std::vector<cv::KeyPoint> & keypointsTrain,
131  const cv::Size & imageSize)
132 {
133  UTimer timer;
134  std::vector<cv::DMatch> matches;
135 
136  if(!pModule_)
137  {
138  UERROR("Python matcher module not loaded!");
139  return matches;
140  }
141 
142  if(!descriptorsQuery.empty() &&
143  descriptorsQuery.cols == descriptorsTrain.cols &&
144  descriptorsQuery.type() == CV_32F &&
145  descriptorsTrain.type() == CV_32F &&
146  descriptorsQuery.rows == (int)keypointsQuery.size() &&
147  descriptorsTrain.rows == (int)keypointsTrain.size() &&
148  imageSize.width>0 && imageSize.height>0)
149  {
150 
151  UDEBUG("matchThreshold=%f, iterations=%d, cuda=%d", matchThreshold_, iterations_, cuda_?1:0);
152 
153  if(!pFunc_)
154  {
155  PyObject * pFunc = PyObject_GetAttrString(pModule_, "init");
156  if(pFunc)
157  {
158  if(PyCallable_Check(pFunc))
159  {
160  PyObject * result = PyObject_CallFunction(pFunc, "ifiis", descriptorsQuery.cols, matchThreshold_, iterations_, cuda_?1:0, model_.c_str());
161 
162  if(result == NULL)
163  {
164  UERROR("Call to \"init(...)\" in \"%s\" failed!", path_.c_str());
165  UERROR("%s", getTraceback().c_str());
166  return matches;
167  }
168  Py_DECREF(result);
169 
170  pFunc_ = PyObject_GetAttrString(pModule_, "match");
171  if(pFunc_ && PyCallable_Check(pFunc_))
172  {
173  // we are ready!
174  }
175  else
176  {
177  UERROR("Cannot find method \"match(...)\" in %s", path_.c_str());
178  UERROR("%s", getTraceback().c_str());
179  if(pFunc_)
180  {
181  Py_DECREF(pFunc_);
182  pFunc_ = 0;
183  }
184  return matches;
185  }
186  }
187  else
188  {
189  UERROR("Cannot call method \"init(...)\" in %s", path_.c_str());
190  UERROR("%s", getTraceback().c_str());
191  return matches;
192  }
193  Py_DECREF(pFunc);
194  }
195  else
196  {
197  UERROR("Cannot find method \"init(...)\"");
198  UERROR("%s", getTraceback().c_str());
199  return matches;
200  }
201  UDEBUG("init time = %fs", timer.ticks());
202  }
203 
204  if(pFunc_)
205  {
206  std::vector<float> descriptorsQueryV(descriptorsQuery.rows * descriptorsQuery.cols);
207  memcpy(descriptorsQueryV.data(), descriptorsQuery.data, descriptorsQuery.total()*sizeof(float));
208  npy_intp dimsFrom[2] = {descriptorsQuery.rows, descriptorsQuery.cols};
209  PyObject* pDescriptorsQuery = PyArray_SimpleNewFromData(2, dimsFrom, NPY_FLOAT, (void*)descriptorsQueryV.data());
210  UASSERT(pDescriptorsQuery);
211 
212  npy_intp dimsTo[2] = {descriptorsTrain.rows, descriptorsTrain.cols};
213  std::vector<float> descriptorsTrainV(descriptorsTrain.rows * descriptorsTrain.cols);
214  memcpy(descriptorsTrainV.data(), descriptorsTrain.data, descriptorsTrain.total()*sizeof(float));
215  PyObject* pDescriptorsTrain = PyArray_SimpleNewFromData(2, dimsTo, NPY_FLOAT, (void*)descriptorsTrainV.data());
216  UASSERT(pDescriptorsTrain);
217 
218  std::vector<float> keypointsQueryV(keypointsQuery.size()*2);
219  std::vector<float> scoresQuery(keypointsQuery.size());
220  for(size_t i=0; i<keypointsQuery.size(); ++i)
221  {
222  keypointsQueryV[i*2] = keypointsQuery[i].pt.x;
223  keypointsQueryV[i*2+1] = keypointsQuery[i].pt.y;
224  scoresQuery[i] = keypointsQuery[i].response;
225  }
226 
227  std::vector<float> keypointsTrainV(keypointsTrain.size()*2);
228  std::vector<float> scoresTrain(keypointsTrain.size());
229  for(size_t i=0; i<keypointsTrain.size(); ++i)
230  {
231  keypointsTrainV[i*2] = keypointsTrain[i].pt.x;
232  keypointsTrainV[i*2+1] = keypointsTrain[i].pt.y;
233  scoresTrain[i] = keypointsTrain[i].response;
234  }
235 
236  npy_intp dimsKpQuery[2] = {(int)keypointsQuery.size(), 2};
237  PyObject* pKeypointsQuery = PyArray_SimpleNewFromData(2, dimsKpQuery, NPY_FLOAT, (void*)keypointsQueryV.data());
238  UASSERT(pKeypointsQuery);
239 
240  npy_intp dimsKpTrain[2] = {(int)keypointsTrain.size(), 2};
241  PyObject* pkeypointsTrain = PyArray_SimpleNewFromData(2, dimsKpTrain, NPY_FLOAT, (void*)keypointsTrainV.data());
242  UASSERT(pkeypointsTrain);
243 
244  npy_intp dimsScoresQuery[1] = {(int)keypointsQuery.size()};
245  PyObject* pScoresQuery = PyArray_SimpleNewFromData(1, dimsScoresQuery, NPY_FLOAT, (void*)scoresQuery.data());
246  UASSERT(pScoresQuery);
247 
248  npy_intp dimsScoresTrain[1] = {(int)keypointsTrain.size()};
249  PyObject* pScoresTrain = PyArray_SimpleNewFromData(1, dimsScoresTrain, NPY_FLOAT, (void*)scoresTrain.data());
250  UASSERT(pScoresTrain);
251 
252  PyObject * pImageWidth = PyLong_FromLong(imageSize.width);
253  PyObject * pImageHeight = PyLong_FromLong(imageSize.height);
254 
255  UDEBUG("Preparing data time = %fs", timer.ticks());
256 
257  PyObject *pReturn = PyObject_CallFunctionObjArgs(pFunc_, pKeypointsQuery, pkeypointsTrain, pScoresQuery, pScoresTrain, pDescriptorsQuery, pDescriptorsTrain, pImageWidth, pImageHeight, NULL);
258  if(pReturn == NULL)
259  {
260  UERROR("Failed to call match() function!");
261  UERROR("%s", getTraceback().c_str());
262  }
263  else
264  {
265  UDEBUG("Python matching time = %fs", timer.ticks());
266 
267  PyArrayObject *np_ret = reinterpret_cast<PyArrayObject*>(pReturn);
268 
269  // Convert back to C++ array and print.
270  int len1 = PyArray_SHAPE(np_ret)[0];
271  int len2 = PyArray_SHAPE(np_ret)[1];
272  int type = PyArray_TYPE(np_ret);
273  UDEBUG("Matches array %dx%d (type=%d)", len1, len2, type);
274  UASSERT_MSG(type == NPY_LONG || type == NPY_INT, uFormat("Returned matches should type INT=5 or LONG=7, received type=%d", type).c_str());
275  if(type == NPY_LONG)
276  {
277  long* c_out = reinterpret_cast<long*>(PyArray_DATA(np_ret));
278  for (int i = 0; i < len1*len2; i+=2)
279  {
280  matches.push_back(cv::DMatch(c_out[i], c_out[i+1], 0));
281  }
282  }
283  else // INT
284  {
285  int* c_out = reinterpret_cast<int*>(PyArray_DATA(np_ret));
286  for (int i = 0; i < len1*len2; i+=2)
287  {
288  matches.push_back(cv::DMatch(c_out[i], c_out[i+1], 0));
289  }
290  }
291  Py_DECREF(pReturn);
292  }
293 
294  Py_DECREF(pDescriptorsQuery);
295  Py_DECREF(pDescriptorsTrain);
296  Py_DECREF(pKeypointsQuery);
297  Py_DECREF(pkeypointsTrain);
298  Py_DECREF(pScoresQuery);
299  Py_DECREF(pScoresTrain);
300  Py_DECREF(pImageWidth);
301  Py_DECREF(pImageHeight);
302 
303  UDEBUG("Fill matches (%d/%d) and cleanup time = %fs", matches.size(), std::min(descriptorsQuery.rows, descriptorsTrain.rows), timer.ticks());
304  }
305  }
306  else
307  {
308  UERROR("Invalid inputs! Supported python matchers require float descriptors.");
309  }
310  return matches;
311 }
312 
313 }
UDirectory::homeDir
static std::string homeDir()
Definition: UDirectory.cpp:355
NULL
#define NULL
UTimer
Definition: UTimer.h:46
UFile::getName
std::string getName()
Definition: UFile.h:135
glm::min
GLM_FUNC_DECL genType min(genType const &x, genType const &y)
PyBytes_AsString
#define PyBytes_AsString
glm::mod
GLM_FUNC_DECL genType mod(genType const &x, genType const &y)
UDirectory::getDir
static std::string getDir(const std::string &filePath)
Definition: UDirectory.cpp:273
UConversion.h
Some conversion functions.
UFile::getExtension
std::string getExtension()
Definition: UFile.h:140
uSplit
std::list< std::string > uSplit(const std::string &str, char separator= ' ')
Definition: UStl.h:566
rtabmap::getTraceback
std::string getTraceback()
Definition: PyMatcher.cpp:33
UASSERT
#define UASSERT(condition)
UStl.h
Wrappers of STL for convenient functions.
UASSERT_MSG
#define UASSERT_MSG(condition, msg_str)
Definition: ULogger.h:67
rtabmap::PyMatcher::model_
std::string model_
Definition: PyMatcher.h:49
UMutex
Definition: UMutex.h:54
rtabmap::g_python
static PythonSingleTon g_python
Definition: PyMatcher.cpp:31
rtabmap::PythonSingleTon::initialized
bool initialized() const
Definition: PyMatcher.cpp:24
rtabmap::PyMatcher::pModule_
PyObject * pModule_
Definition: PyMatcher.h:43
rtabmap::PyMatcher::PyMatcher
PyMatcher(const std::string &pythonMatcherPath, float matchThreshold=0.2f, int iterations=20, bool cuda=true, const std::string &model="indoor")
Definition: PyMatcher.cpp:65
uReplaceChar
std::string UTILITE_EXP uReplaceChar(const std::string &str, char before, char after)
Definition: UConversion.cpp:32
rtabmap::PyMatcher::matchThreshold_
float matchThreshold_
Definition: PyMatcher.h:46
false
#define false
Definition: ConvertUTF.c:56
rtabmap::PyMatcher::~PyMatcher
virtual ~PyMatcher()
Definition: PyMatcher.cpp:114
UDEBUG
#define UDEBUG(...)
UFile::exists
bool exists()
Definition: UFile.h:104
UERROR
#define UERROR(...)
ULogger.h
ULogger class and convenient macros.
UTimer::ticks
double ticks()
Definition: UTimer.cpp:117
PyUnicode_FromString
#define PyUnicode_FromString
rtabmap::PyMatcher::pFunc_
PyObject * pFunc_
Definition: PyMatcher.h:44
rtabmap::PyMatcher::path_
std::string path_
Definition: PyMatcher.h:45
uFormat
std::string UTILITE_EXP uFormat(const char *fmt,...)
Definition: UConversion.cpp:350
rtabmap::PyMatcher::match
std::vector< cv::DMatch > match(const cv::Mat &descriptorsQuery, const cv::Mat &descriptorsTrain, const std::vector< cv::KeyPoint > &keypointsQuery, const std::vector< cv::KeyPoint > &keypointsTrain, const cv::Size &imageSize)
Definition: PyMatcher.cpp:126
UINFO
#define UINFO(...)

rtabmap
Author(s): Mathieu Labbe
autogenerated on Mon Dec 14 2020 03:34:59