Skip to content

Commit 5ed1bf0

Browse files
matlabbematlabbe
authored
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 #1255 * rescaled dot product result Closing #1105
1 parent c33e995 commit 5ed1bf0

15 files changed

Lines changed: 723 additions & 16 deletions

‎corelib/include/rtabmap/core/GlobalDescriptor.h‎

Lines changed: 1 addition & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -1,5 +1,5 @@
11
/*
2-
Copyright (c) 2010-2020, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
2+
Copyright (c) 2010-2024, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
33
All rights reserved.
44
55
Redistribution and use in source and binary forms, with or without

‎corelib/include/rtabmap/core/GlobalDescriptorExtractor.h‎

Lines changed: 73 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,73 @@
1+
/*
2+
Copyright (c) 2010-2024, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
3+
All rights reserved.
4+
5+
Redistribution and use in source and binary forms, with or without
6+
modification, are permitted provided that the following conditions are met:
7+
* Redistributions of source code must retain the above copyright
8+
notice, this list of conditions and the following disclaimer.
9+
* Redistributions in binary form must reproduce the above copyright
10+
notice, this list of conditions and the following disclaimer in the
11+
documentation and/or other materials provided with the distribution.
12+
* Neither the name of the Universite de Sherbrooke nor the
13+
names of its contributors may be used to endorse or promote products
14+
derived from this software without specific prior written permission.
15+
16+
THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
17+
ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
18+
WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
19+
DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY
20+
DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
21+
(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
22+
LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
23+
ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
24+
(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
25+
SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
26+
*/
27+
28+
#ifndef GLOBAL_DESCRIPTOR_EXTRACTOR_H_
29+
#define GLOBAL_DESCRIPTOR_EXTRACTOR_H_
30+
31+
#include "rtabmap/core/rtabmap_core_export.h" // DLL export/import defines
32+
33+
#include "rtabmap/core/Parameters.h"
34+
#include "rtabmap/core/SensorData.h"
35+
36+
37+
namespace rtabmap {
38+
39+
// Feature2D
40+
class RTABMAP_CORE_EXPORT GlobalDescriptorExtractor {
41+
public:
42+
enum Type {
43+
kUndef=0,
44+
kPyDescriptor=1};
45+
46+
static std::string typeName(Type type)
47+
{
48+
switch(type){
49+
case kPyDescriptor:
50+
return "PyDescriptor";
51+
default:
52+
return "Unknown";
53+
}
54+
}
55+
56+
static GlobalDescriptorExtractor * create(const ParametersMap & parameters = ParametersMap());
57+
static GlobalDescriptorExtractor * create(GlobalDescriptorExtractor::Type type, const ParametersMap & parameters = ParametersMap()); // for convenience
58+
59+
public:
60+
virtual ~GlobalDescriptorExtractor();
61+
62+
virtual GlobalDescriptor extract(const SensorData & data) const = 0;
63+
64+
virtual void parseParameters(const ParametersMap & parameters) {}
65+
virtual GlobalDescriptorExtractor::Type getType() const = 0;
66+
67+
protected:
68+
GlobalDescriptorExtractor(const ParametersMap & parameters = ParametersMap());
69+
};
70+
71+
}
72+
73+
#endif /* GLOBAL_DESCRIPTOR_EXTRACTOR_H_ */

‎corelib/include/rtabmap/core/Memory.h‎

Lines changed: 3 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -59,6 +59,7 @@ class RegistrationVis;
5959
class Stereo;
6060
class LocalGridMaker;
6161
class MarkerDetector;
62+
class GlobalDescriptorExtractor;
6263

6364
class RTABMAP_CORE_EXPORT Memory
6465
{
@@ -375,6 +376,8 @@ class RTABMAP_CORE_EXPORT Memory
375376
LocalGridMaker * _localMapMaker;
376377

377378
MarkerDetector * _markerDetector;
379+
380+
GlobalDescriptorExtractor * _globalDescriptorExtractor;
378381
};
379382

380383
} // namespace rtabmap

‎corelib/include/rtabmap/core/Parameters.h‎

Lines changed: 5 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -231,6 +231,7 @@ class RTABMAP_CORE_EXPORT Parameters
231231
RTABMAP_PARAM(Mem, UseOdomFeatures, bool, true, "Use odometry features instead of regenerating them.");
232232
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()));
233233
RTABMAP_PARAM(Mem, CovOffDiagIgnored, bool, true, "Ignore off diagonal values of the covariance matrix.");
234+
RTABMAP_PARAM(Mem, GlobalDescriptorStrategy, int, 0, "Extract global descriptor from sensor data. 0=disabled, 1=PyDescriptor");
234235
RTABMAP_PARAM(Mem, RotateImagesUpsideUp, bool, false, "Rotate images so that upside is up if they are not already. This can be useful in case the robots don't have all same camera orientation but are using the same map, so that not rotation-invariant visual features can still be used across the fleet.");
235236

