0.12.3: Increased Tango rendering performance, modified all handleEvent() with new interface (now returning bool)

corelib/include/rtabmap/core/Memory.h

@@ -255,6 +255,7 @@ private:
 	bool _mapLabelsAdded;
 	int _imagePreDecimation;
 	int _imagePostDecimation;
+	bool _compressionParallelized;
 	float _laserScanDownsampleStepSize;
 	int _laserScanNormalK;
 	bool _reextractLoopClosureFeatures;

corelib/include/rtabmap/core/OdometryThread.h

@@ -45,7 +45,7 @@ public:
 	virtual ~OdometryThread();
 
 protected:
-	virtual void handleEvent(UEvent * event);
+	virtual bool handleEvent(UEvent * event);
 
 private:
 	virtual void mainLoopBegin();

corelib/include/rtabmap/core/Parameters.h

@@ -175,7 +175,7 @@ class RTABMAP_EXP Parameters
 	RTABMAP_PARAM(Rtabmap, PublishLikelihood, 	         bool, true, "Publishing likelihood.");
 	RTABMAP_PARAM(Rtabmap, TimeThr, 		             float, 0,   "Maximum time allowed for the detector (ms) (0 means infinity).");
 	RTABMAP_PARAM(Rtabmap, MemoryThr, 		             int, 0, 	 "Maximum signatures in the Working Memory (ms) (0 means infinity).");
-	RTABMAP_PARAM(Rtabmap, DetectionRate,                float, 1,   "Detection rate. RTAB-Map will filter input images to satisfy this rate.");
+	RTABMAP_PARAM(Rtabmap, DetectionRate,                float, 1,   "Detection rate (Hz). RTAB-Map will filter input images to satisfy this rate.");
 	RTABMAP_PARAM(Rtabmap, ImageBufferSize,              unsigned int, 1, "Data buffer size (0 min inf).");
 	RTABMAP_PARAM(Rtabmap, CreateIntermediateNodes,      bool, false, uFormat("Create intermediate nodes between loop closure detection. Only used when %s>0.", kRtabmapDetectionRate().c_str()));
 	RTABMAP_PARAM_STR(Rtabmap, WorkingDirectory,         "", "Working directory.");
@@ -209,6 +209,7 @@ class RTABMAP_EXP Parameters
 	RTABMAP_PARAM(Mem, InitWMWithAllNodes,      bool, false,     "Initialize the Working Memory with all nodes in Long-Term Memory. When false, it is initialized with nodes of the previous session.");
 	RTABMAP_PARAM(Mem, ImagePreDecimation,      int, 1,          "Image decimation (>=1) before features extraction. Negative decimation is done from RGB size instead of depth size (if depth is smaller than RGB, it may be interpolated depending of the decimation value).");
 	RTABMAP_PARAM(Mem, ImagePostDecimation,     int, 1,          "Image decimation (>=1) of saved data in created signatures (after features extraction). Decimation is done from the original image. Negative decimation is done from RGB size instead of depth size (if depth is smaller than RGB, it may be interpolated depending of the decimation value).");
+	RTABMAP_PARAM(Mem, CompressionParallelized, bool, true,      "Compression of sensor data is multi-threaded.");
 	RTABMAP_PARAM(Mem, LaserScanDownsampleStepSize, int, 1,      "If > 1, downsample the laser scans when creating a signature.");
 	RTABMAP_PARAM(Mem, LaserScanNormalK,        int, 0,          "If > 0 and laser scans are 3D without normals, normals will be computed with K search neighbors when creating a signature.");
 	RTABMAP_PARAM(Mem, UseOdomFeatures,         bool, false, "Use odometry features.");

corelib/include/rtabmap/core/ProgressState.h

@@ -32,11 +32,24 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 class ProgressState
 {
 public:
+	ProgressState():canceled_(false){}
 	virtual bool callback(const std::string & msg) const
 	{
 		return true;
 	}
 	virtual ~ProgressState(){}
+
+	void setCanceled(bool canceled)
+	{
+		canceled_ = canceled;
+	}
+	bool isCanceled() const
+	{
+		return canceled_;
+	}
+
+private:
+	bool canceled_;
 };
 
 

corelib/include/rtabmap/core/RtabmapThread.h

@@ -82,6 +82,10 @@ public:
 	void setDataBufferSize(unsigned int bufferSize);
 	void createIntermediateNodes(bool enabled);
 
