From b2efd4396cb46f9ea627aeb7ee28c68145cc8ec8 Mon Sep 17 00:00:00 2001 From: matlabbe Date: Wed, 22 Jul 2020 16:33:36 -0400 Subject: [PATCH 1/8] First commit for pydescriptor --- CMakeLists.txt | 12 +- Version.h.in | 2 +- .../rtabmap/core/GlobalDescriptorExtractor.h | 77 ++++++++ corelib/include/rtabmap/core/Memory.h | 3 + corelib/include/rtabmap/core/Parameters.h | 5 + .../include/rtabmap/core/RegistrationVis.h | 4 +- corelib/src/CMakeLists.txt | 5 + corelib/src/Features2d.cpp | 2 - corelib/src/GlobalDescriptorExtractor.cpp | 55 ++++++ corelib/src/Memory.cpp | 19 +- corelib/src/Parameters.cpp | 2 +- corelib/src/PyUtil.cpp | 97 ++++++++++ corelib/src/PyUtil.h | 39 ++++ corelib/src/RegistrationVis.cpp | 12 +- corelib/src/pydescriptor/PyDescriptor.cpp | 181 ++++++++++++++++++ corelib/src/pydescriptor/PyDescriptor.h | 40 ++++ corelib/src/pydescriptor/rtabmap_netvlad.py | 44 +++++ corelib/src/pymatcher/PyMatcher.cpp | 88 +-------- guilib/src/AboutDialog.cpp | 2 +- guilib/src/PreferencesDialog.cpp | 2 +- 20 files changed, 588 insertions(+), 103 deletions(-) create mode 100644 corelib/include/rtabmap/core/GlobalDescriptorExtractor.h create mode 100644 corelib/src/GlobalDescriptorExtractor.cpp create mode 100644 corelib/src/PyUtil.cpp create mode 100644 corelib/src/PyUtil.h create mode 100644 corelib/src/pydescriptor/PyDescriptor.cpp create mode 100644 corelib/src/pydescriptor/PyDescriptor.h create mode 100644 corelib/src/pydescriptor/rtabmap_netvlad.py diff --git a/CMakeLists.txt b/CMakeLists.txt index 947501f048..c6d15cd756 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -164,7 +164,7 @@ option(WITH_QT "Include Qt support" ON) ENDIF() option(WITH_ORB_OCTREE "Include ORB Octree feature support" ON) option(WITH_SUPERPOINT_TORCH "Include SuperPoint Torch feature support" ON) -option(WITH_PYMATCHER "Include Python3 matchers support" OFF) +option(WITH_PYTHON3 "Include Python3 support" OFF) option(WITH_FREENECT "Include Freenect support" ON) option(WITH_FREENECT2 "Include Freenect2 support" ON) option(WITH_K4W2 "Include Kinect for Windows v2 support" ON) @@ -329,12 +329,12 @@ IF(WITH_SUPERPOINT_TORCH) ENDIF(TORCH_FOUND) ENDIF(WITH_SUPERPOINT_TORCH) -IF(WITH_PYMATCHER) +IF(WITH_PYTHON3) FIND_PACKAGE(Python3 COMPONENTS Interpreter Development) IF(Python3_FOUND) MESSAGE(STATUS "Found Python3") ENDIF(Python3_FOUND) -ENDIF(WITH_PYMATCHER) +ENDIF(WITH_PYTHON3) IF(WITH_FREENECT) FIND_PACKAGE(Freenect QUIET) @@ -841,7 +841,7 @@ IF(NOT TORCH_FOUND) SET(SUPERPOINT_TORCH "//") ENDIF() IF(NOT Python3_FOUND) - SET(PYMATCHER "//") + SET(PYTHON3 "//") ENDIF() IF(ADD_VTK_GUI_SUPPORT_QT_TO_CONF) SET(CONF_VTK_QT true) @@ -1104,8 +1104,8 @@ ENDIF() IF(Python3_FOUND) MESSAGE(STATUS " With Python3 = YES (License: PSF)") -ELSEIF(NOT WITH_PYMATCHER) -MESSAGE(STATUS " With Python3 = NO (WITH_PYMATCHER=OFF)") +ELSEIF(NOT WITH_PYTHON3) +MESSAGE(STATUS " With Python3 = NO (WITH_PYTHON3=OFF)") ELSE() MESSAGE(STATUS " With Python3 = NO (python3 not found)") ENDIF() diff --git a/Version.h.in b/Version.h.in index 78994dc264..8c8f0f619e 100644 --- a/Version.h.in +++ b/Version.h.in @@ -73,7 +73,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. @ORB_SLAM2@#define RTABMAP_ORB_SLAM2 @ORB_OCTREE@#define RTABMAP_ORB_OCTREE @SUPERPOINT_TORCH@#define RTABMAP_SUPERPOINT_TORCH -@PYMATCHER@#define RTABMAP_PYMATCHER +@PYTHON3@#define RTABMAP_PYTHON3 @MADGWICK@#define RTABMAP_MADGWICK diff --git a/corelib/include/rtabmap/core/GlobalDescriptorExtractor.h b/corelib/include/rtabmap/core/GlobalDescriptorExtractor.h new file mode 100644 index 0000000000..6c65d1f61d --- /dev/null +++ b/corelib/include/rtabmap/core/GlobalDescriptorExtractor.h @@ -0,0 +1,77 @@ +/* +Copyright (c) 2010-2016, 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. +*/ + +#ifndef GLOBAL_DESCRIPTOR_EXTRACTOR_H_ +#define GLOBAL_DESCRIPTOR_EXTRACTOR_H_ + +#include "rtabmap/core/RtabmapExp.h" // DLL export/import defines + +#include +#include +#include +#include +#include "rtabmap/core/Parameters.h" +#include "rtabmap/core/SensorData.h" + + +namespace rtabmap { + +// Feature2D +class RTABMAP_EXP GlobalDescriptorExtractor { +public: + enum Type { + kUndef=-1, + kPyDescriptor=0}; + + static std::string typeName(Type type) + { + switch(type){ + case kPyDescriptor: + return "PyDescriptor"; + default: + return "Unknown"; + } + } + + static GlobalDescriptorExtractor * create(const ParametersMap & parameters = ParametersMap()); + static GlobalDescriptorExtractor * create(GlobalDescriptorExtractor::Type type, const ParametersMap & parameters = ParametersMap()); // for convenience + +public: + virtual ~GlobalDescriptorExtractor(); + + virtual GlobalDescriptor extract(const SensorData & data) const = 0; + + virtual void parseParameters(const ParametersMap & parameters) {} + virtual GlobalDescriptorExtractor::Type getType() const = 0; + +protected: + GlobalDescriptorExtractor(const ParametersMap & parameters = ParametersMap()); +}; + +} + +#endif /* GLOBAL_DESCRIPTOR_EXTRACTOR_H_ */ diff --git a/corelib/include/rtabmap/core/Memory.h b/corelib/include/rtabmap/core/Memory.h index 61e39ce45b..857a6400e8 100644 --- a/corelib/include/rtabmap/core/Memory.h +++ b/corelib/include/rtabmap/core/Memory.h @@ -58,6 +58,7 @@ class RegistrationIcp; class Stereo; class OccupancyGrid; class MarkerDetector; +class GlobalDescriptorExtractor; class RTABMAP_EXP Memory { @@ -360,6 +361,8 @@ class RTABMAP_EXP Memory OccupancyGrid * _occupancy; MarkerDetector * _markerDetector; + + GlobalDescriptorExtractor * _globalDescriptorExtractor; }; } // namespace rtabmap diff --git a/corelib/include/rtabmap/core/Parameters.h b/corelib/include/rtabmap/core/Parameters.h index 86c90022c3..f30135dd90 100644 --- a/corelib/include/rtabmap/core/Parameters.h +++ b/corelib/include/rtabmap/core/Parameters.h @@ -230,6 +230,7 @@ class RTABMAP_EXP Parameters RTABMAP_PARAM(Mem, UseOdomFeatures, bool, true, "Use odometry features instead of regenerating them."); RTABMAP_PARAM(Mem, UseOdomGravity, bool, false, uFormat("Use odometry instead of IMU orientation to add gravity links to new nodes created. We assume that odometry is already aligned with gravity (e.g., we are using a VIO approach). Gravity constraints are used by graph optimization only if \"%s\" is not zero.", kOptimizerGravitySigma().c_str())); RTABMAP_PARAM(Mem, CovOffDiagIgnored, bool, true, "Ignore off diagonal values of the covariance matrix."); + RTABMAP_PARAM(Mem, GlobalDescriptorStrategy, int, -1, "Extract global descriptor from sensor data. -1=not used, 0=PyDescriptor"); // KeypointMemory (Keypoint-based) RTABMAP_PARAM(Kp, NNStrategy, int, 1, "kNNFlannNaive=0, kNNFlannKdTree=1, kNNFlannLSH=2, kNNBruteForce=3, kNNBruteForceGPU=4"); @@ -626,6 +627,10 @@ class RTABMAP_EXP Parameters RTABMAP_PARAM(GMS, WithScale, bool, false, "Take scale transformation into account."); RTABMAP_PARAM(GMS, ThresholdFactor, double, 6.0, "The higher, the less matches."); + // Global descriptor approaches + RTABMAP_PARAM_STR(PyDescriptor, Path, "", "Path to python script file (see available ones in rtabmap/corelib/src/pydescriptor/*). See the header to see where the script should be used."); + RTABMAP_PARAM(PyDescriptor, Dim, int, 4096, "Descriptor dimension."); + // ICP registration parameters RTABMAP_PARAM(Icp, MaxTranslation, float, 0.2, "Maximum ICP translation correction accepted (m)."); RTABMAP_PARAM(Icp, MaxRotation, float, 0.78, "Maximum ICP rotation correction accepted (rad)."); diff --git a/corelib/include/rtabmap/core/RegistrationVis.h b/corelib/include/rtabmap/core/RegistrationVis.h index 8366b63a18..c6227f7222 100644 --- a/corelib/include/rtabmap/core/RegistrationVis.h +++ b/corelib/include/rtabmap/core/RegistrationVis.h @@ -37,7 +37,7 @@ namespace rtabmap { class Feature2D; -#ifdef RTABMAP_PYMATCHER +#ifdef RTABMAP_PYTHON3 class PyMatcher; #endif @@ -105,7 +105,7 @@ class RTABMAP_EXP RegistrationVis : public Registration Feature2D * _detectorFrom; Feature2D * _detectorTo; -#ifdef RTABMAP_PYMATCHER +#ifdef RTABMAP_PYTHON3 PyMatcher * _pyMatcher; #endif }; diff --git a/corelib/src/CMakeLists.txt b/corelib/src/CMakeLists.txt index 67e9abf9de..ad93f5de3e 100644 --- a/corelib/src/CMakeLists.txt +++ b/corelib/src/CMakeLists.txt @@ -104,6 +104,8 @@ SET(SRC_FILES MarkerDetector.cpp + GlobalDescriptorExtractor.cpp + GainCompensator.cpp rtflann/ext/lz4.c @@ -196,10 +198,13 @@ IF(Python3_FOUND) SET(SRC_FILES ${SRC_FILES} pymatcher/PyMatcher.cpp + pydescriptor/PyDescriptor.cpp + PyUtil.cpp ) SET(INCLUDE_DIRS ${TORCH_INCLUDE_DIRS} ${CMAKE_CURRENT_SOURCE_DIR}/pymatcher + ${CMAKE_CURRENT_SOURCE_DIR}/pydescriptor ${INCLUDE_DIRS} ) ENDIF(Python3_FOUND) diff --git a/corelib/src/Features2d.cpp b/corelib/src/Features2d.cpp index e4048eefbf..89ffc19abd 100644 --- a/corelib/src/Features2d.cpp +++ b/corelib/src/Features2d.cpp @@ -617,12 +617,10 @@ Feature2D * Feature2D::create(Feature2D::Type type, const ParametersMap & parame #ifdef RTABMAP_NONFREE default: feature2D = new SURF(parameters); - type = Feature2D::kFeatureSurf; break; #else default: feature2D = new ORB(parameters); - type = Feature2D::kFeatureOrb; break; #endif diff --git a/corelib/src/GlobalDescriptorExtractor.cpp b/corelib/src/GlobalDescriptorExtractor.cpp new file mode 100644 index 0000000000..1317722d20 --- /dev/null +++ b/corelib/src/GlobalDescriptorExtractor.cpp @@ -0,0 +1,55 @@ +/* + * GlobalDescriptorExtractor.cpp + * + * Created on: Jul. 16, 2020 + * Author: mathieu + */ + + +#include "rtabmap/core/GlobalDescriptorExtractor.h" + +#ifdef RTABMAP_PYTHON3 +#include "pydescriptor/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) +{ +#ifdef RTABMAP_PYTHON3 + 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_PYTHON3 + case GlobalDescriptorExtractor::kPyDescriptor: + GlobalDescriptorExtractor = new PyDescriptor(parameters); + break; +#endif + default: + type = GlobalDescriptorExtractor::kUndef; + break; + } + return GlobalDescriptorExtractor; +} + +} + diff --git a/corelib/src/Memory.cpp b/corelib/src/Memory.cpp index 006c34d900..bb0ba5efb3 100644 --- a/corelib/src/Memory.cpp +++ b/corelib/src/Memory.cpp @@ -40,6 +40,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #include #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" @@ -121,7 +122,6 @@ Memory::Memory(const ParametersMap & parameters) : _linksChanged(false), _signaturesAdded(0), _allNodesInWM(true), - _badSignRatio(Parameters::defaultKpBadSignRatio()), _tfIdfLikelihoodUsed(Parameters::defaultKpTfIdfLikelihoodUsed()), _parallelized(Parameters::defaultKpParallelized()) @@ -129,6 +129,7 @@ Memory::Memory(const ParametersMap & parameters) : _feature2D = Feature2D::create(parameters); _vwd = new VWDictionary(parameters); _registrationPipeline = Registration::create(parameters); + _globalDescriptorExtractor = GlobalDescriptorExtractor::create(parameters); // for local scan matching, correspondences ratio should be two times higher as we expect more matches float corRatio = Parameters::defaultIcpCorrespondenceRatio(); @@ -695,6 +696,14 @@ void Memory::parseParameters(const ParametersMap & parameters) _markerDetector->parseParameters(params); } + int globalDescriptorStrategy = -1; + Parameters::parse(params, Parameters::kMemGlobalDescriptorStrategy(), globalDescriptorStrategy); + if(globalDescriptorStrategy != -1) + { + delete _globalDescriptorExtractor; + _globalDescriptorExtractor = GlobalDescriptorExtractor::create(parameters_); + } + // do this after all params are parsed // SLAM mode vs Localization mode iter = params.find(Parameters::kMemIncrementalMemory()); @@ -5172,7 +5181,13 @@ 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 globalDescriptors = data.globalDescriptors(); + if(_globalDescriptorExtractor) + { + globalDescriptors.push_back(_globalDescriptorExtractor->extract(inputData)); + } + s->sensorData().setGlobalDescriptors(globalDescriptors); t = timer.ticks(); if(stats) stats->addStatistic(Statistics::kTimingMemCompressing_data(), t*1000.0f); diff --git a/corelib/src/Parameters.cpp b/corelib/src/Parameters.cpp index 19a6608349..63785e0a8a 100644 --- a/corelib/src/Parameters.cpp +++ b/corelib/src/Parameters.cpp @@ -630,7 +630,7 @@ ParametersMap Parameters::parseArguments(int argc, char * argv[], bool onlyParam std::cout << str << std::setw(spacing - str.size()) << "false" << std::endl; #endif str = "With Python3:"; -#ifdef RTABMAP_PYMATCHER +#ifdef RTABMAP_PYTHON3 std::cout << str << std::setw(spacing - str.size()) << "true" << std::endl; #else std::cout << str << std::setw(spacing - str.size()) << "false" << std::endl; diff --git a/corelib/src/PyUtil.cpp b/corelib/src/PyUtil.cpp new file mode 100644 index 0000000000..8ce60a3abf --- /dev/null +++ b/corelib/src/PyUtil.cpp @@ -0,0 +1,97 @@ +/* + * PyUtil.cpp + * + * Created on: Jul. 16, 2020 + * Author: mathieu + */ + +#include "PyUtil.h" +#include +#include +#include +#include + +#define NPY_NO_DEPRECATED_API NPY_API_VERSION +#include + +namespace rtabmap { + +PyUtil PyUtil::instance_; +bool PyUtil::initialized_ = false; +UMutex PyUtil::mutex_; + +PyUtil::PyUtil() {} +void PyUtil::init() {UScopeMutex lock(mutex_); if(!initialized_)Py_Initialize(); initialized_=true;} +bool PyUtil::initialized() {return initialized_;} +PyUtil::~PyUtil() {if(initialized_) Py_Finalize();} + +std::string PyUtil::getTraceback() +{ + // Author: https://stackoverflow.com/questions/41268061/c-c-python-exception-traceback-not-being-generated + + PyObject* type; + PyObject* value; + PyObject* traceback; + + PyErr_Fetch(&type, &value, &traceback); + PyErr_NormalizeException(&type, &value, &traceback); + + std::string fcn = ""; + fcn += "def get_pretty_traceback(exc_type, exc_value, exc_tb):\n"; + fcn += " import sys, traceback\n"; + fcn += " lines = []\n"; + fcn += " lines = traceback.format_exception(exc_type, exc_value, exc_tb)\n"; + fcn += " output = '\\n'.join(lines)\n"; + fcn += " return output\n"; + + PyRun_SimpleString(fcn.c_str()); + PyObject* mod = PyImport_ImportModule("__main__"); + PyObject* method = PyObject_GetAttrString(mod, "get_pretty_traceback"); + PyObject* outStr = PyObject_CallObject(method, Py_BuildValue("OOO", type, value, traceback)); + std::string pretty = PyBytes_AsString(PyUnicode_AsASCIIString(outStr)); + + Py_DECREF(method); + Py_DECREF(outStr); + Py_DECREF(mod); + + return pretty; +} + +PyObject * PyUtil::importModule(const std::string & path) +{ + if(!UFile::exists(path) || UFile::getExtension(path).compare("py") != 0) + { + UERROR("Cannot initialize Python module, the path is not valid: \"%s\"", path.c_str()); + return 0; + } + + if(!PyUtil::initialized()) + { + PyUtil::init(); + } + + 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()); + PyObject * module = PyImport_Import(pName); + Py_DECREF(pName); + + if(!module) + { + UERROR("Module \"%s\" could not be imported! (File=\"%s\")", scriptName.c_str(), path.c_str()); + UERROR("%s", getTraceback().c_str()); + } + + return module; +} + +} + diff --git a/corelib/src/PyUtil.h b/corelib/src/PyUtil.h new file mode 100644 index 0000000000..0719fb6823 --- /dev/null +++ b/corelib/src/PyUtil.h @@ -0,0 +1,39 @@ +/* + * PythonUtil.h + * + * Created on: Jul. 16, 2020 + * Author: mathieu + */ + +#ifndef CORELIB_SRC_PYUTIL_H_ +#define CORELIB_SRC_PYUTIL_H_ + +#include +#include +#include + +namespace rtabmap { + +class PyUtil +{ +public: + virtual ~PyUtil(); + + static void init(); + static bool initialized(); + + static std::string getTraceback(); + static PyObject* importModule(const std::string & path); + +private: + PyUtil(); + + static bool initialized_; + static UMutex mutex_; + static PyUtil instance_; +}; + +} + + +#endif /* CORELIB_SRC_PYUTIL_H_ */ diff --git a/corelib/src/RegistrationVis.cpp b/corelib/src/RegistrationVis.cpp index e14156c290..ed1a5e25b8 100644 --- a/corelib/src/RegistrationVis.cpp +++ b/corelib/src/RegistrationVis.cpp @@ -51,7 +51,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #include -#ifdef RTABMAP_PYMATCHER +#ifdef RTABMAP_PYTHON3 #include #endif @@ -87,7 +87,7 @@ RegistrationVis::RegistrationVis(const ParametersMap & parameters, Registration _maxInliersMeanDistance(Parameters::defaultVisMeanInliersDistance()), _detectorFrom(0), _detectorTo(0) -#ifdef RTABMAP_PYMATCHER +#ifdef RTABMAP_PYTHON3 , _pyMatcher(0) #endif @@ -153,7 +153,7 @@ void RegistrationVis::parseParameters(const ParametersMap & parameters) if(_nnType == 6) { // verify that we have Python3 support -#ifndef RTABMAP_PYMATCHER +#ifndef RTABMAP_PYTHON3 UWARN("%s is set to 6 but RTAB-Map is not built with Python3 support, using default %d.", Parameters::kVisCorNNType().c_str(), Parameters::defaultVisCorNNType()); _nnType = Parameters::defaultVisCorNNType(); @@ -264,7 +264,7 @@ RegistrationVis::~RegistrationVis() { delete _detectorFrom; delete _detectorTo; -#ifdef RTABMAP_PYMATCHER +#ifdef RTABMAP_PYTHON3 delete _pyMatcher; #endif } @@ -1137,7 +1137,7 @@ Transform RegistrationVis::computeTransformationImpl( // match between all descriptors std::list fromWordIds; std::list toWordIds; -#ifdef RTABMAP_PYMATCHER +#ifdef RTABMAP_PYTHON3 if(_nnType == 5 || (_nnType == 6 && _pyMatcher) || _nnType==7) #else if(_nnType == 5 || _nnType == 7) // bruteforce cross check or GMS @@ -1158,7 +1158,7 @@ Transform RegistrationVis::computeTransformationImpl( { std::vector toWordIdsV(descriptorsTo.rows, 0); std::vector matches; -#ifdef RTABMAP_PYMATCHER +#ifdef RTABMAP_PYTHON3 if(_nnType == 6 && _pyMatcher && descriptorsTo.cols == descriptorsFrom.cols && descriptorsTo.rows == (int)kptsTo.size() && diff --git a/corelib/src/pydescriptor/PyDescriptor.cpp b/corelib/src/pydescriptor/PyDescriptor.cpp new file mode 100644 index 0000000000..e60ae316b5 --- /dev/null +++ b/corelib/src/pydescriptor/PyDescriptor.cpp @@ -0,0 +1,181 @@ + +#include +#include +#include +#include +#include +#include +#include +#include + +#define NPY_NO_DEPRECATED_API NPY_API_VERSION +#include + +namespace rtabmap +{ + +PyDescriptor::PyDescriptor( + const std::string & pythonDescriptorPath, + int dim) : + pModule_(0), + pFunc_(0), + dim_(dim) +{ + path_ = uReplaceChar(pythonDescriptorPath, '~', UDirectory::homeDir()); + UINFO("path = %s", path_.c_str()); + UINFO("dim = %d", dim_); + + pModule_ = PyUtil::importModule(path_); +} + +PyDescriptor::PyDescriptor( + const ParametersMap & parameters) : + GlobalDescriptorExtractor(parameters), + pModule_(0), + pFunc_(0), + dim_(4096) +{ + 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; + + pModule_ = PyUtil::importModule(path_); + + if(pModule_) + { + 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", PyUtil::getTraceback().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", PyUtil::getTraceback().c_str()); + if(pFunc_) + { + Py_DECREF(pFunc_); + pFunc_ = 0; + } + } + } + else + { + UERROR("Cannot call method \"init(...)\" in %s", path_.c_str()); + UERROR("%s", PyUtil::getTraceback().c_str()); + } + Py_DECREF(pFunc); + } + else + { + UERROR("Cannot find method \"init(...)\""); + UERROR("%s", PyUtil::getTraceback().c_str()); + } + UDEBUG("init time = %fs", timer.ticks()); + } +} + +PyDescriptor::~PyDescriptor() +{ + if(pFunc_) + { + Py_DECREF(pFunc_); + } + if(pModule_) + { + Py_DECREF(pModule_); + } +} + +GlobalDescriptor PyDescriptor::extract( + const SensorData & data) const +{ + UTimer timer; + GlobalDescriptor descriptor; + + if(!pModule_) + { + UERROR("Python module not loaded!"); + return descriptor; + } + if(!pFunc_) + { + UERROR("Python function not loaded!"); + return descriptor; + } + + if(!data.imageRaw().empty()) + { + std::vector 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_CHAR, (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", PyUtil::getTraceback().c_str()); + } + else + { + UDEBUG("Python extraction time = %fs", timer.ticks()); + + /* + PyArrayObject *np_ret = reinterpret_cast(pReturn); + + // Convert back to C++ array and print. + int len1 = PyArray_SHAPE(np_ret)[0]; + int len2 = PyArray_SHAPE(np_ret)[1]; + int type = PyArray_TYPE(np_ret); + UDEBUG("Matches array %dx%d (type=%d)", len1, len2, type); + UASSERT_MSG(type == NPY_LONG || type == NPY_INT, uFormat("Returned matches should type INT=5 or LONG=7, received type=%d", type).c_str()); + if(type == NPY_LONG) + { + long* c_out = reinterpret_cast(PyArray_DATA(np_ret)); + for (int i = 0; i < len1*len2; i+=2) + { + matches.push_back(cv::DMatch(c_out[i], c_out[i+1], 0)); + } + } + else // INT + { + int* c_out = reinterpret_cast(PyArray_DATA(np_ret)); + for (int i = 0; i < len1*len2; i+=2) + { + matches.push_back(cv::DMatch(c_out[i], c_out[i+1], 0)); + } + }*/ + Py_DECREF(pReturn); + } + + Py_DECREF(pImageQuery); + } + else + { + UERROR("Invalid inputs! Missing image."); + } + return descriptor; +} + +} diff --git a/corelib/src/pydescriptor/PyDescriptor.h b/corelib/src/pydescriptor/PyDescriptor.h new file mode 100644 index 0000000000..8e0d5fe312 --- /dev/null +++ b/corelib/src/pydescriptor/PyDescriptor.h @@ -0,0 +1,40 @@ +/** + * Python interface for python descriptors like: + * - NetVLAD: https://github.com/uzh-rpg/netvlad_tf_open + */ + +#ifndef PYDESCRIPTOR_H +#define PYDESCRIPTOR_H + +#include +#include +#include + +#include + +namespace rtabmap +{ + +class PyDescriptor : public GlobalDescriptorExtractor +{ +public: + PyDescriptor(const std::string & pythonDescriptorPath, int dim = 4096); + PyDescriptor(const ParametersMap & parameters); + virtual ~PyDescriptor(); + + const std::string & path() const {return path_;} + float dim() const {return dim_;} + + 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 diff --git a/corelib/src/pydescriptor/rtabmap_netvlad.py b/corelib/src/pydescriptor/rtabmap_netvlad.py new file mode 100644 index 0000000000..305fe6228f --- /dev/null +++ b/corelib/src/pydescriptor/rtabmap_netvlad.py @@ -0,0 +1,44 @@ +#! /usr/bin/env python3 +# +# Drop this file in the "demo" folder of OANet git: https://github.com/zjhthu/OANet +# To use with rtabmap: +# --Vis/CorNNType 6 --PyMatcher/Path ~/OANet/demo/rtabmap_oanet.py --PyMatcher/Model ~/OANet/model/gl3d/sift-4000/model_best.pth +# + +import sys +import os +sys.path.append(os.path.dirname(os.path.realpath(__file__))+'/../core') +if not hasattr(sys, 'argv'): + sys.argv = [''] + +#print(os.sys.path) +#print(sys.version) + +import numpy as np +from learnedmatcher import LearnedMatcher + +lm = None + +def init(descriptorDim, matchThreshold, iterations, cuda, model_path): + print("OANet python init()") + global lm + lm = LearnedMatcher(model_path, inlier_threshold=1, use_ratio=0, use_mutual=0) + + +def match(kptsFrom, kptsTo, scoresFrom, scoresTo, descriptorsFrom, descriptorsTo, imageWidth, imageHeight): + #print("OANet python match()") + + kpt1 = np.asarray(kptsFrom) + kpt2 = np.asarray(kptsTo) + desc1 = np.asarray(descriptorsFrom) + desc2 = np.asarray(descriptorsTo) + + global lm + matches, _, _ = lm.infer([kpt1, kpt2], [desc1, desc2]) + return matches + + +if __name__ == '__main__': + #test + init(128, 0.2, 20, False, True) + match([[1, 2], [1,3], [4,6]], [[1, 3], [1,2], [16,2]], [1, 3,6], [1,3,5], np.full((3, 128), 1), np.full((3, 128), 1), 640, 480) diff --git a/corelib/src/pymatcher/PyMatcher.cpp b/corelib/src/pymatcher/PyMatcher.cpp index 79a2d68a3d..0667849978 100644 --- a/corelib/src/pymatcher/PyMatcher.cpp +++ b/corelib/src/pymatcher/PyMatcher.cpp @@ -2,6 +2,7 @@ * Python interface for SuperGlue: https://github.com/magicleap/SuperGluePretrainedNetwork */ +#include #include #include #include @@ -16,52 +17,6 @@ namespace rtabmap { -class PythonSingleTon -{ -public: - PythonSingleTon() : initialized_(false) {} - void init() {UScopeMutex lock(mutex_); if(!initialized_)Py_Initialize(); initialized_=true;} - bool initialized() const {return initialized_;} - virtual ~PythonSingleTon() {if(initialized_) Py_Finalize();} -private: - bool initialized_; - UMutex mutex_; -}; - -static PythonSingleTon g_python; - -std::string getTraceback() -{ - // Author: https://stackoverflow.com/questions/41268061/c-c-python-exception-traceback-not-being-generated - - PyObject* type; - PyObject* value; - PyObject* traceback; - - PyErr_Fetch(&type, &value, &traceback); - PyErr_NormalizeException(&type, &value, &traceback); - - std::string fcn = ""; - fcn += "def get_pretty_traceback(exc_type, exc_value, exc_tb):\n"; - fcn += " import sys, traceback\n"; - fcn += " lines = []\n"; - fcn += " lines = traceback.format_exception(exc_type, exc_value, exc_tb)\n"; - fcn += " output = '\\n'.join(lines)\n"; - fcn += " return output\n"; - - PyRun_SimpleString(fcn.c_str()); - PyObject* mod = PyImport_ImportModule("__main__"); - PyObject* method = PyObject_GetAttrString(mod, "get_pretty_traceback"); - PyObject* outStr = PyObject_CallObject(method, Py_BuildValue("OOO", type, value, traceback)); - std::string pretty = PyBytes_AsString(PyUnicode_AsASCIIString(outStr)); - - Py_DECREF(method); - Py_DECREF(outStr); - Py_DECREF(mod); - - return pretty; -} - PyMatcher::PyMatcher( const std::string & pythonMatcherPath, float matchThreshold, @@ -79,36 +34,7 @@ PyMatcher::PyMatcher( UINFO("path = %s", path_.c_str()); UINFO("model = %s", model_.c_str()); - if(!UFile::exists(path_) || UFile::getExtension(path_).compare("py") != 0) - { - UERROR("Cannot initialize Python matcher, the path is not valid: \"%s\"", path_.c_str()); - return; - } - - if(!g_python.initialized()) - { - g_python.init(); - } - - 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()); - pModule_ = PyImport_Import(pName); - Py_DECREF(pName); - - if(!pModule_) - { - UERROR("Module \"%s\" could not be imported! (File=\"%s\")", scriptName.c_str(), path_.c_str()); - UERROR("%s", getTraceback().c_str()); - } + pModule_ = PyUtil::importModule(path_); } PyMatcher::~PyMatcher() @@ -162,7 +88,7 @@ std::vector PyMatcher::match( if(result == NULL) { UERROR("Call to \"init(...)\" in \"%s\" failed!", path_.c_str()); - UERROR("%s", getTraceback().c_str()); + UERROR("%s", PyUtil::getTraceback().c_str()); return matches; } Py_DECREF(result); @@ -175,7 +101,7 @@ std::vector PyMatcher::match( else { UERROR("Cannot find method \"match(...)\" in %s", path_.c_str()); - UERROR("%s", getTraceback().c_str()); + UERROR("%s", PyUtil::getTraceback().c_str()); if(pFunc_) { Py_DECREF(pFunc_); @@ -187,7 +113,7 @@ std::vector PyMatcher::match( else { UERROR("Cannot call method \"init(...)\" in %s", path_.c_str()); - UERROR("%s", getTraceback().c_str()); + UERROR("%s", PyUtil::getTraceback().c_str()); return matches; } Py_DECREF(pFunc); @@ -195,7 +121,7 @@ std::vector PyMatcher::match( else { UERROR("Cannot find method \"init(...)\""); - UERROR("%s", getTraceback().c_str()); + UERROR("%s", PyUtil::getTraceback().c_str()); return matches; } UDEBUG("init time = %fs", timer.ticks()); @@ -258,7 +184,7 @@ std::vector PyMatcher::match( if(pReturn == NULL) { UERROR("Failed to call match() function!"); - UERROR("%s", getTraceback().c_str()); + UERROR("%s", PyUtil::getTraceback().c_str()); } else { diff --git a/guilib/src/AboutDialog.cpp b/guilib/src/AboutDialog.cpp index 16258b6a64..cf99f71635 100644 --- a/guilib/src/AboutDialog.cpp +++ b/guilib/src/AboutDialog.cpp @@ -86,7 +86,7 @@ AboutDialog::AboutDialog(QWidget * parent) : _ui->label_sptorch->setText("No"); _ui->label_sptorch_license->setEnabled(false); #endif -#ifdef RTABMAP_PYMATCHER +#ifdef RTABMAP_PYTHON3 _ui->label_pymatcher->setText("Yes"); _ui->label_pymatcher_license->setEnabled(true); #else diff --git a/guilib/src/PreferencesDialog.cpp b/guilib/src/PreferencesDialog.cpp index 3cb7d38755..2bfcb7c561 100644 --- a/guilib/src/PreferencesDialog.cpp +++ b/guilib/src/PreferencesDialog.cpp @@ -226,7 +226,7 @@ PreferencesDialog::PreferencesDialog(QWidget * parent) : _ui->vis_feature_detector->setItemData(11, 0, Qt::UserRole - 1); #endif -#ifndef RTABMAP_PYMATCHER +#ifndef RTABMAP_PYTHON3 _ui->reextract_nn->setItemData(6, 0, Qt::UserRole - 1); #endif From ceed95698e4571a316e65c43dbd4b78cc3d86fbd Mon Sep 17 00:00:00 2001 From: matlabbe Date: Thu, 23 Jul 2020 12:21:29 -0400 Subject: [PATCH 2/8] Fixed Python refactoring errors --- corelib/src/PyUtil.cpp | 38 +++++++----- corelib/src/PyUtil.h | 11 ++-- corelib/src/pydescriptor/PyDescriptor.cpp | 5 ++ corelib/src/pydescriptor/rtabmap_netvlad.py | 68 ++++++++++++++------- corelib/src/pymatcher/PyMatcher.cpp | 5 ++ 5 files changed, 84 insertions(+), 43 deletions(-) diff --git a/corelib/src/PyUtil.cpp b/corelib/src/PyUtil.cpp index 8ce60a3abf..2e0cb2f738 100644 --- a/corelib/src/PyUtil.cpp +++ b/corelib/src/PyUtil.cpp @@ -11,19 +11,32 @@ #include #include -#define NPY_NO_DEPRECATED_API NPY_API_VERSION -#include - namespace rtabmap { -PyUtil PyUtil::instance_; -bool PyUtil::initialized_ = false; +size_t PyUtil::references_ = 0; UMutex PyUtil::mutex_; -PyUtil::PyUtil() {} -void PyUtil::init() {UScopeMutex lock(mutex_); if(!initialized_)Py_Initialize(); initialized_=true;} -bool PyUtil::initialized() {return initialized_;} -PyUtil::~PyUtil() {if(initialized_) Py_Finalize();} +void PyUtil::acquire() +{ + UScopeMutex lock(mutex_); + if(references_ == 0) + { + Py_Initialize(); + } + ++references_; +} +void PyUtil::release() +{ + UScopeMutex lock(mutex_); + if(references_>0) + { + --references_; + if(references_==0) + { + Py_Finalize(); + } + } +} std::string PyUtil::getTraceback() { @@ -65,11 +78,6 @@ PyObject * PyUtil::importModule(const std::string & path) return 0; } - if(!PyUtil::initialized()) - { - PyUtil::init(); - } - std::string matcherPythonDir = UDirectory::getDir(path); if(!matcherPythonDir.empty()) { @@ -77,8 +85,6 @@ PyObject * PyUtil::importModule(const std::string & path) 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()); PyObject * module = PyImport_Import(pName); diff --git a/corelib/src/PyUtil.h b/corelib/src/PyUtil.h index 0719fb6823..8271b26ca1 100644 --- a/corelib/src/PyUtil.h +++ b/corelib/src/PyUtil.h @@ -17,20 +17,19 @@ namespace rtabmap { class PyUtil { public: - virtual ~PyUtil(); + virtual ~PyUtil() {} - static void init(); - static bool initialized(); + static void acquire(); + static void release(); static std::string getTraceback(); static PyObject* importModule(const std::string & path); private: - PyUtil(); + PyUtil() {} - static bool initialized_; static UMutex mutex_; - static PyUtil instance_; + static size_t references_; }; } diff --git a/corelib/src/pydescriptor/PyDescriptor.cpp b/corelib/src/pydescriptor/PyDescriptor.cpp index e60ae316b5..2639f9d89d 100644 --- a/corelib/src/pydescriptor/PyDescriptor.cpp +++ b/corelib/src/pydescriptor/PyDescriptor.cpp @@ -25,7 +25,11 @@ PyDescriptor::PyDescriptor( UINFO("path = %s", path_.c_str()); UINFO("dim = %d", dim_); + PyUtil::acquire(); + pModule_ = PyUtil::importModule(path_); + + UASSERT(_import_array()==0); } PyDescriptor::PyDescriptor( @@ -102,6 +106,7 @@ PyDescriptor::~PyDescriptor() { Py_DECREF(pModule_); } + PyUtil::release(); } GlobalDescriptor PyDescriptor::extract( diff --git a/corelib/src/pydescriptor/rtabmap_netvlad.py b/corelib/src/pydescriptor/rtabmap_netvlad.py index 305fe6228f..fa5273d740 100644 --- a/corelib/src/pydescriptor/rtabmap_netvlad.py +++ b/corelib/src/pydescriptor/rtabmap_netvlad.py @@ -1,13 +1,13 @@ #! /usr/bin/env python3 # -# Drop this file in the "demo" folder of OANet git: https://github.com/zjhthu/OANet +# Drop this file in the "python" folder of NetVLAD git (tensorflow-v1 used): https://github.com/uzh-rpg/netvlad_tf_open/ # To use with rtabmap: -# --Vis/CorNNType 6 --PyMatcher/Path ~/OANet/demo/rtabmap_oanet.py --PyMatcher/Model ~/OANet/model/gl3d/sift-4000/model_best.pth +# --PyDescriptor/Dim 128 --PyDescriptor/Path ~/netvlad_tf_open/python/rtabmap_netvlad.py # import sys import os -sys.path.append(os.path.dirname(os.path.realpath(__file__))+'/../core') +sys.path.append(os.path.dirname(os.path.realpath(__file__))) if not hasattr(sys, 'argv'): sys.argv = [''] @@ -15,30 +15,56 @@ #print(sys.version) import numpy as np -from learnedmatcher import LearnedMatcher +import tensorflow as tf +import time -lm = None +import netvlad_tf.net_from_mat as nfm +import netvlad_tf.nets as nets -def init(descriptorDim, matchThreshold, iterations, cuda, model_path): - print("OANet python init()") - global lm - lm = LearnedMatcher(model_path, inlier_threshold=1, use_ratio=0, use_mutual=0) +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.reset_default_graph() + + image_batch = tf.placeholder( + dtype=tf.float32, shape=[None, None, None, 3]) + + net_out = nets.vgg16NetvladPca(image_batch) + saver = tf.train.Saver() -def match(kptsFrom, kptsTo, scoresFrom, scoresTo, descriptorsFrom, descriptorsTo, imageWidth, imageHeight): - #print("OANet python match()") + sess = tf.Session() + saver.restore(sess, nets.defaultCheckpoint()) + + +def extract(image): + print("NetVLAD python extract()") + global image_batch + global net_out + global sess + global dim + + batch = np.expand_dims(image, axis=0) + result = sess.run(net_out, feed_dict={image_batch: batch}) + result = result[:,:dim] - kpt1 = np.asarray(kptsFrom) - kpt2 = np.asarray(kptsTo) - desc1 = np.asarray(descriptorsFrom) - desc2 = np.asarray(descriptorsTo) - - global lm - matches, _, _ = lm.infer([kpt1, kpt2], [desc1, desc2]) - return matches + return result if __name__ == '__main__': #test - init(128, 0.2, 20, False, True) - match([[1, 2], [1,3], [4,6]], [[1, 3], [1,2], [16,2]], [1, 3,6], [1,3,5], np.full((3, 128), 1), np.full((3, 128), 1), 640, 480) + img = np.zeros([100,100,3],dtype=np.uint8) + img.fill(255) + print(extract(img)) diff --git a/corelib/src/pymatcher/PyMatcher.cpp b/corelib/src/pymatcher/PyMatcher.cpp index 0667849978..52898d9804 100644 --- a/corelib/src/pymatcher/PyMatcher.cpp +++ b/corelib/src/pymatcher/PyMatcher.cpp @@ -34,7 +34,11 @@ PyMatcher::PyMatcher( UINFO("path = %s", path_.c_str()); UINFO("model = %s", model_.c_str()); + PyUtil::acquire(); + pModule_ = PyUtil::importModule(path_); + + UASSERT(_import_array()==0); } PyMatcher::~PyMatcher() @@ -47,6 +51,7 @@ PyMatcher::~PyMatcher() { Py_DECREF(pModule_); } + PyUtil::release(); } std::vector PyMatcher::match( From 6c93d07b210d66fa7ee6eac7086aeaaa2bceccb5 Mon Sep 17 00:00:00 2001 From: matlabbe Date: Thu, 23 Jul 2020 15:14:20 -0400 Subject: [PATCH 3/8] GUI: added PyDescriptor parameters --- CMakeLists.txt | 2 +- .../rtabmap/core/GlobalDescriptorExtractor.h | 4 +- corelib/include/rtabmap/core/Parameters.h | 2 +- corelib/src/GlobalDescriptorExtractor.cpp | 3 +- corelib/src/Memory.cpp | 18 ++- corelib/src/pydescriptor/PyDescriptor.cpp | 138 ++++++++++-------- corelib/src/pydescriptor/PyDescriptor.h | 4 +- corelib/src/pydescriptor/rtabmap_netvlad.py | 7 +- .../include/rtabmap/gui/PreferencesDialog.h | 2 + guilib/src/PreferencesDialog.cpp | 33 +++++ guilib/src/ui/preferencesDialog.ui | 130 ++++++++++++++++- 11 files changed, 266 insertions(+), 77 deletions(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index c6d15cd756..e17153c1f1 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -21,7 +21,7 @@ SET(CMAKE_MODULE_PATH "${PROJECT_SOURCE_DIR}/cmake_modules") ####################### SET(RTABMAP_MAJOR_VERSION 0) SET(RTABMAP_MINOR_VERSION 20) -SET(RTABMAP_PATCH_VERSION 2) +SET(RTABMAP_PATCH_VERSION 3) SET(RTABMAP_VERSION ${RTABMAP_MAJOR_VERSION}.${RTABMAP_MINOR_VERSION}.${RTABMAP_PATCH_VERSION}) diff --git a/corelib/include/rtabmap/core/GlobalDescriptorExtractor.h b/corelib/include/rtabmap/core/GlobalDescriptorExtractor.h index 6c65d1f61d..02de98cd60 100644 --- a/corelib/include/rtabmap/core/GlobalDescriptorExtractor.h +++ b/corelib/include/rtabmap/core/GlobalDescriptorExtractor.h @@ -44,8 +44,8 @@ namespace rtabmap { class RTABMAP_EXP GlobalDescriptorExtractor { public: enum Type { - kUndef=-1, - kPyDescriptor=0}; + kUndef=0, + kPyDescriptor=1}; static std::string typeName(Type type) { diff --git a/corelib/include/rtabmap/core/Parameters.h b/corelib/include/rtabmap/core/Parameters.h index f30135dd90..ea4dbaedcd 100644 --- a/corelib/include/rtabmap/core/Parameters.h +++ b/corelib/include/rtabmap/core/Parameters.h @@ -230,7 +230,7 @@ class RTABMAP_EXP Parameters RTABMAP_PARAM(Mem, UseOdomFeatures, bool, true, "Use odometry features instead of regenerating them."); RTABMAP_PARAM(Mem, UseOdomGravity, bool, false, uFormat("Use odometry instead of IMU orientation to add gravity links to new nodes created. We assume that odometry is already aligned with gravity (e.g., we are using a VIO approach). Gravity constraints are used by graph optimization only if \"%s\" is not zero.", kOptimizerGravitySigma().c_str())); RTABMAP_PARAM(Mem, CovOffDiagIgnored, bool, true, "Ignore off diagonal values of the covariance matrix."); - RTABMAP_PARAM(Mem, GlobalDescriptorStrategy, int, -1, "Extract global descriptor from sensor data. -1=not used, 0=PyDescriptor"); + RTABMAP_PARAM(Mem, GlobalDescriptorStrategy, int, 0, "Extract global descriptor from sensor data. 0=disabled, 1=PyDescriptor"); // KeypointMemory (Keypoint-based) RTABMAP_PARAM(Kp, NNStrategy, int, 1, "kNNFlannNaive=0, kNNFlannKdTree=1, kNNFlannLSH=2, kNNBruteForce=3, kNNBruteForceGPU=4"); diff --git a/corelib/src/GlobalDescriptorExtractor.cpp b/corelib/src/GlobalDescriptorExtractor.cpp index 1317722d20..6b9190c47a 100644 --- a/corelib/src/GlobalDescriptorExtractor.cpp +++ b/corelib/src/GlobalDescriptorExtractor.cpp @@ -28,7 +28,8 @@ GlobalDescriptorExtractor * GlobalDescriptorExtractor::create(const ParametersMa } GlobalDescriptorExtractor * GlobalDescriptorExtractor::create(GlobalDescriptorExtractor::Type type, const ParametersMap & parameters) { -#ifdef RTABMAP_PYTHON3 + UDEBUG("Creating global descriptor of type %d", (int)type); +#ifndef RTABMAP_PYTHON3 if(type == GlobalDescriptorExtractor::kPyDescriptor) { UWARN("PyDescriptor cannot be used as rtabmap is not built with Python3 support."); diff --git a/corelib/src/Memory.cpp b/corelib/src/Memory.cpp index bb0ba5efb3..9dd9601dc9 100644 --- a/corelib/src/Memory.cpp +++ b/corelib/src/Memory.cpp @@ -698,11 +698,19 @@ void Memory::parseParameters(const ParametersMap & parameters) int globalDescriptorStrategy = -1; Parameters::parse(params, Parameters::kMemGlobalDescriptorStrategy(), globalDescriptorStrategy); - if(globalDescriptorStrategy != -1) + if(globalDescriptorStrategy != -1 && + (_globalDescriptorExtractor==0 || (int)_globalDescriptorExtractor->getType() != globalDescriptorStrategy)) { - delete _globalDescriptorExtractor; + 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 @@ -5185,7 +5193,11 @@ Signature * Memory::createSignature(const SensorData & inputData, const Transfor std::vector globalDescriptors = data.globalDescriptors(); if(_globalDescriptorExtractor) { - globalDescriptors.push_back(_globalDescriptorExtractor->extract(inputData)); + GlobalDescriptor gdescriptor = _globalDescriptorExtractor->extract(inputData); + if(!gdescriptor.data().empty()) + { + globalDescriptors.push_back(gdescriptor); + } } s->sensorData().setGlobalDescriptors(globalDescriptors); diff --git a/corelib/src/pydescriptor/PyDescriptor.cpp b/corelib/src/pydescriptor/PyDescriptor.cpp index 2639f9d89d..1503d25a86 100644 --- a/corelib/src/pydescriptor/PyDescriptor.cpp +++ b/corelib/src/pydescriptor/PyDescriptor.cpp @@ -15,100 +15,116 @@ namespace rtabmap { PyDescriptor::PyDescriptor( - const std::string & pythonDescriptorPath, - int dim) : + const ParametersMap & parameters) : + GlobalDescriptorExtractor(parameters), pModule_(0), pFunc_(0), - dim_(dim) + dim_(Parameters::defaultPyDescriptorDim()) { - path_ = uReplaceChar(pythonDescriptorPath, '~', UDirectory::homeDir()); - UINFO("path = %s", path_.c_str()); - UINFO("dim = %d", dim_); - PyUtil::acquire(); + UASSERT(_import_array()==0); - pModule_ = PyUtil::importModule(path_); + this->parseParameters(parameters); +} - UASSERT(_import_array()==0); +PyDescriptor::~PyDescriptor() +{ + if(pFunc_) + { + Py_DECREF(pFunc_); + } + if(pModule_) + { + Py_DECREF(pModule_); + } + PyUtil::release(); } -PyDescriptor::PyDescriptor( - const ParametersMap & parameters) : - GlobalDescriptorExtractor(parameters), - pModule_(0), - pFunc_(0), - dim_(4096) +void PyDescriptor::parseParameters(const ParametersMap & parameters) { - Parameters::parse(parameters, Parameters::kPyDescriptorPath(), path_); + std::string path; + Parameters::parse(parameters, Parameters::kPyDescriptorPath(), path); Parameters::parse(parameters, Parameters::kPyDescriptorDim(), dim_); - path_ = uReplaceChar(path_, '~', UDirectory::homeDir()); - UINFO("path = %s", path_.c_str()); + path = uReplaceChar(path, '~', UDirectory::homeDir()); + UINFO("path = %s", path.c_str()); UINFO("dim = %d", dim_); UTimer timer; - pModule_ = PyUtil::importModule(path_); - if(pModule_) { - PyObject * pFunc = PyObject_GetAttrString(pModule_, "init"); - if(pFunc) + if(!path.empty() && path.compare(path_)!=0) { - if(PyCallable_Check(pFunc)) + UDEBUG("we changed script (old=%s), we need to reload (new=%s)", + path_.c_str(), path.c_str()); + if(pFunc_) { - PyObject * result = PyObject_CallFunction(pFunc, "i", dim_); + Py_DECREF(pFunc_); + } + pFunc_=0; + Py_DECREF(pModule_); + pModule_ = 0; + path_.clear(); + } + } - if(result == NULL) - { - UERROR("Call to \"init(...)\" in \"%s\" failed!", path_.c_str()); - UERROR("%s", PyUtil::getTraceback().c_str()); - } - Py_DECREF(result); + if(pModule_==0) + { + if(path.empty()) + { + return; + } + pModule_ = PyUtil::importModule(path); - pFunc_ = PyObject_GetAttrString(pModule_, "extract"); - if(pFunc_ && PyCallable_Check(pFunc_)) + if(pModule_) + { + path_ = path; + PyObject * pFunc = PyObject_GetAttrString(pModule_, "init"); + if(pFunc) + { + if(PyCallable_Check(pFunc)) { - // we are ready! + PyObject * result = PyObject_CallFunction(pFunc, "i", dim_); + + if(result == NULL) + { + UERROR("Call to \"init(...)\" in \"%s\" failed!", path_.c_str()); + UERROR("%s", PyUtil::getTraceback().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", PyUtil::getTraceback().c_str()); + if(pFunc_) + { + Py_DECREF(pFunc_); + pFunc_ = 0; + } + } } else { - UERROR("Cannot find method \"extract(...)\" in %s", path_.c_str()); + UERROR("Cannot call method \"init(...)\" in %s", path_.c_str()); UERROR("%s", PyUtil::getTraceback().c_str()); - if(pFunc_) - { - Py_DECREF(pFunc_); - pFunc_ = 0; - } } + Py_DECREF(pFunc); } else { - UERROR("Cannot call method \"init(...)\" in %s", path_.c_str()); + UERROR("Cannot find method \"init(...)\""); UERROR("%s", PyUtil::getTraceback().c_str()); } - Py_DECREF(pFunc); + UDEBUG("init time = %fs", timer.ticks()); } - else - { - UERROR("Cannot find method \"init(...)\""); - UERROR("%s", PyUtil::getTraceback().c_str()); - } - UDEBUG("init time = %fs", timer.ticks()); } } -PyDescriptor::~PyDescriptor() -{ - if(pFunc_) - { - Py_DECREF(pFunc_); - } - if(pModule_) - { - Py_DECREF(pModule_); - } - PyUtil::release(); -} - GlobalDescriptor PyDescriptor::extract( const SensorData & data) const { @@ -131,7 +147,7 @@ GlobalDescriptor PyDescriptor::extract( std::vector 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_CHAR, (void*)data.imageRaw().data); + PyObject* pImageQuery = PyArray_SimpleNewFromData(3, dimsFrom, NPY_BYTE, (void*)data.imageRaw().data); UASSERT(pImageQuery); UDEBUG("Preparing data time = %fs", timer.ticks()); diff --git a/corelib/src/pydescriptor/PyDescriptor.h b/corelib/src/pydescriptor/PyDescriptor.h index 8e0d5fe312..5dd4767e9f 100644 --- a/corelib/src/pydescriptor/PyDescriptor.h +++ b/corelib/src/pydescriptor/PyDescriptor.h @@ -18,13 +18,13 @@ namespace rtabmap class PyDescriptor : public GlobalDescriptorExtractor { public: - PyDescriptor(const std::string & pythonDescriptorPath, int dim = 4096); - PyDescriptor(const ParametersMap & parameters); + 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;} diff --git a/corelib/src/pydescriptor/rtabmap_netvlad.py b/corelib/src/pydescriptor/rtabmap_netvlad.py index fa5273d740..4d4e34a2a3 100644 --- a/corelib/src/pydescriptor/rtabmap_netvlad.py +++ b/corelib/src/pydescriptor/rtabmap_netvlad.py @@ -7,6 +7,8 @@ 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 = [''] @@ -14,10 +16,7 @@ #print(os.sys.path) #print(sys.version) -import numpy as np import tensorflow as tf -import time - import netvlad_tf.net_from_mat as nfm import netvlad_tf.nets as nets @@ -56,6 +55,8 @@ def extract(image): global sess global dim + print(image.shape) + batch = np.expand_dims(image, axis=0) result = sess.run(net_out, feed_dict={image_batch: batch}) result = result[:,:dim] diff --git a/guilib/include/rtabmap/gui/PreferencesDialog.h b/guilib/include/rtabmap/gui/PreferencesDialog.h index 8344538d06..d7b782dd44 100644 --- a/guilib/include/rtabmap/gui/PreferencesDialog.h +++ b/guilib/include/rtabmap/gui/PreferencesDialog.h @@ -331,6 +331,7 @@ private Q_SLOTS: void updateKpROI(); void updateStereoDisparityVisibility(); void updateFeatureMatchingVisibility(); + void updateGlobalDescriptorVisibility(); void useOdomFeatures(); void changeWorkingDirectory(); void changeDictionaryPath(); @@ -341,6 +342,7 @@ private Q_SLOTS: void changeSuperPointModelPath(); void changePyMatcherPath(); void changePyMatcherModel(); + void changePyDescriptorPath(); void readSettingsEnd(); void setupTreeView(); void updateBasicParameter(); diff --git a/guilib/src/PreferencesDialog.cpp b/guilib/src/PreferencesDialog.cpp index 2bfcb7c561..9ec9316e12 100644 --- a/guilib/src/PreferencesDialog.cpp +++ b/guilib/src/PreferencesDialog.cpp @@ -228,6 +228,7 @@ PreferencesDialog::PreferencesDialog(QWidget * parent) : #ifndef RTABMAP_PYTHON3 _ui->reextract_nn->setItemData(6, 0, Qt::UserRole - 1); + _ui->comboBox_globalDescriptorExtractor->setItemData(1, 0, Qt::UserRole - 1); #endif #if !defined(HAVE_OPENCV_XFEATURES2D) || (CV_MAJOR_VERSION == 3 && (CV_MINOR_VERSION<4 || CV_MINOR_VERSION==4 && CV_SUBMINOR_VERSION<1)) @@ -866,6 +867,8 @@ PreferencesDialog::PreferencesDialog(QWidget * parent) : // Create hypotheses _ui->general_doubleSpinBox_hardThr->setObjectName(Parameters::kRtabmapLoopThr().c_str()); _ui->general_doubleSpinBox_loopRatio->setObjectName(Parameters::kRtabmapLoopRatio().c_str()); + _ui->comboBox_globalDescriptorExtractor->setObjectName(Parameters::kMemGlobalDescriptorStrategy().c_str()); + connect(_ui->comboBox_globalDescriptorExtractor, SIGNAL(currentIndexChanged(int)), this, SLOT(updateGlobalDescriptorVisibility())); //Bayes _ui->general_doubleSpinBox_vp->setObjectName(Parameters::kBayesVirtualPlacePriorThr().c_str()); @@ -991,6 +994,11 @@ PreferencesDialog::PreferencesDialog(QWidget * parent) : _ui->checkBox_gms_withScale->setObjectName(Parameters::kGMSWithScale().c_str()); _ui->gms_thresholdFactor->setObjectName(Parameters::kGMSThresholdFactor().c_str()); + // PyDescriptor + _ui->lineEdit_pydescriptor_path->setObjectName(Parameters::kPyDescriptorPath().c_str()); + connect(_ui->toolButton_pydescriptor_path, SIGNAL(clicked()), this, SLOT(changePyDescriptorPath())); + _ui->pydescriptor_dim->setObjectName(Parameters::kPyDescriptorDim().c_str()); + // verifyHypotheses _ui->groupBox_vh_epipolar2->setObjectName(Parameters::kVhEpEnabled().c_str()); _ui->surf_spinBox_matchCountMinAccepted->setObjectName(Parameters::kVhEpMatchCountMin().c_str()); @@ -1393,6 +1401,9 @@ PreferencesDialog::PreferencesDialog(QWidget * parent) : this->setupTreeView(); _obsoletePanels = kPanelAll; + + updateFeatureMatchingVisibility(); + updateGlobalDescriptorVisibility(); } PreferencesDialog::~PreferencesDialog() { @@ -4685,6 +4696,11 @@ void PreferencesDialog::updateFeatureMatchingVisibility() _ui->groupBox_gms->setVisible(_ui->reextract_nn->currentIndex() == 7); } +void PreferencesDialog::updateGlobalDescriptorVisibility() +{ + _ui->groupBox_pydescriptor->setVisible(_ui->comboBox_globalDescriptorExtractor->currentIndex() == 1); +} + void PreferencesDialog::useOdomFeatures() { if(this->isVisible() && _ui->checkBox_useOdomFeatures->isChecked()) @@ -4855,6 +4871,23 @@ void PreferencesDialog::changePyMatcherModel() } } +void PreferencesDialog::changePyDescriptorPath() +{ + QString path; + if(_ui->lineEdit_pydescriptor_path->text().isEmpty()) + { + path = QFileDialog::getOpenFileName(this, tr("Select file"), this->getWorkingDirectory(), tr("Python wrapper (*.py)")); + } + else + { + path = QFileDialog::getOpenFileName(this, tr("Select file"), _ui->lineEdit_pydescriptor_path->text(), tr("Python wrapper (*.py)")); + } + if(!path.isEmpty()) + { + _ui->lineEdit_pydescriptor_path->setText(path); + } +} + void PreferencesDialog::updateSourceGrpVisibility() { _ui->groupBox_sourceRGBD->setVisible(_ui->comboBox_sourceType->currentIndex() == 0); diff --git a/guilib/src/ui/preferencesDialog.ui b/guilib/src/ui/preferencesDialog.ui index 197e0cd11a..06bc613411 100644 --- a/guilib/src/ui/preferencesDialog.ui +++ b/guilib/src/ui/preferencesDialog.ui @@ -63,7 +63,7 @@ 0 - -360 + -92 680 3270 @@ -95,7 +95,7 @@ QFrame::Raised - 5 + 11 @@ -9971,7 +9971,7 @@ see Sqlite3 doc 'PRAGMA synchronous'. Loop Closure Detection - + @@ -10132,6 +10132,36 @@ see Sqlite3 doc 'PRAGMA synchronous'. + + + + Global descriptor extractor. + + + true + + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + + + + + + + QComboBox::AdjustToContents + + + + Disabled + + + + + PyDescriptor + + + + @@ -10230,6 +10260,100 @@ see Sqlite3 doc 'PRAGMA synchronous'. + + + + PyDescriptor + + + + + + <html><head/><body><p>Python wrapper for python descriptor like <a href="https://github.com/uzh-rpg/netvlad_tf_open"><span style=" text-decoration: underline; color:#0000ff;">NetVLAD</span></a>. Download the interface <a href="https://github.com/introlab/rtabmap/tree/master/corelib/src/pydescriptor"><span style=" text-decoration: underline; color:#0000ff;">script</span></a> and copy it in the python folder of NetVLAD, then set its path below.</p></body></html> + + + true + + + true + + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + + + + + + + + + ... + + + + + + + 0 + + + 999999 + + + 1 + + + 4096 + + + + + + + + + + [Required] Path to rtabmap python script. + + + true + + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + + + + + + + Descriptor dimension. + + + true + + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + + + + + + + + + Qt::Vertical + + + + 0 + 0 + + + + + + + From 321caf6178ff94a91554896089ae1f41a8b9956d Mon Sep 17 00:00:00 2001 From: matlabbe Date: Mon, 27 Jul 2020 16:26:08 -0400 Subject: [PATCH 4/8] reordered python3 includes --- corelib/src/CMakeLists.txt | 15 +++++++-------- corelib/src/PyUtil.cpp | 2 ++ corelib/src/pydescriptor/.gitignore | 1 + corelib/src/pymatcher/.gitignore | 1 + 4 files changed, 11 insertions(+), 8 deletions(-) create mode 100644 corelib/src/pydescriptor/.gitignore create mode 100644 corelib/src/pymatcher/.gitignore diff --git a/corelib/src/CMakeLists.txt b/corelib/src/CMakeLists.txt index ad93f5de3e..a3f29cb1b8 100644 --- a/corelib/src/CMakeLists.txt +++ b/corelib/src/CMakeLists.txt @@ -176,24 +176,24 @@ ENDIF() IF(TORCH_FOUND) SET(LIBRARIES - ${LIBRARIES} ${TORCH_LIBRARIES} + ${LIBRARIES} ) SET(SRC_FILES ${SRC_FILES} superpoint_torch/SuperPoint.cc ) SET(INCLUDE_DIRS - ${TORCH_INCLUDE_DIRS} - ${CMAKE_CURRENT_SOURCE_DIR}/superpoint_torch + ${TORCH_INCLUDE_DIRS} + ${CMAKE_CURRENT_SOURCE_DIR}/superpoint_torch ${INCLUDE_DIRS} ) ENDIF(TORCH_FOUND) IF(Python3_FOUND) SET(LIBRARIES - ${LIBRARIES} Python3::Python + ${LIBRARIES} ) SET(SRC_FILES ${SRC_FILES} @@ -201,10 +201,9 @@ IF(Python3_FOUND) pydescriptor/PyDescriptor.cpp PyUtil.cpp ) - SET(INCLUDE_DIRS - ${TORCH_INCLUDE_DIRS} - ${CMAKE_CURRENT_SOURCE_DIR}/pymatcher - ${CMAKE_CURRENT_SOURCE_DIR}/pydescriptor + SET(INCLUDE_DIRS + ${CMAKE_CURRENT_SOURCE_DIR}/pymatcher + ${CMAKE_CURRENT_SOURCE_DIR}/pydescriptor ${INCLUDE_DIRS} ) ENDIF(Python3_FOUND) diff --git a/corelib/src/PyUtil.cpp b/corelib/src/PyUtil.cpp index 2e0cb2f738..ee0d1470cd 100644 --- a/corelib/src/PyUtil.cpp +++ b/corelib/src/PyUtil.cpp @@ -19,6 +19,7 @@ UMutex PyUtil::mutex_; void PyUtil::acquire() { UScopeMutex lock(mutex_); + UDEBUG("references_=%d", (int)references_); if(references_ == 0) { Py_Initialize(); @@ -28,6 +29,7 @@ void PyUtil::acquire() void PyUtil::release() { UScopeMutex lock(mutex_); + UDEBUG("references_=%d", (int)references_); if(references_>0) { --references_; diff --git a/corelib/src/pydescriptor/.gitignore b/corelib/src/pydescriptor/.gitignore new file mode 100644 index 0000000000..77ebff9b52 --- /dev/null +++ b/corelib/src/pydescriptor/.gitignore @@ -0,0 +1 @@ +__* diff --git a/corelib/src/pymatcher/.gitignore b/corelib/src/pymatcher/.gitignore new file mode 100644 index 0000000000..77ebff9b52 --- /dev/null +++ b/corelib/src/pymatcher/.gitignore @@ -0,0 +1 @@ +__* From 651037ae52ec5114d1ed40ec3cd1ff08ad1de15e Mon Sep 17 00:00:00 2001 From: matlabbe Date: Mon, 27 Jul 2020 16:30:36 -0400 Subject: [PATCH 5/8] updated rtabmap_netvlad.py test main --- corelib/src/pydescriptor/rtabmap_netvlad.py | 1 + 1 file changed, 1 insertion(+) diff --git a/corelib/src/pydescriptor/rtabmap_netvlad.py b/corelib/src/pydescriptor/rtabmap_netvlad.py index 4d4e34a2a3..9a1f2962a8 100644 --- a/corelib/src/pydescriptor/rtabmap_netvlad.py +++ b/corelib/src/pydescriptor/rtabmap_netvlad.py @@ -68,4 +68,5 @@ def extract(image): #test img = np.zeros([100,100,3],dtype=np.uint8) img.fill(255) + init(128) print(extract(img)) From e05488fe7c94de0e2abcef44646a1f61803d5ef6 Mon Sep 17 00:00:00 2001 From: matlabbe Date: Sun, 31 Mar 2024 18:10:24 -0700 Subject: [PATCH 6/8] fixed build from last merge --- .../include/rtabmap/core/GlobalDescriptor.h | 2 +- .../rtabmap/core/GlobalDescriptorExtractor.h | 10 +- corelib/src/CMakeLists.txt | 6 +- corelib/src/Features2d.cpp | 1 + corelib/src/GlobalDescriptorExtractor.cpp | 36 ++++-- corelib/src/PyUtil.cpp | 105 ------------------ corelib/src/PyUtil.h | 38 ------- corelib/src/python/.gitignore | 1 - corelib/src/python/PyDescriptor.cpp | 74 +++++++----- corelib/src/python/PyDescriptor.h | 4 +- corelib/src/python/PyDetector.h | 9 +- corelib/src/python/PyMatcher.cpp | 1 - guilib/src/PreferencesDialog.cpp | 5 - guilib/src/ui/preferencesDialog.ui | 4 +- 14 files changed, 91 insertions(+), 205 deletions(-) delete mode 100644 corelib/src/PyUtil.cpp delete mode 100644 corelib/src/PyUtil.h delete mode 100644 corelib/src/python/.gitignore diff --git a/corelib/include/rtabmap/core/GlobalDescriptor.h b/corelib/include/rtabmap/core/GlobalDescriptor.h index 2dcff1b048..9940043e06 100644 --- a/corelib/include/rtabmap/core/GlobalDescriptor.h +++ b/corelib/include/rtabmap/core/GlobalDescriptor.h @@ -1,5 +1,5 @@ /* -Copyright (c) 2010-2020, Mathieu Labbe - IntRoLab - Universite de Sherbrooke +Copyright (c) 2010-2024, Mathieu Labbe - IntRoLab - Universite de Sherbrooke All rights reserved. Redistribution and use in source and binary forms, with or without diff --git a/corelib/include/rtabmap/core/GlobalDescriptorExtractor.h b/corelib/include/rtabmap/core/GlobalDescriptorExtractor.h index 02de98cd60..7a95940392 100644 --- a/corelib/include/rtabmap/core/GlobalDescriptorExtractor.h +++ b/corelib/include/rtabmap/core/GlobalDescriptorExtractor.h @@ -1,5 +1,5 @@ /* -Copyright (c) 2010-2016, Mathieu Labbe - IntRoLab - Universite de Sherbrooke +Copyright (c) 2010-2024, Mathieu Labbe - IntRoLab - Universite de Sherbrooke All rights reserved. Redistribution and use in source and binary forms, with or without @@ -28,12 +28,8 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #ifndef GLOBAL_DESCRIPTOR_EXTRACTOR_H_ #define GLOBAL_DESCRIPTOR_EXTRACTOR_H_ -#include "rtabmap/core/RtabmapExp.h" // DLL export/import defines +#include "rtabmap/core/rtabmap_core_export.h" // DLL export/import defines -#include -#include -#include -#include #include "rtabmap/core/Parameters.h" #include "rtabmap/core/SensorData.h" @@ -41,7 +37,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. namespace rtabmap { // Feature2D -class RTABMAP_EXP GlobalDescriptorExtractor { +class RTABMAP_CORE_EXPORT GlobalDescriptorExtractor { public: enum Type { kUndef=0, diff --git a/corelib/src/CMakeLists.txt b/corelib/src/CMakeLists.txt index 2b6f4fff00..39a59ae0c8 100644 --- a/corelib/src/CMakeLists.txt +++ b/corelib/src/CMakeLists.txt @@ -197,16 +197,16 @@ ENDIF() IF(TORCH_FOUND) SET(LIBRARIES - ${TORCH_LIBRARIES} ${LIBRARIES} + ${TORCH_LIBRARIES} ) SET(SRC_FILES ${SRC_FILES} superpoint_torch/SuperPoint.cc ) SET(INCLUDE_DIRS - ${TORCH_INCLUDE_DIRS} - ${CMAKE_CURRENT_SOURCE_DIR}/superpoint_torch + ${TORCH_INCLUDE_DIRS} + ${CMAKE_CURRENT_SOURCE_DIR}/superpoint_torch ${INCLUDE_DIRS} ) ENDIF(TORCH_FOUND) diff --git a/corelib/src/Features2d.cpp b/corelib/src/Features2d.cpp index 5a10c2d450..add45c75cc 100644 --- a/corelib/src/Features2d.cpp +++ b/corelib/src/Features2d.cpp @@ -654,6 +654,7 @@ Feature2D * Feature2D::create(Feature2D::Type type, const ParametersMap & parame #ifdef RTABMAP_NONFREE default: feature2D = new SURF(parameters); + type = Feature2D::kFeatureSurf; break; #else default: diff --git a/corelib/src/GlobalDescriptorExtractor.cpp b/corelib/src/GlobalDescriptorExtractor.cpp index 6b9190c47a..43084f0cb4 100644 --- a/corelib/src/GlobalDescriptorExtractor.cpp +++ b/corelib/src/GlobalDescriptorExtractor.cpp @@ -1,15 +1,35 @@ /* - * GlobalDescriptorExtractor.cpp - * - * Created on: Jul. 16, 2020 - * Author: mathieu - */ +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_PYTHON3 -#include "pydescriptor/PyDescriptor.h" +#ifdef RTABMAP_PYTHON +#include "python/PyDescriptor.h" #endif namespace rtabmap { @@ -40,7 +60,7 @@ GlobalDescriptorExtractor * GlobalDescriptorExtractor::create(GlobalDescriptorEx GlobalDescriptorExtractor * GlobalDescriptorExtractor = 0; switch(type) { -#ifdef RTABMAP_PYTHON3 +#ifdef RTABMAP_PYTHON case GlobalDescriptorExtractor::kPyDescriptor: GlobalDescriptorExtractor = new PyDescriptor(parameters); break; diff --git a/corelib/src/PyUtil.cpp b/corelib/src/PyUtil.cpp deleted file mode 100644 index ee0d1470cd..0000000000 --- a/corelib/src/PyUtil.cpp +++ /dev/null @@ -1,105 +0,0 @@ -/* - * PyUtil.cpp - * - * Created on: Jul. 16, 2020 - * Author: mathieu - */ - -#include "PyUtil.h" -#include -#include -#include -#include - -namespace rtabmap { - -size_t PyUtil::references_ = 0; -UMutex PyUtil::mutex_; - -void PyUtil::acquire() -{ - UScopeMutex lock(mutex_); - UDEBUG("references_=%d", (int)references_); - if(references_ == 0) - { - Py_Initialize(); - } - ++references_; -} -void PyUtil::release() -{ - UScopeMutex lock(mutex_); - UDEBUG("references_=%d", (int)references_); - if(references_>0) - { - --references_; - if(references_==0) - { - Py_Finalize(); - } - } -} - -std::string PyUtil::getTraceback() -{ - // Author: https://stackoverflow.com/questions/41268061/c-c-python-exception-traceback-not-being-generated - - PyObject* type; - PyObject* value; - PyObject* traceback; - - PyErr_Fetch(&type, &value, &traceback); - PyErr_NormalizeException(&type, &value, &traceback); - - std::string fcn = ""; - fcn += "def get_pretty_traceback(exc_type, exc_value, exc_tb):\n"; - fcn += " import sys, traceback\n"; - fcn += " lines = []\n"; - fcn += " lines = traceback.format_exception(exc_type, exc_value, exc_tb)\n"; - fcn += " output = '\\n'.join(lines)\n"; - fcn += " return output\n"; - - PyRun_SimpleString(fcn.c_str()); - PyObject* mod = PyImport_ImportModule("__main__"); - PyObject* method = PyObject_GetAttrString(mod, "get_pretty_traceback"); - PyObject* outStr = PyObject_CallObject(method, Py_BuildValue("OOO", type, value, traceback)); - std::string pretty = PyBytes_AsString(PyUnicode_AsASCIIString(outStr)); - - Py_DECREF(method); - Py_DECREF(outStr); - Py_DECREF(mod); - - return pretty; -} - -PyObject * PyUtil::importModule(const std::string & path) -{ - if(!UFile::exists(path) || UFile::getExtension(path).compare("py") != 0) - { - UERROR("Cannot initialize Python module, the path is not valid: \"%s\"", path.c_str()); - return 0; - } - - 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()); - } - - std::string scriptName = uSplit(UFile::getName(path), '.').front(); - PyObject * pName = PyUnicode_FromString(scriptName.c_str()); - PyObject * module = PyImport_Import(pName); - Py_DECREF(pName); - - if(!module) - { - UERROR("Module \"%s\" could not be imported! (File=\"%s\")", scriptName.c_str(), path.c_str()); - UERROR("%s", getTraceback().c_str()); - } - - return module; -} - -} - diff --git a/corelib/src/PyUtil.h b/corelib/src/PyUtil.h deleted file mode 100644 index 8271b26ca1..0000000000 --- a/corelib/src/PyUtil.h +++ /dev/null @@ -1,38 +0,0 @@ -/* - * PythonUtil.h - * - * Created on: Jul. 16, 2020 - * Author: mathieu - */ - -#ifndef CORELIB_SRC_PYUTIL_H_ -#define CORELIB_SRC_PYUTIL_H_ - -#include -#include -#include - -namespace rtabmap { - -class PyUtil -{ -public: - virtual ~PyUtil() {} - - static void acquire(); - static void release(); - - static std::string getTraceback(); - static PyObject* importModule(const std::string & path); - -private: - PyUtil() {} - - static UMutex mutex_; - static size_t references_; -}; - -} - - -#endif /* CORELIB_SRC_PYUTIL_H_ */ diff --git a/corelib/src/python/.gitignore b/corelib/src/python/.gitignore deleted file mode 100644 index 77ebff9b52..0000000000 --- a/corelib/src/python/.gitignore +++ /dev/null @@ -1 +0,0 @@ -__* diff --git a/corelib/src/python/PyDescriptor.cpp b/corelib/src/python/PyDescriptor.cpp index 1503d25a86..0b5f979327 100644 --- a/corelib/src/python/PyDescriptor.cpp +++ b/corelib/src/python/PyDescriptor.cpp @@ -1,6 +1,5 @@ -#include -#include +#include #include #include #include @@ -8,6 +7,8 @@ #include #include +#include + #define NPY_NO_DEPRECATED_API NPY_API_VERSION #include @@ -17,18 +18,17 @@ namespace rtabmap PyDescriptor::PyDescriptor( const ParametersMap & parameters) : GlobalDescriptorExtractor(parameters), - pModule_(0), - pFunc_(0), - dim_(Parameters::defaultPyDescriptorDim()) + pModule_(0), + pFunc_(0), + dim_(Parameters::defaultPyDescriptorDim()) { - PyUtil::acquire(); - UASSERT(_import_array()==0); - this->parseParameters(parameters); } PyDescriptor::~PyDescriptor() { + pybind11::gil_scoped_acquire acquire; + if(pFunc_) { Py_DECREF(pFunc_); @@ -37,25 +37,26 @@ PyDescriptor::~PyDescriptor() { Py_DECREF(pModule_); } - PyUtil::release(); } void PyDescriptor::parseParameters(const ParametersMap & parameters) { - std::string path; - Parameters::parse(parameters, Parameters::kPyDescriptorPath(), path); + 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()); + 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(!path.empty() && path.compare(path_)!=0) + if(!previousPath.empty() && previousPath.compare(path_)!=0) { UDEBUG("we changed script (old=%s), we need to reload (new=%s)", - path_.c_str(), path.c_str()); + previousPath.c_str(), path_.c_str()); if(pFunc_) { Py_DECREF(pFunc_); @@ -63,21 +64,40 @@ void PyDescriptor::parseParameters(const ParametersMap & parameters) pFunc_=0; Py_DECREF(pModule_); pModule_ = 0; - path_.clear(); } } if(pModule_==0) { - if(path.empty()) + UASSERT(pFunc_ == 0); + if(path_.empty()) { return; } - pModule_ = PyUtil::importModule(path); + 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(); - if(pModule_) + 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 { - path_ = path; PyObject * pFunc = PyObject_GetAttrString(pModule_, "init"); if(pFunc) { @@ -88,7 +108,7 @@ void PyDescriptor::parseParameters(const ParametersMap & parameters) if(result == NULL) { UERROR("Call to \"init(...)\" in \"%s\" failed!", path_.c_str()); - UERROR("%s", PyUtil::getTraceback().c_str()); + UERROR("%s", getPythonTraceback().c_str()); } Py_DECREF(result); @@ -100,7 +120,7 @@ void PyDescriptor::parseParameters(const ParametersMap & parameters) else { UERROR("Cannot find method \"extract(...)\" in %s", path_.c_str()); - UERROR("%s", PyUtil::getTraceback().c_str()); + UERROR("%s", getPythonTraceback().c_str()); if(pFunc_) { Py_DECREF(pFunc_); @@ -111,16 +131,15 @@ void PyDescriptor::parseParameters(const ParametersMap & parameters) else { UERROR("Cannot call method \"init(...)\" in %s", path_.c_str()); - UERROR("%s", PyUtil::getTraceback().c_str()); + UERROR("%s", getPythonTraceback().c_str()); } Py_DECREF(pFunc); } else { UERROR("Cannot find method \"init(...)\""); - UERROR("%s", PyUtil::getTraceback().c_str()); + UERROR("%s", getPythonTraceback().c_str()); } - UDEBUG("init time = %fs", timer.ticks()); } } } @@ -136,12 +155,15 @@ GlobalDescriptor PyDescriptor::extract( 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 descriptorsQueryV(data.imageRaw().total()*data.imageRaw().channels()); @@ -156,7 +178,7 @@ GlobalDescriptor PyDescriptor::extract( if(pReturn == NULL) { UERROR("Failed to call extract() function!"); - UERROR("%s", PyUtil::getTraceback().c_str()); + UERROR("%s", getPythonTraceback().c_str()); } else { diff --git a/corelib/src/python/PyDescriptor.h b/corelib/src/python/PyDescriptor.h index 5dd4767e9f..1588b60259 100644 --- a/corelib/src/python/PyDescriptor.h +++ b/corelib/src/python/PyDescriptor.h @@ -6,10 +6,8 @@ #ifndef PYDESCRIPTOR_H #define PYDESCRIPTOR_H -#include #include -#include - +#include "rtabmap/core/PythonInterface.h" #include namespace rtabmap diff --git a/corelib/src/python/PyDetector.h b/corelib/src/python/PyDetector.h index 5f292abdc5..fdbf43ae2b 100644 --- a/corelib/src/python/PyDetector.h +++ b/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 #include diff --git a/corelib/src/python/PyMatcher.cpp b/corelib/src/python/PyMatcher.cpp index e18c58d67e..aac482efe1 100644 --- a/corelib/src/python/PyMatcher.cpp +++ b/corelib/src/python/PyMatcher.cpp @@ -76,7 +76,6 @@ PyMatcher::~PyMatcher() { Py_DECREF(pModule_); } - PyUtil::release(); } std::vector PyMatcher::match( diff --git a/guilib/src/PreferencesDialog.cpp b/guilib/src/PreferencesDialog.cpp index 397a695dd4..8612784adc 100644 --- a/guilib/src/PreferencesDialog.cpp +++ b/guilib/src/PreferencesDialog.cpp @@ -5416,11 +5416,6 @@ void PreferencesDialog::changePyDetectorPath() } else { - path = QFileDialog::getOpenFileName(this, tr("Select file"), _ui->lineEdit_pydescriptor_path->text(), tr("Python wrapper (*.py)")); - } - if(!path.isEmpty()) - { - _ui->lineEdit_pydescriptor_path->setText(path); path = QFileDialog::getOpenFileName(this, tr("Select file"), _ui->lineEdit_pydetector_path->text(), tr("Python wrapper (*.py)")); } if(!path.isEmpty()) diff --git a/guilib/src/ui/preferencesDialog.ui b/guilib/src/ui/preferencesDialog.ui index d05d14184f..16a484557b 100644 --- a/guilib/src/ui/preferencesDialog.ui +++ b/guilib/src/ui/preferencesDialog.ui @@ -95,7 +95,7 @@ QFrame::Raised - 12 + 11 @@ -11209,7 +11209,7 @@ see Sqlite3 doc 'PRAGMA synchronous'. Loop Closure Detection - + From ff17577ccc8ae1353d609c499abf4f2b2fe21937 Mon Sep 17 00:00:00 2001 From: matlabbe Date: Sun, 31 Mar 2024 19:46:50 -0700 Subject: [PATCH 7/8] integrated https://github.com/introlab/rtabmap/pull/1255 --- corelib/src/GlobalDescriptorExtractor.cpp | 2 +- corelib/src/Signature.cpp | 34 +++++++++++++++++----- corelib/src/python/PyDescriptor.cpp | 34 +++++++++------------- corelib/src/python/rtabmap_netvlad.py | 35 +++++++++++++++-------- 4 files changed, 65 insertions(+), 40 deletions(-) diff --git a/corelib/src/GlobalDescriptorExtractor.cpp b/corelib/src/GlobalDescriptorExtractor.cpp index 43084f0cb4..00fdfc13dd 100644 --- a/corelib/src/GlobalDescriptorExtractor.cpp +++ b/corelib/src/GlobalDescriptorExtractor.cpp @@ -49,7 +49,7 @@ GlobalDescriptorExtractor * GlobalDescriptorExtractor::create(const ParametersMa GlobalDescriptorExtractor * GlobalDescriptorExtractor::create(GlobalDescriptorExtractor::Type type, const ParametersMap & parameters) { UDEBUG("Creating global descriptor of type %d", (int)type); -#ifndef RTABMAP_PYTHON3 +#ifndef RTABMAP_PYTHON if(type == GlobalDescriptorExtractor::kPyDescriptor) { UWARN("PyDescriptor cannot be used as rtabmap is not built with Python3 support."); diff --git a/corelib/src/Signature.cpp b/corelib/src/Signature.cpp index ee08ab6804..3e4f491f0c 100644 --- a/corelib/src/Signature.cpp +++ b/corelib/src/Signature.cpp @@ -221,17 +221,37 @@ void Signature::removeVirtualLinks() float Signature::compareTo(const Signature & s) const { + UASSERT(this->sensorData().globalDescriptors().size() == s.sensorData().globalDescriptors().size()); + float similarity = 0.0f; - const std::multimap & words = s.getWords(); + int totalDescs = 0; + + for(size_t i=0; isensorData().globalDescriptors().size(); ++i) + { + if(this->sensorData().globalDescriptors()[i].type()==1 && s.sensorData().globalDescriptors()[i].type()==1) + { + similarity += this->sensorData().globalDescriptors()[i].data().dot(s.sensorData().globalDescriptors()[i].data()); + totalDescs += 1; + } + } - if(!s.isBadSignature() && !this->isBadSignature()) + if(totalDescs) { - std::list > > 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 /= totalDescs; + } + else + { + const std::multimap & words = s.getWords(); - similarity = float(pairs.size()) / float(totalWords); + if(!s.isBadSignature() && !this->isBadSignature()) + { + std::list > > 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; } diff --git a/corelib/src/python/PyDescriptor.cpp b/corelib/src/python/PyDescriptor.cpp index 0b5f979327..f99b742708 100644 --- a/corelib/src/python/PyDescriptor.cpp +++ b/corelib/src/python/PyDescriptor.cpp @@ -22,11 +22,13 @@ PyDescriptor::PyDescriptor( pFunc_(0), dim_(Parameters::defaultPyDescriptorDim()) { + UDEBUG(""); this->parseParameters(parameters); } PyDescriptor::~PyDescriptor() { + UDEBUG(""); pybind11::gil_scoped_acquire acquire; if(pFunc_) @@ -41,6 +43,7 @@ PyDescriptor::~PyDescriptor() void PyDescriptor::parseParameters(const ParametersMap & parameters) { + UDEBUG(""); std::string previousPath = path_; Parameters::parse(parameters, Parameters::kPyDescriptorPath(), path_); Parameters::parse(parameters, Parameters::kPyDescriptorDim(), dim_); @@ -147,6 +150,7 @@ void PyDescriptor::parseParameters(const ParametersMap & parameters) GlobalDescriptor PyDescriptor::extract( const SensorData & data) const { + UDEBUG(""); UTimer timer; GlobalDescriptor descriptor; @@ -184,31 +188,21 @@ GlobalDescriptor PyDescriptor::extract( { UDEBUG("Python extraction time = %fs", timer.ticks()); - /* PyArrayObject *np_ret = reinterpret_cast(pReturn); // Convert back to C++ array and print. int len1 = PyArray_SHAPE(np_ret)[0]; - int len2 = PyArray_SHAPE(np_ret)[1]; + int dim = PyArray_SHAPE(np_ret)[1]; int type = PyArray_TYPE(np_ret); - UDEBUG("Matches array %dx%d (type=%d)", len1, len2, type); - UASSERT_MSG(type == NPY_LONG || type == NPY_INT, uFormat("Returned matches should type INT=5 or LONG=7, received type=%d", type).c_str()); - if(type == NPY_LONG) - { - long* c_out = reinterpret_cast(PyArray_DATA(np_ret)); - for (int i = 0; i < len1*len2; i+=2) - { - matches.push_back(cv::DMatch(c_out[i], c_out[i+1], 0)); - } - } - else // INT - { - int* c_out = reinterpret_cast(PyArray_DATA(np_ret)); - for (int i = 0; i < len1*len2; i+=2) - { - matches.push_back(cv::DMatch(c_out[i], c_out[i+1], 0)); - } - }*/ + 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(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); } diff --git a/corelib/src/python/rtabmap_netvlad.py b/corelib/src/python/rtabmap_netvlad.py index 9a1f2962a8..3fa7a131ef 100644 --- a/corelib/src/python/rtabmap_netvlad.py +++ b/corelib/src/python/rtabmap_netvlad.py @@ -1,8 +1,9 @@ #! /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: -# --PyDescriptor/Dim 128 --PyDescriptor/Path ~/netvlad_tf_open/python/rtabmap_netvlad.py +# --Mem/GlobalDescriptorStrategy 1 --Kp/TfIdfLikelihoodUsed false --Mem/RehearsalSimilarity 1 --PyDescriptor/Dim 128 --PyDescriptor/Path ~/netvlad_tf_open/python/rtabmap_netvlad.py # import sys @@ -36,32 +37,40 @@ def init(descriptorDim): dim = descriptorDim - tf.reset_default_graph() + tf.compat.v1.disable_eager_execution() + tf.compat.v1.reset_default_graph() - image_batch = tf.placeholder( + image_batch = tf.compat.v1.placeholder( dtype=tf.float32, shape=[None, None, None, 3]) net_out = nets.vgg16NetvladPca(image_batch) - saver = tf.train.Saver() + saver = tf.compat.v1.train.Saver() - sess = tf.Session() + sess = tf.compat.v1.Session() saver.restore(sess, nets.defaultCheckpoint()) def extract(image): - print("NetVLAD python extract()") + print(f"NetVLAD python extract{image.shape}") global image_batch global net_out global sess global dim - - print(image.shape) - + + 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}) - result = result[:,:dim] - return result + # 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__': @@ -69,4 +78,6 @@ def extract(image): img = np.zeros([100,100,3],dtype=np.uint8) img.fill(255) init(128) - print(extract(img)) + descriptor = extract(img) + print(descriptor.shape) + print(descriptor) \ No newline at end of file From 0ab9b84d82475d5ca9e7b030ab88658277a296ca Mon Sep 17 00:00:00 2001 From: matlabbe Date: Mon, 1 Apr 2024 21:49:59 -0700 Subject: [PATCH 8/8] rescaled dot product result --- corelib/src/Signature.cpp | 5 ++++- 1 file changed, 4 insertions(+), 1 deletion(-) diff --git a/corelib/src/Signature.cpp b/corelib/src/Signature.cpp index 3e4f491f0c..113556642e 100644 --- a/corelib/src/Signature.cpp +++ b/corelib/src/Signature.cpp @@ -230,7 +230,10 @@ float Signature::compareTo(const Signature & s) const { if(this->sensorData().globalDescriptors()[i].type()==1 && s.sensorData().globalDescriptors()[i].type()==1) { - similarity += this->sensorData().globalDescriptors()[i].data().dot(s.sensorData().globalDescriptors()[i].data()); + // 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; } }