236237
// KeypointMemory (Keypoint-based)
@@ -725,6 +726,10 @@ class RTABMAP_CORE_EXPORT Parameters
725726
RTABMAP_PARAM(GMS, WithScale, bool, false, "Take scale transformation into account.");
726727
RTABMAP_PARAM(GMS, ThresholdFactor, double, 6.0, "The higher, the less matches.");
727728

729+
// Global descriptor approaches
730+
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.");
731+
RTABMAP_PARAM(PyDescriptor, Dim, int, 4096, "Descriptor dimension.");
732+
728733
// ICP registration parameters
729734
#ifdef RTABMAP_POINTMATCHER
730735
RTABMAP_PARAM(Icp, Strategy, int, 1, "ICP implementation: 0=Point Cloud Library, 1=libpointmatcher, 2=CCCoreLib (CloudCompare).");

‎corelib/src/CMakeLists.txt‎

Lines changed: 3 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -115,6 +115,8 @@ SET(SRC_FILES
115115

116116
MarkerDetector.cpp
117117

118+
GlobalDescriptorExtractor.cpp
119+
118120
GainCompensator.cpp
119121

120122
rtflann/ext/lz4.c
@@ -224,6 +226,7 @@ IF(WITH_PYTHON AND Python3_FOUND)
224226
python/PythonInterface.cpp
225227
python/PyMatcher.cpp
226228
python/PyDetector.cpp
229+
python/PyDescriptor.cpp
227230
)
228231
SET(INCLUDE_DIRS
229232
${TORCH_INCLUDE_DIRS}

‎corelib/src/GlobalDescriptorExtractor.cpp‎

Lines changed: 76 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,76 @@
1+
/*
2+
Copyright (c) 2010-2024, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
3+
All rights reserved.
4+
5+
Redistribution and use in source and binary forms, with or without
6+
modification, are permitted provided that the following conditions are met:
7+
* Redistributions of source code must retain the above copyright
8+
notice, this list of conditions and the following disclaimer.
9+
* Redistributions in binary form must reproduce the above copyright
10+
notice, this list of conditions and the following disclaimer in the
11+
documentation and/or other materials provided with the distribution.
12+
* Neither the name of the Universite de Sherbrooke nor the
13+
names of its contributors may be used to endorse or promote products
14+
derived from this software without specific prior written permission.
15+
16+
THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
17+
ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
18+
WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
19+
DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY
20+
DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
21+
(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
22+
LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
23+
ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
24+
(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
25+
SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
26+
*/
27+
28+
29+
#include "rtabmap/core/GlobalDescriptorExtractor.h"
30+
31+
#ifdef RTABMAP_PYTHON
32+
#include "python/PyDescriptor.h"
33+
#endif
34+
35+
namespace rtabmap {
36+
37+
GlobalDescriptorExtractor::GlobalDescriptorExtractor(const ParametersMap & parameters)
38+
{
39+
}
40+
GlobalDescriptorExtractor::~GlobalDescriptorExtractor()
41+
{
42+
}
43+
GlobalDescriptorExtractor * GlobalDescriptorExtractor::create(const ParametersMap & parameters)
44+
{
45+
int type = Parameters::defaultMemGlobalDescriptorStrategy();
46+
Parameters::parse(parameters, Parameters::kMemGlobalDescriptorStrategy(), type);
47+
return create((GlobalDescriptorExtractor::Type)type, parameters);
48+
}
49+
GlobalDescriptorExtractor * GlobalDescriptorExtractor::create(GlobalDescriptorExtractor::Type type, const ParametersMap & parameters)
50+
{
51+
UDEBUG("Creating global descriptor of type %d", (int)type);
52+
#ifndef RTABMAP_PYTHON
53+
if(type == GlobalDescriptorExtractor::kPyDescriptor)
54+
{
55+
UWARN("PyDescriptor cannot be used as rtabmap is not built with Python3 support.");
56+
type = GlobalDescriptorExtractor::kUndef;
57+
}
58+
#endif
59+
60+
GlobalDescriptorExtractor * GlobalDescriptorExtractor = 0;
61+
switch(type)
62+
{
63+
#ifdef RTABMAP_PYTHON
64+
case GlobalDescriptorExtractor::kPyDescriptor:
65+
GlobalDescriptorExtractor = new PyDescriptor(parameters);
66+
break;
67+
#endif
68+
default:
69+
type = GlobalDescriptorExtractor::kUndef;
70+
break;
71+
}
72+
return GlobalDescriptorExtractor;
73+
}
74+
75+
}
76+

‎corelib/src/Memory.cpp‎

Lines changed: 29 additions & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -40,6 +40,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
4040
#include <rtabmap/core/EpipolarGeometry.h>
4141
#include "rtabmap/core/VisualWord.h"
4242
#include "rtabmap/core/Features2d.h"
43+
#include "rtabmap/core/GlobalDescriptorExtractor.h"
4344
#include "rtabmap/core/RegistrationIcp.h"
4445
#include "rtabmap/core/Registration.h"
4546
#include "rtabmap/core/RegistrationVis.h"
@@ -132,6 +133,7 @@ Memory::Memory(const ParametersMap & parameters) :
132133
_feature2D = Feature2D::create(parameters);
133134
_vwd = new VWDictionary(parameters);
134135
_registrationPipeline = Registration::create(parameters);
136+
_globalDescriptorExtractor = GlobalDescriptorExtractor::create(parameters);
135137
if(!_registrationPipeline->isImageRequired())
136138
{
137139
// make sure feature matching is used instead of optical flow to compute the guess
@@ -761,6 +763,22 @@ void Memory::parseParameters(const ParametersMap & parameters)
761763
_markerDetector->parseParameters(params);
762764
}
763765

766+
int globalDescriptorStrategy = -1;
767+
Parameters::parse(params, Parameters::kMemGlobalDescriptorStrategy(), globalDescriptorStrategy);
768+
if(globalDescriptorStrategy != -1 &&
769+
(_globalDescriptorExtractor==0 || (int)_globalDescriptorExtractor->getType() != globalDescriptorStrategy))
770+
{
771+
if(_globalDescriptorExtractor)
772+
{
773+
delete _globalDescriptorExtractor;
774+
}
775+
_globalDescriptorExtractor = GlobalDescriptorExtractor::create(parameters_);
776+
}
777+
else if(_globalDescriptorExtractor)
778+
{
779+
_globalDescriptorExtractor->parseParameters(params);
780+
}
781+
764782
// do this after all params are parsed
765783
// SLAM mode vs Localization mode
766784
iter = params.find(Parameters::kMemIncrementalMemory());
@@ -5934,7 +5952,17 @@ Signature * Memory::createSignature(const SensorData & inputData, const Transfor
59345952
s->sensorData().setGroundTruth(data.groundTruth());
59355953
s->sensorData().setGPS(data.gps());
59365954
s->sensorData().setEnvSensors(data.envSensors());
5937-
s->sensorData().setGlobalDescriptors(data.globalDescriptors());
5955+
5956+
std::vector<GlobalDescriptor> globalDescriptors = data.globalDescriptors();
5957+
if(_globalDescriptorExtractor)
5958+
{
5959+
GlobalDescriptor gdescriptor = _globalDescriptorExtractor->extract(inputData);
5960+
if(!gdescriptor.data().empty())
5961+
{
5962+
globalDescriptors.push_back(gdescriptor);
5963+
}
5964+
}
5965+
s->sensorData().setGlobalDescriptors(globalDescriptors);
59385966

59395967
t = timer.ticks();
59405968
if(stats) stats->addStatistic(Statistics::kTimingMemCompressing_data(), t*1000.0f);

‎corelib/src/Signature.cpp‎

Lines changed: 30 additions & 7 deletions
Original file line numberDiff line numberDiff line change
@@ -249,17 +249,40 @@ void Signature::removeLandmark(int landmarkId)
249249

250250
float Signature::compareTo(const Signature & s) const
251251
{
252+
UASSERT(this->sensorData().globalDescriptors().size() == s.sensorData().globalDescriptors().size());
253+
252254
float similarity = 0.0f;
253-
const std::multimap<int, int> & words = s.getWords();
255+
int totalDescs = 0;
256+
257+
for(size_t i=0; i<this->sensorData().globalDescriptors().size(); ++i)
258+
{
259+
if(this->sensorData().globalDescriptors()[i].type()==1 && s.sensorData().globalDescriptors()[i].type()==1)
260+
{
261+
// rescale dot product from -1<->1 to 0<->1 (we assume normalized vectors!)
262+
float dotProd = (this->sensorData().globalDescriptors()[i].data().dot(s.sensorData().globalDescriptors()[i].data()) + 1.0f) / 2.0f;
263+
UASSERT_MSG(dotProd>=0, "Global descriptors should be normalized!");
264+
similarity += dotProd;
265+
totalDescs += 1;
266+
}
267+
}
254268

255-
if(!s.isBadSignature() && !this->isBadSignature())
269+
if(totalDescs)
256270
{
257-
std::list<std::pair<int, std::pair<int, int> > > pairs;
258-
int totalWords = ((int)_words.size()-_invalidWordsCount)>((int)words.size()-s.getInvalidWordsCount())?((int)_words.size()-_invalidWordsCount):((int)words.size()-s.getInvalidWordsCount());
259-
UASSERT(totalWords > 0);
260-
EpipolarGeometry::findPairs(words, _words, pairs);
271+
similarity /= totalDescs;
272+
}
273+
else
274+
{
275+
const std::multimap<int, int> & words = s.getWords();
261276

262-
similarity = float(pairs.size()) / float(totalWords);
277+
if(!s.isBadSignature() && !this->isBadSignature())
278+
{
279+
std::list<std::pair<int, std::pair<int, int> > > pairs;
280+
int totalWords = ((int)_words.size()-_invalidWordsCount)>((int)words.size()-s.getInvalidWordsCount())?((int)_words.size()-_invalidWordsCount):((int)words.size()-s.getInvalidWordsCount());
281+
UASSERT(totalWords > 0);
282+
EpipolarGeometry::findPairs(words, _words, pairs);
283+
284+
similarity = float(pairs.size()) / float(totalWords);
285+
}
263286
}
264287
return similarity;
265288
}

0 commit comments

Comments
 (0)