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

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