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/CMakeLists.txt

@@ -115,6 +115,8 @@ SET(SRC_FILES
     
     MarkerDetector.cpp
     
+    GlobalDescriptorExtractor.cpp
+    
     GainCompensator.cpp
 
     rtflann/ext/lz4.c
@@ -224,6 +226,7 @@ IF(WITH_PYTHON AND Python3_FOUND)
 		python/PythonInterface.cpp
 		python/PyMatcher.cpp
 		python/PyDetector.cpp
+		python/PyDescriptor.cpp
 	)
 	 SET(INCLUDE_DIRS
 	    ${TORCH_INCLUDE_DIRS}

corelib/src/GlobalDescriptorExtractor.cpp

@@ -0,0 +1,76 @@
+/*
+Copyright (c) 2010-2024, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
+All rights reserved.
+
+Redistribution and use in source and binary forms, with or without
+modification, are permitted provided that the following conditions are met:
+    * Redistributions of source code must retain the above copyright
+      notice, this list of conditions and the following disclaimer.
+    * Redistributions in binary form must reproduce the above copyright
+      notice, this list of conditions and the following disclaimer in the
+      documentation and/or other materials provided with the distribution.
+    * Neither the name of the Universite de Sherbrooke nor the
+      names of its contributors may be used to endorse or promote products
+      derived from this software without specific prior written permission.
+
+THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
+ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY
+DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+*/
+
+
+#include "rtabmap/core/GlobalDescriptorExtractor.h"
+
+#ifdef RTABMAP_PYTHON
+#include "python/PyDescriptor.h"
+#endif
+
+namespace rtabmap {
+
+GlobalDescriptorExtractor::GlobalDescriptorExtractor(const ParametersMap & parameters)
+{
+}
+GlobalDescriptorExtractor::~GlobalDescriptorExtractor()
+{
+}
+GlobalDescriptorExtractor * GlobalDescriptorExtractor::create(const ParametersMap & parameters)
+{
+	int type = Parameters::defaultMemGlobalDescriptorStrategy();
+	Parameters::parse(parameters, Parameters::kMemGlobalDescriptorStrategy(), type);
+	return create((GlobalDescriptorExtractor::Type)type, parameters);
+}
+GlobalDescriptorExtractor * GlobalDescriptorExtractor::create(GlobalDescriptorExtractor::Type type, const ParametersMap & parameters)
+{
+	UDEBUG("Creating global descriptor of type %d", (int)type);
+#ifndef RTABMAP_PYTHON
+	if(type == GlobalDescriptorExtractor::kPyDescriptor)
+	{
+		UWARN("PyDescriptor cannot be used as rtabmap is not built with Python3 support.");
+		type = GlobalDescriptorExtractor::kUndef;
+	}
+#endif
+
+	GlobalDescriptorExtractor * GlobalDescriptorExtractor = 0;
+	switch(type)
+	{
+#ifdef RTABMAP_PYTHON
+	case GlobalDescriptorExtractor::kPyDescriptor:
+		GlobalDescriptorExtractor = new PyDescriptor(parameters);
+		break;
+#endif
+	default:
+		type = GlobalDescriptorExtractor::kUndef;
+		break;
+	}
+	return GlobalDescriptorExtractor;
+}
+
+}
+

corelib/src/Memory.cpp

@@ -40,6 +40,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #include <rtabmap/core/EpipolarGeometry.h>
 #include "rtabmap/core/VisualWord.h"
 #include "rtabmap/core/Features2d.h"
+#include "rtabmap/core/GlobalDescriptorExtractor.h"
 #include "rtabmap/core/RegistrationIcp.h"
 #include "rtabmap/core/Registration.h"
 #include "rtabmap/core/RegistrationVis.h"
@@ -132,6 +133,7 @@ Memory::Memory(const ParametersMap & parameters) :
 	_feature2D = Feature2D::create(parameters);
 	_vwd = new VWDictionary(parameters);
 	_registrationPipeline = Registration::create(parameters);
+	_globalDescriptorExtractor = GlobalDescriptorExtractor::create(parameters);
 	if(!_registrationPipeline->isImageRequired())
 	{
 		// make sure feature matching is used instead of optical flow to compute the guess
@@ -761,6 +763,22 @@ void Memory::parseParameters(const ParametersMap & parameters)
 		_markerDetector->parseParameters(params);
 	}
 
+	int globalDescriptorStrategy = -1;
+	Parameters::parse(params, Parameters::kMemGlobalDescriptorStrategy(), globalDescriptorStrategy);
+	if(globalDescriptorStrategy != -1 &&
+			(_globalDescriptorExtractor==0 || (int)_globalDescriptorExtractor->getType() != globalDescriptorStrategy))
+	{
+		if(_globalDescriptorExtractor)
+		{
+			delete _globalDescriptorExtractor;
+		}
+		_globalDescriptorExtractor = GlobalDescriptorExtractor::create(parameters_);
+	}
+	else if(_globalDescriptorExtractor)
+	{
+		_globalDescriptorExtractor->parseParameters(params);
+	}
+
 	// do this after all params are parsed
 	// SLAM mode vs Localization mode
 	iter = params.find(Parameters::kMemIncrementalMemory());
@@ -5934,7 +5952,17 @@ Signature * Memory::createSignature(const SensorData & inputData, const Transfor
 	s->sensorData().setGroundTruth(data.groundTruth());
 	s->sensorData().setGPS(data.gps());
 	s->sensorData().setEnvSensors(data.envSensors());
-	s->sensorData().setGlobalDescriptors(data.globalDescriptors());
+
+	std::vector<GlobalDescriptor> globalDescriptors = data.globalDescriptors();
+	if(_globalDescriptorExtractor)
+	{
+		GlobalDescriptor gdescriptor = _globalDescriptorExtractor->extract(inputData);
+		if(!gdescriptor.data().empty())
+		{
+			globalDescriptors.push_back(gdescriptor);
+		}
+	}
+	s->sensorData().setGlobalDescriptors(globalDescriptors);
 
 	t = timer.ticks();
 	if(stats) stats->addStatistic(Statistics::kTimingMemCompressing_data(), t*1000.0f);

corelib/src/Signature.cpp

@@ -249,17 +249,40 @@ void Signature::removeLandmark(int landmarkId)
 
 float Signature::compareTo(const Signature & s) const
 {
+	UASSERT(this->sensorData().globalDescriptors().size() == s.sensorData().globalDescriptors().size());
+
 	float similarity = 0.0f;
-	const std::multimap<int, int> & words = s.getWords();
+	int totalDescs = 0;
 
-	if(!s.isBadSignature() && !this->isBadSignature())
+	for(size_t i=0; i<this->sensorData().globalDescriptors().size(); ++i)
 	{
-		std::list<std::pair<int, std::pair<int, int> > > pairs;
-		int totalWords = ((int)_words.size()-_invalidWordsCount)>((int)words.size()-s.getInvalidWordsCount())?((int)_words.size()-_invalidWordsCount):((int)words.size()-s.getInvalidWordsCount());
-		UASSERT(totalWords > 0);
-		EpipolarGeometry::findPairs(words, _words, pairs);
+		if(this->sensorData().globalDescriptors()[i].type()==1 && s.sensorData().globalDescriptors()[i].type()==1)
+		{
+			// rescale dot product from -1<->1 to 0<->1 (we assume normalized vectors!)
+			float dotProd = (this->sensorData().globalDescriptors()[i].data().dot(s.sensorData().globalDescriptors()[i].data()) + 1.0f) / 2.0f;
+			UASSERT_MSG(dotProd>=0, "Global descriptors should be normalized!");
+			similarity += dotProd;
+			totalDescs += 1;
+		}
+	}
 
-		similarity = float(pairs.size()) / float(totalWords);
+	if(totalDescs)
+	{
+		similarity /= totalDescs;
+	}
+	else
+	{
+		const std::multimap<int, int> & words = s.getWords();
+
+		if(!s.isBadSignature() && !this->isBadSignature())
+		{
+			std::list<std::pair<int, std::pair<int, int> > > pairs;
+			int totalWords = ((int)_words.size()-_invalidWordsCount)>((int)words.size()-s.getInvalidWordsCount())?((int)_words.size()-_invalidWordsCount):((int)words.size()-s.getInvalidWordsCount());
+			UASSERT(totalWords > 0);
+			EpipolarGeometry::findPairs(words, _words, pairs);
+
+			similarity = float(pairs.size()) / float(totalWords);
+		}
 	}
 	return similarity;
 }

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)