+	float getDetectorRate() const {return _rate;}
+	unsigned int getDataBufferSize() const {return _dataBufferMaxSize;}
+	bool getCreateIntermediateNodes() const {return _createIntermediateNodes;}
+
 	/**
 	 * Close rtabmap. This will delete rtabmap object if set.
 	 * @param databaseSaved true=database saved, false=database discarded.
@@ -92,7 +96,7 @@ public:
 	void close(bool databaseSaved, const std::string & databasePath = "");
 
 protected:
-	virtual void handleEvent(UEvent * anEvent);
+	virtual bool handleEvent(UEvent * anEvent);
 
 private:
 	virtual void mainLoopBegin();

corelib/src/Memory.cpp

@@ -85,6 +85,7 @@ Memory::Memory(const ParametersMap & parameters) :
 	_mapLabelsAdded(Parameters::defaultMemMapLabelsAdded()),
 	_imagePreDecimation(Parameters::defaultMemImagePreDecimation()),
 	_imagePostDecimation(Parameters::defaultMemImagePostDecimation()),
+	_compressionParallelized(Parameters::defaultMemCompressionParallelized()),
 	_laserScanDownsampleStepSize(Parameters::defaultMemLaserScanDownsampleStepSize()),
 	_laserScanNormalK(Parameters::defaultMemLaserScanNormalK()),
 	_reextractLoopClosureFeatures(Parameters::defaultRGBDLoopClosureReextractFeatures()),
@@ -429,6 +430,7 @@ void Memory::parseParameters(const ParametersMap & parameters)
 	Parameters::parse(parameters, Parameters::kMemSTMSize(), _maxStMemSize);
 	Parameters::parse(parameters, Parameters::kMemImagePreDecimation(), _imagePreDecimation);
 	Parameters::parse(parameters, Parameters::kMemImagePostDecimation(), _imagePostDecimation);
+	Parameters::parse(parameters, Parameters::kMemCompressionParallelized(), _compressionParallelized);
 	Parameters::parse(parameters, Parameters::kMemLaserScanDownsampleStepSize(), _laserScanDownsampleStepSize);
 	Parameters::parse(parameters, Parameters::kMemLaserScanNormalK(), _laserScanNormalK);
 	Parameters::parse(parameters, Parameters::kRGBDLoopClosureReextractFeatures(), _reextractLoopClosureFeatures);
@@ -3679,30 +3681,49 @@ Signature * Memory::createSignature(const SensorData & data, const Transform & p
 			depthOrRightImage = util2d::cvtDepthFromFloat(depthOrRightImage);
 		}
 
-		rtabmap::CompressionThread ctImage(image, std::string(".jpg"));
-		rtabmap::CompressionThread ctDepth(depthOrRightImage, std::string(".png"));
-		rtabmap::CompressionThread ctLaserScan(laserScan);
-		rtabmap::CompressionThread ctUserData(data.userDataRaw());
-		if(!image.empty())
+		cv::Mat compressedImage;
+		cv::Mat compressedDepth;
+		cv::Mat compressedScan;
+		cv::Mat compressedUserData;
+		if(_compressionParallelized)
 		{
-			ctImage.start();
+			rtabmap::CompressionThread ctImage(image, std::string(".jpg"));
+			rtabmap::CompressionThread ctDepth(depthOrRightImage, std::string(".png"));
+			rtabmap::CompressionThread ctLaserScan(laserScan);
+			rtabmap::CompressionThread ctUserData(data.userDataRaw());
+			if(!image.empty())
+			{
+				ctImage.start();
+			}
+			if(!depthOrRightImage.empty())
+			{
+				ctDepth.start();
+			}
+			if(!laserScan.empty())
+			{
+				ctLaserScan.start();
+			}
+			if(!data.userDataRaw().empty())
+			{
+				ctUserData.start();
+			}
+			ctImage.join();
+			ctDepth.join();
+			ctLaserScan.join();
+			ctUserData.join();
+
+			compressedImage = ctImage.getCompressedData();
+			compressedDepth = ctDepth.getCompressedData();
+			compressedScan = ctLaserScan.getCompressedData();
+			compressedUserData = ctUserData.getCompressedData();
 		}
-		if(!depthOrRightImage.empty())
+		else
 		{
-			ctDepth.start();
+			compressedImage = compressImage2(image, std::string(".jpg"));
+			compressedDepth = compressImage2(depthOrRightImage, depthOrRightImage.type() == CV_32FC1 || depthOrRightImage.type() == CV_16UC1?std::string(".png"):std::string(".jpg"));
+			compressedScan = compressData2(laserScan);
+			compressedUserData = compressData2(data.userDataRaw());
 		}
-		if(!laserScan.empty())
-		{
-			ctLaserScan.start();
-		}
-		if(!data.userDataRaw().empty())
-		{
-			ctUserData.start();
-		}
-		ctImage.join();
-		ctDepth.join();
-		ctLaserScan.join();
-		ctUserData.join();
 
 		s = new Signature(id,
 			_idMapCount,
@@ -3713,23 +3734,23 @@ Signature * Memory::createSignature(const SensorData & data, const Transform & p
 			data.groundTruth(),
 			stereoCameraModel.isValidForProjection()?
 				SensorData(
-						ctLaserScan.getCompressedData(),
+						compressedScan,
 						LaserScanInfo(maxLaserScanMaxPts, data.laserScanInfo().maxRange(), data.laserScanInfo().localTransform()),
-						ctImage.getCompressedData(),
-						ctDepth.getCompressedData(),
+						compressedImage,
+						compressedDepth,
 						stereoCameraModel,
 						id,
 						0,
-						ctUserData.getCompressedData()):
+						compressedUserData):
 				SensorData(
-						ctLaserScan.getCompressedData(),
+						compressedScan,
 						LaserScanInfo(maxLaserScanMaxPts, data.laserScanInfo().maxRange(), data.laserScanInfo().localTransform()),
-						ctImage.getCompressedData(),
-						ctDepth.getCompressedData(),
+						compressedImage,
+						compressedDepth,
 						cameraModels,
 						id,
 						0,
-						ctUserData.getCompressedData()));
+						compressedUserData));
 	}
 	else
 	{
@@ -3738,18 +3759,31 @@ Signature * Memory::createSignature(const SensorData & data, const Transform & p
 						data.userDataRaw().empty()?0:1);
 
 		// just compress user data and laser scan (scans can be used for local scan matching)
-		rtabmap::CompressionThread ctUserData(data.userDataRaw());
-		rtabmap::CompressionThread ctLaserScan(laserScan);
-		if(!data.userDataRaw().empty() && !isIntermediateNode)
+		cv::Mat compressedScan;
+		cv::Mat compressedUserData;
+		if(_compressionParallelized)
 		{
-			ctUserData.start();
+			rtabmap::CompressionThread ctUserData(data.userDataRaw());
+			rtabmap::CompressionThread ctLaserScan(laserScan);
+			if(!data.userDataRaw().empty() && !isIntermediateNode)
+			{
+				ctUserData.start();
+			}
+			if(!laserScan.empty() && !isIntermediateNode)
+			{
+				ctLaserScan.start();
+			}
+			ctUserData.join();
+			ctLaserScan.join();
+
+			compressedScan = ctLaserScan.getCompressedData();
+			compressedUserData = ctUserData.getCompressedData();
 		}
-		if(!laserScan.empty() && !isIntermediateNode)
+		else
 		{
-			ctLaserScan.start();
+			compressedScan = compressData2(laserScan);
+			compressedUserData = compressData2(data.userDataRaw());
 		}
-		ctUserData.join();
-		ctLaserScan.join();
 
 		s = new Signature(id,
 			_idMapCount,
@@ -3760,23 +3794,23 @@ Signature * Memory::createSignature(const SensorData & data, const Transform & p
 			data.groundTruth(),
 			stereoCameraModel.isValidForProjection()?
 				SensorData(
-						ctLaserScan.getCompressedData(),
+						compressedScan,
 						LaserScanInfo(maxLaserScanMaxPts, data.laserScanInfo().maxRange(), data.laserScanInfo().localTransform()),
 						cv::Mat(),
 						cv::Mat(),
 						stereoCameraModel,
 						id,
 						0,
-						ctUserData.getCompressedData()):
+						compressedUserData):
 				SensorData(
-						ctLaserScan.getCompressedData(),
+						compressedScan,
 						LaserScanInfo(maxLaserScanMaxPts, data.laserScanInfo().maxRange(), data.laserScanInfo().localTransform()),
 						cv::Mat(),
 						cv::Mat(),
 						cameraModels,
 						id,
 						0,
-						ctUserData.getCompressedData()));
+						compressedUserData));
 	}
 
 	s->setWords(words);

corelib/src/OdometryThread.cpp

@@ -55,7 +55,7 @@ OdometryThread::~OdometryThread()
 	UDEBUG("");
 }
 
-void OdometryThread::handleEvent(UEvent * event)
+bool OdometryThread::handleEvent(UEvent * event)
 {
 	if(this->isRunning())
 	{
@@ -72,6 +72,7 @@ void OdometryThread::handleEvent(UEvent * event)
 			_resetOdometry = true;
 		}
 	}
+	return false;
 }
 
 void OdometryThread::mainLoopBegin()

corelib/src/Rtabmap.cpp

@@ -3456,6 +3456,12 @@ int Rtabmap::detectMoreLoopClosures(float clusterRadius, float clusterAngle, int
 		std::set<int> addedLinks;
 		for(std::multimap<int, int>::iterator iter=clusters.begin(); iter!= clusters.end(); ++iter, ++i)
 		{
+			if(processState && processState->isCanceled())
+			{
+				return -1;
+				break;
+			}
+
 			int from = iter->first;
 			int to = iter->second;
 			if(iter->first < iter->second)

corelib/src/RtabmapThread.cpp

@@ -324,7 +324,7 @@ void RtabmapThread::mainLoop()
 }
 
 
-void RtabmapThread::handleEvent(UEvent* event)
+bool RtabmapThread::handleEvent(UEvent* event)
 {
 	if(this->isRunning())
 	{
@@ -517,6 +517,7 @@ void RtabmapThread::handleEvent(UEvent* event)
 			pushNewState(kStateChangingParameters, ((ParamEvent*)event)->getParameters());
 		}
 	}
+	return false;
 }
 
 //============================================================

corelib/src/rtflann/algorithms/kdtree_index.h

@@ -79,6 +79,75 @@ public:
 
     typedef bool needs_kdtree_distance;
 
+private:
+	 /*--------------------- Internal Data Structures --------------------------*/
+	struct Node
+	{
+		/**
+		 * Dimension used for subdivision.
+		 */
+		int divfeat;
+		/**
+		 * The values used for subdivision.
+		 */
+		DistanceType divval;
+		/**
+		 * Point data
+		 */
+		ElementType* point;
+		/**
+		* The child nodes.
+		*/
+		Node* child1, *child2;
+		Node(){
+			child1 = NULL;
+			child2 = NULL;
+		}
+		~Node() {
+			if (child1 != NULL) { child1->~Node(); child1 = NULL; }
+
+			if (child2 != NULL) { child2->~Node(); child2 = NULL; }
+		}
+
+	private:
+		template<typename Archive>
+		void serialize(Archive& ar)
+		{
+			typedef KDTreeIndex<Distance> Index;
+			Index* obj = static_cast<Index*>(ar.getObject());
+
+			ar & divfeat;
+			ar & divval;
+
+			bool leaf_node = false;
+			if (Archive::is_saving::value) {
+				leaf_node = ((child1==NULL) && (child2==NULL));
+			}
+			ar & leaf_node;
+
+			if (leaf_node) {
+				if (Archive::is_loading::value) {
+					point = obj->points_[divfeat];
+				}
+			}
+
+			if (!leaf_node) {
+				if (Archive::is_loading::value) {
+					child1 = new(obj->pool_) Node();
+					child2 = new(obj->pool_) Node();
+				}
+				ar & *child1;
+				ar & *child2;
+			}
+		}
+		friend struct serialization::access;
+	};
+
+	typedef Node* NodePtr;
+	typedef BranchStruct<NodePtr, DistanceType> BranchSt;
+	typedef BranchSt* Branch;
+
+public:
 
     /**
      * KDTree constructor
@@ -245,6 +314,349 @@ public:
         }
     }
 
+#ifdef ANDROID
+
+    /**
+	 * Find set of nearest neighbors to vec. Their indices are stored inside
+	 * the result object.
+	 *
+	 * Params:
+	 *     result = the result object in which the indices of the nearest-neighbors are stored
+	 *     vec = the vector for which to search the nearest neighbors
+	 *     maxCheck = the maximum number of restarts (in a best-bin-first manner)
+	 */
+	void findNeighbors(ResultSet<DistanceType>& result, const ElementType* vec, const SearchParams& searchParams, Heap<BranchSt>* heap) const
+	{
+		int maxChecks = searchParams.checks;
+		float epsError = 1+searchParams.eps;
+
+		if (maxChecks==FLANN_CHECKS_UNLIMITED) {
+			if (removed_) {
+				getExactNeighbors<true>(result, vec, epsError);
+			}
+			else {
+				getExactNeighbors<false>(result, vec, epsError);
+			}
+		}
+		else {
+			if (removed_) {
+				getNeighbors<true>(result, vec, maxChecks, epsError, heap);
+			}
+			else {
+				getNeighbors<false>(result, vec, maxChecks, epsError, heap);
+			}
+		}
+	}
+
+	/**
+	 * @brief Perform k-nearest neighbor search
+	 * @param[in] queries The query points for which to find the nearest neighbors
+	 * @param[out] indices The indices of the nearest neighbors found
+	 * @param[out] dists Distances to the nearest neighbors found
+	 * @param[in] knn Number of nearest neighbors to return
+	 * @param[in] params Search parameters
+	 */
+	virtual int knnSearch(const Matrix<ElementType>& queries,
+			Matrix<size_t>& indices,
+			Matrix<DistanceType>& dists,
+			size_t knn,
+			const SearchParams& params) const
+	{
+		assert(queries.cols == veclen());
+		assert(indices.rows >= queries.rows);
+		assert(dists.rows >= queries.rows);
+		assert(indices.cols >= knn);
+		assert(dists.cols >= knn);
+		bool use_heap;
+
+		if (params.use_heap==FLANN_Undefined) {
+			use_heap = (knn>KNN_HEAP_THRESHOLD)?true:false;
+		}
+		else {
+			use_heap = (params.use_heap==FLANN_True)?true:false;
+		}
+		int count = 0;
+
+		Heap<BranchSt>* heap = new Heap<BranchSt>((int)size_);
+
+		if (use_heap) {
+	//#pragma omp parallel num_threads(params.cores)
+			{
+				KNNResultSet2<DistanceType> resultSet(knn);
+	//#pragma omp for schedule(static) reduction(+:count)
+				for (int i = 0; i < (int)queries.rows; i++) {
+					resultSet.clear();
+					findNeighbors(resultSet, queries[i], params, heap);
+					size_t n = std::min(resultSet.size(), knn);
+					resultSet.copy(indices[i], dists[i], n, params.sorted);
+					indices_to_ids(indices[i], indices[i], n);
+					count += n;
+				}
+			}
+		}
+		else {
+			std::vector<double> times(queries.rows);
+	//#pragma omp parallel num_threads(params.cores)
+			{
+				KNNSimpleResultSet<DistanceType> resultSet(knn);
+	//#pragma omp for schedule(static) reduction(+:count)
+				for (int i = 0; i < (int)queries.rows; i++) {
+					resultSet.clear();
+					findNeighbors(resultSet, queries[i], params, heap);
+					size_t n = std::min(resultSet.size(), knn);
+					resultSet.copy(indices[i], dists[i], n, params.sorted);
+					indices_to_ids(indices[i], indices[i], n);
+					count += n;
+				}
+			}
+			std::sort(times.begin(), times.end());
+		}
+		delete heap;
+		return count;
+	}
+
+
+	/**
+	 * @brief Perform k-nearest neighbor search
+	 * @param[in] queries The query points for which to find the nearest neighbors
+	 * @param[out] indices The indices of the nearest neighbors found
+	 * @param[out] dists Distances to the nearest neighbors found
+	 * @param[in] knn Number of nearest neighbors to return
+	 * @param[in] params Search parameters
+	 */
+	virtual int knnSearch(const Matrix<ElementType>& queries,
+					std::vector< std::vector<size_t> >& indices,
+					std::vector<std::vector<DistanceType> >& dists,
+					size_t knn,
+					const SearchParams& params) const
+	{
+		assert(queries.cols == veclen());
+		bool use_heap;
+		if (params.use_heap==FLANN_Undefined) {
+			use_heap = (knn>KNN_HEAP_THRESHOLD)?true:false;
+		}
+		else {
+			use_heap = (params.use_heap==FLANN_True)?true:false;
+		}
+
+		if (indices.size() < queries.rows ) indices.resize(queries.rows);
+		if (dists.size() < queries.rows ) dists.resize(queries.rows);
+
+		Heap<BranchSt>* heap = new Heap<BranchSt>((int)size_);
+
+		int count = 0;
+		if (use_heap) {
+	//#pragma omp parallel num_threads(params.cores)
+			{
+				KNNResultSet2<DistanceType> resultSet(knn);
+	//#pragma omp for schedule(static) reduction(+:count)
+				for (int i = 0; i < (int)queries.rows; i++) {
+					resultSet.clear();
+					findNeighbors(resultSet, queries[i], params, heap);
+					size_t n = std::min(resultSet.size(), knn);
+					indices[i].resize(n);
+					dists[i].resize(n);
+					if (n>0) {
+						resultSet.copy(&indices[i][0], &dists[i][0], n, params.sorted);
+						indices_to_ids(&indices[i][0], &indices[i][0], n);
+					}
+					count += n;
+				}
+			}
+		}
+		else {
+	//#pragma omp parallel num_threads(params.cores)
+			{
+				KNNSimpleResultSet<DistanceType> resultSet(knn);
+	//#pragma omp for schedule(static) reduction(+:count)
+				for (int i = 0; i < (int)queries.rows; i++) {
+					resultSet.clear();
+					findNeighbors(resultSet, queries[i], params, heap);
+					size_t n = std::min(resultSet.size(), knn);
+					indices[i].resize(n);
+					dists[i].resize(n);
+					if (n>0) {
+						resultSet.copy(&indices[i][0], &dists[i][0], n, params.sorted);
+						indices_to_ids(&indices[i][0], &indices[i][0], n);
+					}
+					count += n;
+				}
+			}
+		}
+		delete heap;
+
+		return count;
+	}
+
+	/**
+	 * @brief Perform radius search
+	 * @param[in] query The query point
+	 * @param[out] indices The indices of the neighbors found within the given radius
+	 * @param[out] dists The distances to the nearest neighbors found
+	 * @param[in] radius The radius used for search
+	 * @param[in] params Search parameters
+	 * @return Number of neighbors found
+	 */
+	virtual int radiusSearch(const Matrix<ElementType>& queries,
+			Matrix<size_t>& indices,
+			Matrix<DistanceType>& dists,
+			float radius,
+			const SearchParams& params) const
+	{
+		assert(queries.cols == veclen());
+		int count = 0;
+		size_t num_neighbors = std::min(indices.cols, dists.cols);
+		int max_neighbors = params.max_neighbors;
+		if (max_neighbors<0) max_neighbors = num_neighbors;
+		else max_neighbors = std::min(max_neighbors,(int)num_neighbors);
+
+		Heap<BranchSt>* heap = new Heap<BranchSt>((int)size_);
+
+		if (max_neighbors==0) {
+	//#pragma omp parallel num_threads(params.cores)
+			{
+				CountRadiusResultSet<DistanceType> resultSet(radius);
+	//#pragma omp for schedule(static) reduction(+:count)
+				for (int i = 0; i < (int)queries.rows; i++) {
+					resultSet.clear();
+					findNeighbors(resultSet, queries[i], params, heap);
+					count += resultSet.size();
+				}
+			}
+		}
+		else {
+			// explicitly indicated to use unbounded radius result set
+			// and we know there'll be enough room for resulting indices and dists
+			if (params.max_neighbors<0 && (num_neighbors>=this->size())) {
+	//#pragma omp parallel num_threads(params.cores)
+				{
+					RadiusResultSet<DistanceType> resultSet(radius);
+	//#pragma omp for schedule(static) reduction(+:count)
+					for (int i = 0; i < (int)queries.rows; i++) {
+						resultSet.clear();
+						findNeighbors(resultSet, queries[i], params, heap);
+						size_t n = resultSet.size();
+						count += n;
+						if (n>num_neighbors) n = num_neighbors;
+						resultSet.copy(indices[i], dists[i], n, params.sorted);
+
+						// mark the next element in the output buffers as unused
+						if (n<indices.cols) indices[i][n] = size_t(-1);
+						if (n<dists.cols) dists[i][n] = std::numeric_limits<DistanceType>::infinity();
+						indices_to_ids(indices[i], indices[i], n);
+					}
+				}
+			}
+			else {
+				// number of neighbors limited to max_neighbors
+	//#pragma omp parallel num_threads(params.cores)
+				{
+					KNNRadiusResultSet<DistanceType> resultSet(radius, max_neighbors);
+	//#pragma omp for schedule(static) reduction(+:count)
+					for (int i = 0; i < (int)queries.rows; i++) {
+						resultSet.clear();
+						findNeighbors(resultSet, queries[i], params, heap);
+						size_t n = resultSet.size();
+						count += n;
+						if ((int)n>max_neighbors) n = max_neighbors;
+						resultSet.copy(indices[i], dists[i], n, params.sorted);
+
+						// mark the next element in the output buffers as unused
+						if (n<indices.cols) indices[i][n] = size_t(-1);
+						if (n<dists.cols) dists[i][n] = std::numeric_limits<DistanceType>::infinity();
+						indices_to_ids(indices[i], indices[i], n);
+					}
+				}
+			}
+		}
+		delete heap;
+		return count;
+	}
+
+	/**
+	 * @brief Perform radius search
+	 * @param[in] query The query point
+	 * @param[out] indices The indices of the neighbors found within the given radius
+	 * @param[out] dists The distances to the nearest neighbors found
+	 * @param[in] radius The radius used for search
+	 * @param[in] params Search parameters
+	 * @return Number of neighbors found
+	 */
+	virtual int radiusSearch(const Matrix<ElementType>& queries,
+			std::vector< std::vector<size_t> >& indices,
+			std::vector<std::vector<DistanceType> >& dists,
+			float radius,
+			const SearchParams& params) const
+	{
+		assert(queries.cols == veclen());
+		int count = 0;
+
+		Heap<BranchSt>* heap = new Heap<BranchSt>((int)size_);
+
+		// just count neighbors
+		if (params.max_neighbors==0) {
+	//#pragma omp parallel num_threads(params.cores)
+			{
+				CountRadiusResultSet<DistanceType> resultSet(radius);
+	//#pragma omp for schedule(static) reduction(+:count)
+				for (int i = 0; i < (int)queries.rows; i++) {
+					resultSet.clear();
+					findNeighbors(resultSet, queries[i], params, heap);
+					count += resultSet.size();
+				}
+			}
+		}
+		else {
+			if (indices.size() < queries.rows ) indices.resize(queries.rows);
+			if (dists.size() < queries.rows ) dists.resize(queries.rows);
+
+			if (params.max_neighbors<0) {
+				// search for all neighbors
+	//#pragma omp parallel num_threads(params.cores)
+				{
+					RadiusResultSet<DistanceType> resultSet(radius);
+	//#pragma omp for schedule(static) reduction(+:count)
+					for (int i = 0; i < (int)queries.rows; i++) {
+						resultSet.clear();
+						findNeighbors(resultSet, queries[i], params, heap);
+						size_t n = resultSet.size();
+						count += n;
+						indices[i].resize(n);
+						dists[i].resize(n);
+						if (n > 0) {
+							resultSet.copy(&indices[i][0], &dists[i][0], n, params.sorted);
+							indices_to_ids(&indices[i][0], &indices[i][0], n);
+						}
+					}
+				}
+			}
+			else {
+				// number of neighbors limited to max_neighbors
+	//#pragma omp parallel num_threads(params.cores)
+				{
+					KNNRadiusResultSet<DistanceType> resultSet(radius, params.max_neighbors);
+	//#pragma omp for schedule(static) reduction(+:count)
+					for (int i = 0; i < (int)queries.rows; i++) {
+						resultSet.clear();
+						findNeighbors(resultSet, queries[i], params, heap);
+						size_t n = resultSet.size();
+						count += n;
+						if ((int)n>params.max_neighbors) n = params.max_neighbors;
+						indices[i].resize(n);
+						dists[i].resize(n);
+						if (n > 0) {
+							resultSet.copy(&indices[i][0], &dists[i][0], n, params.sorted);
+							indices_to_ids(&indices[i][0], &indices[i][0], n);
+						}
+					}
+				}
+			}
+		}
+		delete heap;
+		return count;
+	}
+#endif
+
 protected:
 
     /**
@@ -284,73 +696,6 @@ protected:
 
 private:
 
-    /*--------------------- Internal Data Structures --------------------------*/
-    struct Node
-    {
-    	/**
-         * Dimension used for subdivision.
-         */
-        int divfeat;
-        /**
-         * The values used for subdivision.
-         */
-        DistanceType divval;
-        /**
-         * Point data
-         */
-        ElementType* point;
-		/**
-		* The child nodes.
-		*/
-		Node* child1, *child2;
-		Node(){
-			child1 = NULL;
-			child2 = NULL;
-		}
-		~Node() {
-			if (child1 != NULL) { child1->~Node(); child1 = NULL; }
-
-			if (child2 != NULL) { child2->~Node(); child2 = NULL; }
-		}
-
-    private:
-    	template<typename Archive>
-    	void serialize(Archive& ar)
-    	{
-    		typedef KDTreeIndex<Distance> Index;
-    		Index* obj = static_cast<Index*>(ar.getObject());
-
-    		ar & divfeat;
-    		ar & divval;
-
-    		bool leaf_node = false;
-    		if (Archive::is_saving::value) {
-    			leaf_node = ((child1==NULL) && (child2==NULL));
-    		}
-    		ar & leaf_node;
-
-    		if (leaf_node) {
-    			if (Archive::is_loading::value) {
-    				point = obj->points_[divfeat];
-    			}
-    		}
-
-    		if (!leaf_node) {
-				if (Archive::is_loading::value) {
-					child1 = new(obj->pool_) Node();
-					child2 = new(obj->pool_) Node();
-				}
-    			ar & *child1;
-    			ar & *child2;
-    		}
-    	}
-    	friend struct serialization::access;
-    };
-    typedef Node* NodePtr;
-    typedef BranchStruct<NodePtr, DistanceType> BranchSt;
-    typedef BranchSt* Branch;
-
-
     void copyTree(NodePtr& dst, const NodePtr& src)
     {
     	dst = new(pool_) Node();
@@ -563,6 +908,35 @@ private:
 
     }
 
