VWDictionary: use multi-core FLANN kNN search (#1760)

* VWDictionary: use multi-core FLANN kNN search

* Add parameter with default 1 thread

* Kp/FlannTreads plumbing  to UI. Also added to performance tests for comparison.

* fixing ci error

* dump debug data for windows ci

* Adding  more dll debugging report windows ci

* install vc2012 runtime explicitly

* updated comment

---------

Co-authored-by: matlabbe <[email protected]>

corelib/include/rtabmap/core/FlannIndex.h

@@ -201,6 +201,7 @@ public:
 	 *        structures ignoring it
 	 * @param eps Search for eps-approximate neighbors
 	 * @param sorted Give the neighbors back by increasing distance
+	 * @param cores Threads for the batch search (0 = all available)
 	 */
 	void knnSearch(
 			const cv::Mat & query,
@@ -209,8 +210,8 @@ public:
 	        int knn,
 			int checks = 32,
 			float eps = 0.0,
-			bool sorted = true) const;
-
+			bool sorted = true,
+			int cores = 1) const;
 	/**
 	 * @brief Search the neighbors of each query within a radius
 	 * @param query One feature per row, of the type and dimension the index was
@@ -225,6 +226,7 @@ public:
 	 *        structures ignoring it
 	 * @param eps Search for eps-approximate neighbors
 	 * @param sorted Give the neighbors back by increasing distance
+	 * @param cores Threads for the batch search (0 = all available)
 	 */
 	void radiusSearch(
 			const cv::Mat & query,
@@ -234,7 +236,8 @@ public:
 			int maxNeighbors = 0,
 			int checks = 32,
 			float eps = 0.0,
-			bool sorted = true) const;
+			bool sorted = true,
+			int cores = 1) const;
 
 private:
 	void * index_;               // rtflann backend

corelib/include/rtabmap/core/Parameters.h

@@ -256,6 +256,7 @@ class RTABMAP_CORE_EXPORT Parameters
     RTABMAP_PARAM(Kp, IncrementalDictionary,    bool, true,   "");
     RTABMAP_PARAM(Kp, IncrementalFlann,         bool, true,   uFormat("When using FLANN based strategy, add/remove points to its index without always rebuilding the index (the index is only rebuilt when too many of its features have been removed, see \"%s\").", kKpFlannRebalancingFactor().c_str()));
     RTABMAP_PARAM(Kp, FlannRebalancingFactor,   float, 2.0,   uFormat("Rebuild the incremental FLANN index (see \"%s\") once the ratio (factor-1)/factor of its features has been removed, e.g. half of them for a factor of 2. Rebuilding frees the memory of the removed features and speeds up the searches. Features are mostly removed when memory management is enabled (\"%s\" or \"%s\"). Set to 1 to never rebuild, which also uses less memory as the features don't have to be referenced one by one.", kKpIncrementalFlann().c_str(), kRtabmapTimeThr().c_str(), kRtabmapMemoryThr().c_str()));
+    RTABMAP_PARAM(Kp, FlannThreads,             int, 1,       "Number of threads used for FLANN kNN search (batched queries). Set to 0 for all available.");
     RTABMAP_PARAM(Kp, ByteToFloat,              bool, false,  uFormat("For %s=1, binary descriptors are converted to float by converting each byte to float instead of converting each bit to float. When converting bytes instead of bits, less memory is used and search is faster at the cost of slightly less accurate matching.", kKpNNStrategy().c_str()));
     RTABMAP_PARAM(Kp, MaxDepth,                 float, 0,     "Filter extracted keypoints by depth (0=inf).");
     RTABMAP_PARAM(Kp, MinDepth,                 float, 0,     "Filter extracted keypoints by depth.");

corelib/include/rtabmap/core/VWDictionary.h

@@ -495,6 +495,11 @@ private:
 	 */
 	float _rebalancingFactor;
 	
+	/**
+	 * @brief Threads for FLANN batched kNN search (0 = all available)
+	 */
+	int _flannThreads;
+	
 	/**
 	 * @brief Whether to convert descriptors from byte to float format
 	 */

