GlobalDescriptor (PyDescriptor / netvlad) (#1163)

* First commit for pydescriptor

* Fixed Python refactoring errors

* GUI: added PyDescriptor parameters

* reordered python3 includes

* updated rtabmap_netvlad.py test main

* fixed build from last merge

* integrated https://github.com/introlab/rtabmap/pull/1255

* rescaled dot product result

Closing https://github.com/introlab/rtabmap/issues/1105

corelib/src/python/PyDescriptor.cpp

@@ -0,0 +1,218 @@
+
+#include <python/PyDescriptor.h>
+#include <rtabmap/utilite/ULogger.h>
+#include <rtabmap/utilite/UDirectory.h>
+#include <rtabmap/utilite/UFile.h>
+#include <rtabmap/utilite/UStl.h>
+#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>
+
+namespace rtabmap
+{
+
+PyDescriptor::PyDescriptor(
+		const ParametersMap & parameters) :
+		GlobalDescriptorExtractor(parameters),
+		pModule_(0),
+		pFunc_(0),
+		dim_(Parameters::defaultPyDescriptorDim())
+{
+	UDEBUG("");
+	this->parseParameters(parameters);
+}
+
+PyDescriptor::~PyDescriptor()
+{
+	UDEBUG("");
+	pybind11::gil_scoped_acquire acquire;
+
+	if(pFunc_)
+	{
+		Py_DECREF(pFunc_);
+	}
+	if(pModule_)
+	{
+		Py_DECREF(pModule_);
+	}
+}
+
+void PyDescriptor::parseParameters(const ParametersMap & parameters)
+{
+	UDEBUG("");
+	std::string previousPath = path_;
+	Parameters::parse(parameters, Parameters::kPyDescriptorPath(), path_);
+	Parameters::parse(parameters, Parameters::kPyDescriptorDim(), dim_);
+	path_ = uReplaceChar(path_, '~', UDirectory::homeDir());
+	UINFO("path = %s", path_.c_str());
+	UINFO("dim = %d", dim_);
+	UTimer timer;
+
+	pybind11::gil_scoped_acquire acquire;
+
+	if(pModule_)
+	{
+		if(!previousPath.empty() && previousPath.compare(path_)!=0)
+		{
+			UDEBUG("we changed script (old=%s), we need to reload (new=%s)",
+					previousPath.c_str(), path_.c_str());
+			if(pFunc_)
+			{
+				Py_DECREF(pFunc_);
+			}
+			pFunc_=0;
+			Py_DECREF(pModule_);
+			pModule_ = 0;
+		}
+	}
+
+	if(pModule_==0)
+	{
+		UASSERT(pFunc_ == 0);
+		if(path_.empty())
+		{
+			return;
+		}
+		std::string matcherPythonDir = UDirectory::getDir(path_);
+		if(!matcherPythonDir.empty())
+		{
+			PyRun_SimpleString("import sys");
+			PyRun_SimpleString(uFormat("sys.path.append(\"%s\")", matcherPythonDir.c_str()).c_str());
+		}
+
+		_import_array();
+
+		std::string scriptName = uSplit(UFile::getName(path_), '.').front();
+		PyObject * pName = PyUnicode_FromString(scriptName.c_str());
+		UDEBUG("PyImport_Import() beg");
+		pModule_ = PyImport_Import(pName);
+		UDEBUG("PyImport_Import() end");
+
+		Py_DECREF(pName);
+
+		if(!pModule_)
+		{
+			UERROR("Module \"%s\" could not be imported! (File=\"%s\")", scriptName.c_str(), path_.c_str());
+			UERROR("%s", getPythonTraceback().c_str());
+		}
+		else
+		{
+			PyObject * pFunc = PyObject_GetAttrString(pModule_, "init");
+			if(pFunc)
+			{
+				if(PyCallable_Check(pFunc))
+				{
+					PyObject * result = PyObject_CallFunction(pFunc, "i", dim_);
+
+					if(result == NULL)
+					{
+						UERROR("Call to \"init(...)\" in \"%s\" failed!", path_.c_str());
+						UERROR("%s", getPythonTraceback().c_str());
+					}
+					Py_DECREF(result);
+
+					pFunc_ = PyObject_GetAttrString(pModule_, "extract");
+					if(pFunc_ && PyCallable_Check(pFunc_))
+					{
+						// we are ready!
+					}
+					else
+					{
+						UERROR("Cannot find method \"extract(...)\" in %s", path_.c_str());
+						UERROR("%s", getPythonTraceback().c_str());
+						if(pFunc_)
+						{
+							Py_DECREF(pFunc_);
+							pFunc_ = 0;
+						}
+					}
+				}
+				else
+				{
+					UERROR("Cannot call method \"init(...)\" in %s", path_.c_str());
+					UERROR("%s", getPythonTraceback().c_str());
+				}
+				Py_DECREF(pFunc);
+			}
+			else
+			{
+				UERROR("Cannot find method \"init(...)\"");
+				UERROR("%s", getPythonTraceback().c_str());
+			}
+		}
+	}
+}
+
+GlobalDescriptor PyDescriptor::extract(
+		  const SensorData & data) const
+{
+	UDEBUG("");
+	UTimer timer;
+	GlobalDescriptor descriptor;
+
+	if(!pModule_)
+	{
+		UERROR("Python module not loaded!");
+		return descriptor;
+	}
+
+	if(!pFunc_)
+	{
+		UERROR("Python function not loaded!");
+		return descriptor;
+	}
+
+	pybind11::gil_scoped_acquire acquire;
+
+	if(!data.imageRaw().empty())
+	{
+		std::vector<unsigned char> descriptorsQueryV(data.imageRaw().total()*data.imageRaw().channels());
+		memcpy(descriptorsQueryV.data(), data.imageRaw().data, data.imageRaw().total()*data.imageRaw().channels()*sizeof(char));
+		npy_intp dimsFrom[3] = {data.imageRaw().rows, data.imageRaw().cols, data.imageRaw().channels()};
+		PyObject* pImageQuery = PyArray_SimpleNewFromData(3, dimsFrom, NPY_BYTE, (void*)data.imageRaw().data);
+		UASSERT(pImageQuery);
+
+		UDEBUG("Preparing data time = %fs", timer.ticks());
+
+		PyObject *pReturn = PyObject_CallFunctionObjArgs(pFunc_, pImageQuery, NULL);
+		if(pReturn == NULL)
+		{
+			UERROR("Failed to call extract() function!");
+			UERROR("%s", getPythonTraceback().c_str());
+		}
+		else
+		{
+			UDEBUG("Python extraction time = %fs", timer.ticks());
+
+			PyArrayObject *np_ret = reinterpret_cast<PyArrayObject*>(pReturn);
+
+			// Convert back to C++ array and print.
+			int len1 = PyArray_SHAPE(np_ret)[0];
+			int dim = PyArray_SHAPE(np_ret)[1];
+			int type = PyArray_TYPE(np_ret);
+			UDEBUG("Descriptor array %dx%d (type=%d)", len1, dim, type);
+			UASSERT(len1 == 1);
+			UASSERT_MSG(type == NPY_FLOAT, uFormat("Returned descriptor should type FLOAT=11, received type=%d", type).c_str());
+
+			float* d_out = reinterpret_cast<float*>(PyArray_DATA(np_ret));
+			descriptor = GlobalDescriptor(1, cv::Mat(1, dim, CV_32FC1, d_out).clone());
+
+			//std::cout << descriptor.data() << std::endl;
+
+			Py_DECREF(pReturn);
+		}
+
+		Py_DECREF(pImageQuery);
+	}
+	else
+	{
+		UERROR("Invalid inputs! Missing image.");
+	}
+	return descriptor;
+}
+
+}

corelib/src/python/PyDescriptor.h

@@ -0,0 +1,38 @@
+/**
+ * Python interface for python descriptors like:
+ *  - NetVLAD: https://github.com/uzh-rpg/netvlad_tf_open
+ */
+
+#ifndef PYDESCRIPTOR_H
+#define PYDESCRIPTOR_H
+
+#include <rtabmap/core/GlobalDescriptorExtractor.h>
+#include "rtabmap/core/PythonInterface.h"
+#include <Python.h>
+
+namespace rtabmap
+{
+
+class PyDescriptor : public GlobalDescriptorExtractor
+{
+public:
+  PyDescriptor(const ParametersMap & parameters = ParametersMap());
+  virtual ~PyDescriptor();
+
+  const std::string & path() const {return path_;}
+  float dim() const {return dim_;}
+
+  virtual void parseParameters(const ParametersMap & parameters);
+  virtual GlobalDescriptor extract(const SensorData & data) const;
+  virtual GlobalDescriptorExtractor::Type getType() const {return kPyDescriptor;}
+
+private:
+  PyObject * pModule_;
+  PyObject * pFunc_;
+  std::string path_;
+  int dim_;
+};
+
+}
+
+#endif

corelib/src/python/PyDetector.h

@@ -1,11 +1,10 @@
 /**
- * Python interface for python matchers like:
- *  - SuperGlue: https://github.com/magicleap/SuperGluePretrainedNetwork
- *  - OANET https://github.com/zjhthu/OANet
+ * Python interface for python local feature detectors like:
+ *  - SuperPoint: https://github.com/magicleap/SuperPointPretrainedNetwork
  */
 
-#ifndef PYMATCHER_H
-#define PYMATCHER_H
+#ifndef PYDETECTOR_H
+#define PYDETECTOR_H
 
 #include <rtabmap/core/Features2d.h>
 #include <opencv2/core/types.hpp>

corelib/src/python/rtabmap_netvlad.py

@@ -0,0 +1,83 @@
+#! /usr/bin/env python3
+#
+# Drop this file in the "python" folder of NetVLAD git (tensorflow-v1 used): https://github.com/uzh-rpg/netvlad_tf_open/
+# Updated to work with https://github.com/uzh-rpg/netvlad_tf_open/pull/9
+# To use with rtabmap:
+#   --Mem/GlobalDescriptorStrategy 1 --Kp/TfIdfLikelihoodUsed false --Mem/RehearsalSimilarity 1 --PyDescriptor/Dim 128 --PyDescriptor/Path ~/netvlad_tf_open/python/rtabmap_netvlad.py
+#
+
+import sys    
+import os
+import numpy as np
+import time
+sys.path.append(os.path.dirname(os.path.realpath(__file__)))
+if not hasattr(sys, 'argv'):
+    sys.argv  = ['']
+    
+#print(os.sys.path)
+#print(sys.version)
+
+import tensorflow as tf
+import netvlad_tf.net_from_mat as nfm
+import netvlad_tf.nets as nets
+
+image_batch = None
+net_out = None
+saver = None
+sess = None
+dim = 4096
+
+def init(descriptorDim):
+    print("NetVLAD python init()")
+    global image_batch
+    global net_out
+    global saver
+    global sess
+    global dim
+    
+    dim = descriptorDim
+
+    tf.compat.v1.disable_eager_execution()
+    tf.compat.v1.reset_default_graph()
+
+    image_batch = tf.compat.v1.placeholder(
+        dtype=tf.float32, shape=[None, None, None, 3])
+
+    net_out = nets.vgg16NetvladPca(image_batch)
+    saver = tf.compat.v1.train.Saver()
+
+    sess = tf.compat.v1.Session()
+    saver.restore(sess, nets.defaultCheckpoint())
+
+
+def extract(image):
+    print(f"NetVLAD python extract{image.shape}")
+    global image_batch
+    global net_out
+    global sess
+    global dim
+
+    if(image.shape[2] == 1):
+        image = np.dstack((image, image, image))
+
+    batch = np.expand_dims(image, axis=0)
+    result = sess.run(net_out, feed_dict={image_batch: batch})
+    
+    # All that needs to be done (only valid for NetVLAD+whitening networks!)
+    # to reduce the dimensionality of the NetVLAD representation below 4096 to D
+    # is to keep the first D dimensions and L2-normalize.
+    if(result.shape[1] > dim):
+        v = result[:, :dim]
+        result = v/np.linalg.norm(v)
+
+    return np.float32(result)
+
+
+if __name__ == '__main__':
+    #test
+    img = np.zeros([100,100,3],dtype=np.uint8)
+    img.fill(255)
+    init(128)
+    descriptor = extract(img)
+    print(descriptor.shape)
+    print(descriptor)