+#ifdef ANDROID
+    /**
+	 * Performs the approximate nearest-neighbor search. The search is approximate
+	 * because the tree traversal is abandoned after a given number of descends in
+	 * the tree.
+	 */
+	template<bool with_removed>
+	void getNeighbors(ResultSet<DistanceType>& result, const ElementType* vec, int maxCheck, float epsError, Heap<BranchSt>* heap) const
+	{
+		int i;
+		BranchSt branch;
+
+		int checkCount = 0;
+		DynamicBitset checked(size_);
+		heap->clear();
+
+		/* Search once through each tree down to root. */
+		for (i = 0; i < trees_; ++i) {
+			searchLevel<with_removed>(result, vec, tree_roots_[i], 0, checkCount, maxCheck, epsError, heap, checked);
+		}
+
+		/* Keep searching other branches from heap until finished. */
+		while ( heap->popMin(branch) && (checkCount < maxCheck || !result.full() )) {
+			searchLevel<with_removed>(result, vec, branch.node, branch.mindist, checkCount, maxCheck, epsError, heap, checked);
+		}
+	}
+#endif
+
+
     /**
      *  Search starting from a given node of the tree.  Based on any mismatches at
      *  higher levels, all exemplars below this level must have a distance of

corelib/src/rtflann/algorithms/nn_index.h

@@ -402,7 +402,7 @@ public:
      * @param[in] knn Number of nearest neighbors to return
      * @param[in] params Search parameters
      */
