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

rtabmap
Author(s): Mathieu Labbe
autogenerated on Thu Jul 25 2024 02:50:14