corelib/src/FlannIndex.cpp

@@ -36,9 +36,33 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #include "rtflann/flann.hpp"
 #include "nanoflann/NanoFlannIndex.h"
 #include <boost/crc.hpp>
+#ifdef _OPENMP
+#include <omp.h>
+#endif
 
 namespace rtabmap {
 
+namespace {
+// A count of 0 means one thread per core, as Kp/FlannThreads spells it.
+// rtflann would reach the same place by leaving num_threads(0) to OpenMP, but
+// only where it is compiled with it: resolving the count here makes 0 mean the
+// same thing in both builds, and keeps a negative count from reaching
+// num_threads(), where it wraps around to an unsigned and asks the runtime for
+// billions of threads.
+int resolveCores(int cores)
+{
+	if(cores > 0)
+	{
+		return cores;
+	}
+#ifdef _OPENMP
+	return omp_get_max_threads();
+#else
+	return 1;
+#endif
+}
+}
+
 FlannIndex::FlannIndex():
 		index_(0),
 		nanoIndex_(0),
@@ -910,7 +934,8 @@ void FlannIndex::knnSearch(
 		int knn,
 		int checks,
 		float eps,
-		bool sorted) const
+		bool sorted,
+		int cores) const
 {
 	if(nanoIndex_)
 	{
@@ -930,6 +955,7 @@ void FlannIndex::knnSearch(
 	rtflann::Matrix<size_t> indicesF((size_t*)indicesBuffer.data(), query.rows, knn);
 
 	rtflann::SearchParams params = rtflann::SearchParams(checks, eps, sorted);
+	params.cores = resolveCores(cores);
 
 	if(featuresType_ == CV_8UC1)
 	{
@@ -974,11 +1000,12 @@ void FlannIndex::radiusSearch(
 		int maxNeighbors,
 		int checks,
 		float eps,
-		bool sorted) const
+		bool sorted,
+		int cores) const
 {
 	if(nanoIndex_)
 	{
-		// "checks" doesn't apply
+		// "checks" and "cores" don't apply, it searches on one core
 		nanoIndex_->radiusSearch(query, indices, dists, radius, maxNeighbors, eps, sorted);
 		return;
 	}
@@ -990,6 +1017,7 @@ void FlannIndex::radiusSearch(
 
 	rtflann::SearchParams params = rtflann::SearchParams(checks, eps, sorted);
 	params.max_neighbors = maxNeighbors<=0?-1:maxNeighbors; // -1 is all in radius
+	params.cores = resolveCores(cores);
 
 	if(featuresType_ == CV_8UC1)
 	{

corelib/src/VWDictionary.cpp

@@ -101,6 +101,7 @@ VWDictionary::VWDictionary(const ParametersMap & parameters) :
 	_incrementalDictionary(Parameters::defaultKpIncrementalDictionary()),
 	_incrementalFlann(Parameters::defaultKpIncrementalFlann()),
 	_rebalancingFactor(Parameters::defaultKpFlannRebalancingFactor()),
+	_flannThreads(Parameters::defaultKpFlannThreads()),
 	_byteToFloat(Parameters::defaultKpByteToFloat()),
 	_nndrRatio(Parameters::defaultKpNndrRatio()),
 	_newDictionaryPath(Parameters::defaultKpDictionaryPath()),
@@ -130,6 +131,7 @@ void VWDictionary::parseParameters(const ParametersMap & parameters)
 	Parameters::parse(parameters, Parameters::kKpSerializeWithChecksum(), _serializeWithChecksum);
 	Parameters::parse(parameters, Parameters::kKpIncrementalFlann(), _incrementalFlann);
 	Parameters::parse(parameters, Parameters::kKpFlannRebalancingFactor(), _rebalancingFactor);
+	Parameters::parse(parameters, Parameters::kKpFlannThreads(), _flannThreads);
 	bool byteToFloat = _byteToFloat;
 	Parameters::parse(parameters, Parameters::kKpByteToFloat(), _byteToFloat);
 
@@ -1074,7 +1076,7 @@ std::list<int> VWDictionary::addNewWords(
 
 		if(isFlannStrategy(_strategy))
 		{
-			_flannIndex->knnSearch(descriptors, results, dists, k, KNN_CHECKS);
+			_flannIndex->knnSearch(descriptors, results, dists, k, KNN_CHECKS, 0.0f, true, _flannThreads);
 		}
 		else if(_strategy == kNNBruteForce)
 		{
@@ -1396,7 +1398,7 @@ std::vector<int> VWDictionary::findNN(const cv::Mat & queryIn) const
 
 			if(isFlannStrategy(_strategy))
 			{
-				_flannIndex->knnSearch(query, results, dists, k, KNN_CHECKS);
+				_flannIndex->knnSearch(query, results, dists, k, KNN_CHECKS, 0.0f, true, _flannThreads);
 			}
 			else if(_strategy == kNNBruteForce)
 			{

corelib/test/FlannIndexBackends.h

@@ -28,12 +28,21 @@ struct Backend
 	float rebalancingFactor = 2.0f;
 	// Not a FlannIndex at all: cv::BFMatcher, what the brute force strategies of
 	// VWDictionary and RegistrationVis use. Kept in the comparisons as the
-	// baseline every index has to beat. OpenCV threads its search where the
-	// indexes here search on one core, so it comes in two flavours: as the
-	// application gets it, and held to one core to compare the work done rather
-	// than the time it takes on an idle machine.
+	// baseline every index has to beat.
 	bool bruteForce = false;
-	bool singleCore = false;
+	// Threads the batch of queries is searched with, as Kp/FlannThreads sets it
+	// on VWDictionary: 1 to search on one core, 0 for one per core. It says the
+	// same thing on both sides of bruteForce, which is what makes the rows
+	// comparable: cv::BFMatcher threads its search too, so it appears in the
+	// same two flavours as the rtflann trees. A row named "threaded" is the one
+	// per core one, a row named without it searches on a single core, so that
+	// the tables compare the work done rather than the time it takes on an idle
+	// machine.
+	//
+	// Of the indexes only the rtflann ones read it, they are the ones searching
+	// a batch under an OpenMP loop; FlannIndex ignores it for the nanoflann
+	// ones, which always search on one core.
+	int cores = 1;
 };
 
 // Every algorithm that indexes float features. The exhaustive search comes
@@ -41,9 +50,8 @@ struct Backend
 // found and for the time taken.
 const Backend FLOAT_BACKENDS[] = {
 	{"linear    exhaustive                ", FlannIndex::FLANN_INDEX_LINEAR},
-	// No single core row for the float features: OpenCV doesn't thread that
-	// match at these sizes, it measures the same thing as the one above.
-	{"cv        BFMatcher                 ", FlannIndex::FLANN_INDEX_LINEAR, 1.0f, true},
+	{"cv        BFMatcher                 ", FlannIndex::FLANN_INDEX_LINEAR, 1.0f, true, 1},
+	{"cv        BFMatcher threaded        ", FlannIndex::FLANN_INDEX_LINEAR, 1.0f, true, 0},
 	{"rtflann   kd-tree (4 randomized)    ", FlannIndex::FLANN_INDEX_KDTREE},
 	{"rtflann   kd-tree single            ", FlannIndex::FLANN_INDEX_KDTREE_SINGLE},
 	{"nanoflann kd-tree single            ", FlannIndex::NANOFLANN_INDEX_KDTREE_SINGLE, 1.0f},
@@ -64,8 +72,8 @@ const Backend EXACT_BACKENDS[] = {
 // LSH is for.
 const Backend BINARY_BACKENDS[] = {
 	{"linear    exhaustive (hamming)      ", FlannIndex::FLANN_INDEX_LINEAR},
-	{"cv        BFMatcher (hamming)       ", FlannIndex::FLANN_INDEX_LINEAR, 1.0f, true},
-	{"cv        BFMatcher (hamming,1 core)", FlannIndex::FLANN_INDEX_LINEAR, 1.0f, true, true},
+	{"cv        BFMatcher hamming         ", FlannIndex::FLANN_INDEX_LINEAR, 1.0f, true, 1},
+	{"cv        BFMatcher hamming threaded", FlannIndex::FLANN_INDEX_LINEAR, 1.0f, true, 0},
 	{"rtflann   LSH                       ", FlannIndex::FLANN_INDEX_LSH},
 };
 
@@ -188,11 +196,12 @@ inline Result run(
 
 	if(backend.bruteForce)
 	{
-		// cv::setNumThreads() is global, put it back before leaving.
+		// cv::setNumThreads() is global, put it back before leaving. Left alone
+		// for cores=0: OpenCV's own default is already one thread per core.
 		const int threads = cv::getNumThreads();
-		if(backend.singleCore)
+		if(backend.cores > 0)
 		{
-			cv::setNumThreads(1);
+			cv::setNumThreads(backend.cores);
 		}
 
 		UTimer timer;
@@ -221,10 +230,7 @@ inline Result run(
 			result.radiusTime = timer.ticks();
 		}
 		result.memory = 0; // it indexes nothing
-		if(backend.singleCore)
-		{
-			cv::setNumThreads(threads);
-		}
+		cv::setNumThreads(threads);
 		return result;
 	}
 
@@ -233,7 +239,7 @@ inline Result run(
 	index.buildIndex(backend.algorithm, data, false, rebalancingFactor);
 	result.buildTime = timer.ticks();
 
-	index.knnSearch(queries, result.indices, dists, knn);
+	index.knnSearch(queries, result.indices, dists, knn, 32, 0.0f, true, backend.cores);
 	result.knnTime = timer.ticks();
 
 	if(radius > 0.0f)

corelib/test/perf_flann_index.cpp

@@ -11,6 +11,10 @@
 // version can be compared to what it replaces.
 #include "FlannIndexBackends.h"
 
+#ifdef _OPENMP
+#include <omp.h>
+#endif
+
 // The times are reported rather than asserted on: which backend is the fastest
 // depends on the machine. They are here so that a change of backend, of
 // parameters or of nanoflann version can be compared to what it replaces.
@@ -517,16 +521,31 @@ TEST(FlannIndexPerfTest, RegistrationGuessMatching)
 
 	// A factor of 1 for the rtflann rows keeps their per-point bookkeeping out
 	// of the measurement, and picks the nanoflann tree that is built once.
-	const Backend backends[] = {
-		{"cv        BFMatcher                 ", FlannIndex::FLANN_INDEX_LINEAR, 1.0f, true},
+	std::vector<Backend> backends = {
+		{"cv        BFMatcher                 ", FlannIndex::FLANN_INDEX_LINEAR, 1.0f, true, 1},
+		{"cv        BFMatcher threaded        ", FlannIndex::FLANN_INDEX_LINEAR, 1.0f, true, 0},
 		{"rtflann   kd-tree (4 randomized)    ", FlannIndex::FLANN_INDEX_KDTREE, 1.0f},
 		{"rtflann   kd-tree single            ", FlannIndex::FLANN_INDEX_KDTREE_SINGLE, 1.0f},
 		{"nanoflann kd-tree single            ", FlannIndex::NANOFLANN_INDEX_KDTREE_SINGLE, 1.0f},
 		{"nanoflann kd-tree single incremental", FlannIndex::NANOFLANN_INDEX_KDTREE_SINGLE, 2.0f},
 	};
 
+#ifdef _OPENMP
+	// rtflann threads a radius search over its batch of queries the same way it
+	// threads a kNN one, so the two trees come back with one thread per core.
+	// The tree is still built on one core, and here it is rebuilt every frame,
+	// which caps what threading can take off the total: the times say how much
+	// of a frame is the search rather than the build.
+	backends.push_back({"rtflann   kd-tree (4 rand.) threaded", FlannIndex::FLANN_INDEX_KDTREE,        1.0f, false, 0});
+	backends.push_back({"rtflann   kd-tree single threaded   ", FlannIndex::FLANN_INDEX_KDTREE_SINGLE, 1.0f, false, 0});
+#endif
+
 	std::cout << "[          ] " << keypoints << " keypoints indexed and as many looked up in a "
 			  << radius << " px radius, per frame" << std::endl;
+#ifdef _OPENMP
+	std::cout << "[          ] the threaded rows search with " << omp_get_max_threads()
+			  << " threads, the others with one" << std::endl;
+#endif
 
 	for(const Backend & backend: backends)
 	{
@@ -538,7 +557,7 @@ TEST(FlannIndexPerfTest, RegistrationGuessMatching)
 		{
 			FlannIndex index;
 			index.buildIndex(backend.algorithm, points, false, backend.rebalancingFactor);
-			index.radiusSearch(projected, indices, dists, radius, 0, 32, 0.0f, false);
+			index.radiusSearch(projected, indices, dists, radius, 0, 32, 0.0f, false, backend.cores);
 		}
 		const double perFrame = timer.ticks()/double(frames);
 
@@ -571,15 +590,31 @@ void compareDictionaryMatching(int indexedCount, int queriedCount)
 	// built once, so it is neither kept ready to be added to nor rebuilt. The
 	// incremental nanoflann tree is kept in the comparison to show what asking
 	// for one costs here.
-	const Backend backends[] = {
+	std::vector<Backend> backends = {
 		{"linear    exhaustive                ", FlannIndex::FLANN_INDEX_LINEAR, 1.0f},
-		{"cv        BFMatcher                 ", FlannIndex::FLANN_INDEX_LINEAR, 1.0f, true},
+		{"cv        BFMatcher                 ", FlannIndex::FLANN_INDEX_LINEAR, 1.0f, true, 1},
+		{"cv        BFMatcher threaded        ", FlannIndex::FLANN_INDEX_LINEAR, 1.0f, true, 0},
 		{"rtflann   kd-tree (4 randomized)    ", FlannIndex::FLANN_INDEX_KDTREE, 1.0f},
 		{"rtflann   kd-tree single            ", FlannIndex::FLANN_INDEX_KDTREE_SINGLE, 1.0f},
 		{"nanoflann kd-tree single            ", FlannIndex::NANOFLANN_INDEX_KDTREE_SINGLE, 1.0f},
 		{"nanoflann kd-tree single incremental", FlannIndex::NANOFLANN_INDEX_KDTREE_SINGLE, 2.0f},
 	};
 
+#ifdef _OPENMP
+	// The same two rtflann trees searched with Kp/FlannThreads=0, one thread per
+	// core: rtflann is the only backend here threading a batch of queries, over
+	// an OpenMP loop. Only the search half of the times can improve, the trees
+	// are still built on one core. Queries are independent of each other, so
+	// threading doesn't change what is found: the exact tree holds its recall to
+	// the digit. The randomized one moves by a tenth of a percent from one run to
+	// the next whether threaded or not, it randomizes its splits on every build.
+	backends.push_back({"rtflann   kd-tree (4 rand.) threaded", FlannIndex::FLANN_INDEX_KDTREE,        1.0f, false, 0});
+	backends.push_back({"rtflann   kd-tree single threaded   ", FlannIndex::FLANN_INDEX_KDTREE_SINGLE, 1.0f, false, 0});
+
+	std::cout << "[          ] the threaded rows search with " << omp_get_max_threads()
+			  << " threads, the others with one" << std::endl;
+#endif
+
 	for(int dim: {32, 64, 128, 256})
 	{
 		const cv::Mat from = makeDescriptors(indexedCount, dim, clusterCount(indexedCount), 150);
@@ -599,7 +634,7 @@ void compareDictionaryMatching(int indexedCount, int queriedCount)
 			{
 				FlannIndex index;
 				index.buildIndex(backend.algorithm, from, false, backend.rebalancingFactor);
-				index.knnSearch(to, indices, dists, KNN);
+				index.knnSearch(to, indices, dists, KNN, 32, 0.0f, true, backend.cores);
 			}
 			const double perFrame = timer.ticks()/double(frames);