-    int knnSearch(const Matrix<ElementType>& queries,
+    virtual int knnSearch(const Matrix<ElementType>& queries,
 					std::vector< std::vector<size_t> >& indices,
 					std::vector<std::vector<DistanceType> >& dists,
     				size_t knn,
@@ -498,7 +498,7 @@ public:
      * @param[in] params Search parameters
      * @return Number of neighbors found
      */
-    int radiusSearch(const Matrix<ElementType>& queries,
+    virtual int radiusSearch(const Matrix<ElementType>& queries,
     		Matrix<size_t>& indices,
     		Matrix<DistanceType>& dists,
     		float radius,
@@ -608,7 +608,7 @@ public:
      * @param[in] params Search parameters
      * @return Number of neighbors found
      */
-    int radiusSearch(const Matrix<ElementType>& queries,
+    virtual int radiusSearch(const Matrix<ElementType>& queries,
     		std::vector< std::vector<size_t> >& indices,
     		std::vector<std::vector<DistanceType> >& dists,
     		float radius,

corelib/src/rtflann/util/heap.h

@@ -33,6 +33,7 @@
 
 #include <algorithm>
 #include <vector>
+#include <rtabmap/utilite/ULogger.h>
 
 namespace rtflann
 {
@@ -86,6 +87,15 @@ public:
         return count;
     }
 
+    /**
+	 *
+	 * Returns: heap size
+	 */
+	int capacity()
+	{
+		return length;
+	}
+
     /**
      * Tests if the heap is empty
      *
@@ -129,6 +139,7 @@ public:
             return;
         }
 
+        UASSERT(heap.size() < heap.capacity());
         heap.push_back(value);
         static CompareT compareT;
         std::push_heap(heap.begin(), heap.end(), compareT);