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

CMakeLists.txt

@@ -21,7 +21,7 @@ SET(CMAKE_MODULE_PATH "${PROJECT_SOURCE_DIR}/cmake_modules")
 #######################
 SET(RTABMAP_MAJOR_VERSION 0)
 SET(RTABMAP_MINOR_VERSION 12)
-SET(RTABMAP_PATCH_VERSION 2)
+SET(RTABMAP_PATCH_VERSION 3)
 SET(RTABMAP_VERSION
   ${RTABMAP_MAJOR_VERSION}.${RTABMAP_MINOR_VERSION}.${RTABMAP_PATCH_VERSION})
   
@@ -43,7 +43,12 @@ ENDIF(${CMAKE_GENERATOR} MATCHES ".*Makefiles")
 
 IF(NOT ANDROID)
     SET(CMAKE_DEBUG_POSTFIX "d")
-ENDIF(NOT ANDROID)
+ELSE()
+	option(DISABLE_LOG        "Disable Android logging (should be true in release)"    ON)
+	IF(DISABLE_LOG)
+	   ADD_DEFINITIONS(-DDISABLE_LOG)
+	ENDIF(DISABLE_LOG)
+ENDIF()
 
 IF(WIN32 AND NOT MINGW)
   	ADD_DEFINITIONS("-DNOMINMAX")

app/android/AndroidManifest.xml.in

@@ -2,7 +2,7 @@
 <!-- BEGIN_INCLUDE(manifest) -->
 <manifest xmlns:android="http://schemas.android.com/apk/res/android"
         package="com.introlab.rtabmap"
-        android:versionCode="44"
+        android:versionCode="45"
         android:versionName="@RTABMAP_VERSION@">
 
     <uses-permission android:name="android.permission.CAMERA" />
@@ -20,7 +20,8 @@
     <!-- This .apk has no Java code itself, so set hasCode to false. -->
     <application 
         android:label="@string/app_name" 
-        android:icon="@drawable/ic_launcher">
+        android:icon="@drawable/ic_launcher"
+        android:debuggable="@ANDROID_DEBUGGABLE@">
         
         <uses-library android:name="com.projecttango.libtango_device2" android:required="true" />
 

app/android/CMakeLists.txt

@@ -6,6 +6,12 @@ add_subdirectory(jni)
 # Packaging
 ######
 
+IF(DISABLE_LOG)
+   SET(ANDROID_DEBUGGABLE false)
+ELSE()
+   SET(ANDROID_DEBUGGABLE true)
+ENDIF()
+
 # find android
 find_host_program(ANDROID_EXECUTABLE
   NAMES android

app/android/jni/ProgressionStatus.h

@@ -27,7 +27,7 @@ public:
 class ProgressionStatus: public ProgressState, public UEventsHandler
 {
 public:
-	ProgressionStatus() : count_(0), max_(100), canceled_(false), jvm_(0), rtabmap_(0)
+	ProgressionStatus() : count_(0), max_(100), jvm_(0), rtabmap_(0)
 	{
 		registerToEventsManager();
 	}
@@ -42,7 +42,7 @@ public:
 	{
 		count_=-1;
 		max_ = max;
-		canceled_ = false;
+		setCanceled(false);
 
 		increment();
 	}
@@ -65,27 +65,17 @@ public:
 
 	virtual bool callback(const std::string & msg) const
 	{
-		if(!canceled_)
+		if(!isCanceled())
 		{
 			increment();
 		}
 
-		return !canceled_;
+		return !isCanceled();
 	}
 	virtual ~ProgressionStatus(){}
 
-	void cancel()
-	{
-		canceled_ = true;
-	}
-
-	bool isCanceled() const
-	{
-		return canceled_;
-	}
-
 protected:
-	virtual void handleEvent(UEvent * event)
+	virtual bool handleEvent(UEvent * event)
 	{
 		if(event->getClassName().compare("ProgressEvent") == 0)
 		{
@@ -118,12 +108,12 @@ protected:
 				UERROR("Failed to call rtabmap::updateProgressionCallback");
 			}
 		}
+		return false;
 	}
 
 private:
 	int count_;
 	int max_;
-	bool canceled_;
 	JavaVM *jvm_;
 	jobject rtabmap_;
 };

app/android/jni/RTABMapApp.h

@@ -136,8 +136,9 @@ class RTABMapApp : public UEventsHandler {
   void setMeshDecimation(int value);
   void setMeshAngleTolerance(float value);
   void setMeshTriangleSize(int value);
-  void setMinClusterSize(int value);
+  void setClusterRatio(float value);
   void setMaxGainRadius(float value);
+  void setRenderingTextureDecimation(int value);
   int setMappingParameter(const std::string & key, const std::string & value);
 
   void resetMapping();
@@ -165,11 +166,14 @@ class RTABMapApp : public UEventsHandler {
   int postProcessing(int approach);
 
  protected:
-  virtual void handleEvent(UEvent * event);
+  virtual bool handleEvent(UEvent * event);
 
  private:
   rtabmap::ParametersMap getRtabmapParameters();
   bool smoothMesh(int id, Mesh & mesh);
+  void gainCompensation(bool full = false);
+  std::vector<pcl::Vertices> filterOrganizedPolygons(const std::vector<pcl::Vertices> & polygons, int cloudSize) const;
+  std::vector<pcl::Vertices> filterPolygons(const std::vector<pcl::Vertices> & polygons, int cloudSize) const;
 
  private:
   rtabmap::CameraTango * camera_;
@@ -191,15 +195,18 @@ class RTABMapApp : public UEventsHandler {
   int meshDecimation_;
   int meshTrianglePix_;
   float meshAngleToleranceDeg_;
-  int minClusterSize_;
+  float clusterRatio_;
   float maxGainRadius_;
+  int renderingTextureDecimation_;
 
   rtabmap::ParametersMap mappingParameters_;
 
   bool paused_;
   bool dataRecorderMode_;
   bool clearSceneOnNextRender_;
-  bool optimizeOpenedDatabase_;
+  bool openingDatabase_;
+  bool exporting_;
+  bool postProcessing_;
   bool filterPolygonsOnNextRender_;
   int gainCompensationOnNextRender_;
   bool bilateralFilteringOnNextRender_;
@@ -208,9 +215,10 @@ class RTABMapApp : public UEventsHandler {
   int totalPolygons_;
   int lastDrawnCloudsCount_;
   float renderingTime_;
-  float previousRenderingTime_;
+  double lastPostRenderEventTime_;
   long processMemoryUsedBytes;
   long processGPUMemoryUsedBytes;
+  std::map<std::string, float> bufferedStatsData_;
 
   bool visualizingMesh_;
   bool exportedMeshUpdated_;
@@ -221,7 +229,7 @@ class RTABMapApp : public UEventsHandler {
   // movement and point cloud.
   Scene main_scene_;
 
-	std::list<rtabmap::Statistics> rtabmapEvents_;
+	std::list<rtabmap::RtabmapEvent*> rtabmapEvents_;
 	std::list<rtabmap::OdometryEvent> odomEvents_;
 	std::list<rtabmap::Transform> poseEvents_;
 

app/android/jni/bounding_box_drawable.h

@@ -0,0 +1,131 @@
+/*
+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 BOUNDING_BOX_DRAWABLE_H_
+#define BOUNDING_BOX_DRAWABLE_H_
+
+#include "tango-gl/line.h"
+
+class BoundingBoxDrawable : public tango_gl::Line
+{
+public:
+	BoundingBoxDrawable() :
+		Line(3.0f, GL_LINES)
+	{
+		vec_vertices_.resize(24);
+	}
+
+	void updateVertices(const pcl::PointXYZ & min, const pcl::PointXYZ & max)
+	{
+		int index = 0;
+		vec_vertices_[index].x = min.x;
+		vec_vertices_[index].y = min.y;
+		vec_vertices_[index++].z = min.z;
+		vec_vertices_[index].x = min.x;
+		vec_vertices_[index].y = max.y;
+		vec_vertices_[index++].z = min.z;
+
+		vec_vertices_[index].x = min.x;
+		vec_vertices_[index].y = min.y;
+		vec_vertices_[index++].z = min.z;
+		vec_vertices_[index].x = max.x;
+		vec_vertices_[index].y = min.y;
+		vec_vertices_[index++].z = min.z;
+
+		vec_vertices_[index].x = min.x;
+		vec_vertices_[index].y = min.y;
+		vec_vertices_[index++].z = min.z;
+		vec_vertices_[index].x = min.x;
+		vec_vertices_[index].y = min.y;
+		vec_vertices_[index++].z = max.z;
+
+		vec_vertices_[index].x = max.x;
+		vec_vertices_[index].y = max.y;
+		vec_vertices_[index++].z = max.z;
+		vec_vertices_[index].x = max.x;
+		vec_vertices_[index].y = min.y;
+		vec_vertices_[index++].z = max.z;
+
+		vec_vertices_[index].x = max.x;
+		vec_vertices_[index].y = max.y;
+		vec_vertices_[index++].z = max.z;
+		vec_vertices_[index].x = min.x;
+		vec_vertices_[index].y = max.y;
+		vec_vertices_[index++].z = max.z;
+
+		vec_vertices_[index].x = max.x;
+		vec_vertices_[index].y = max.y;
+		vec_vertices_[index++].z = max.z;
+		vec_vertices_[index].x = max.x;
+		vec_vertices_[index].y = max.y;
+		vec_vertices_[index++].z = min.z;
+
+		vec_vertices_[index].x = max.x;
+		vec_vertices_[index].y = min.y;
+		vec_vertices_[index++].z = min.z;
+		vec_vertices_[index].x = max.x;
+		vec_vertices_[index].y = min.y;
+		vec_vertices_[index++].z = max.z;
+
+		vec_vertices_[index].x = max.x;
+		vec_vertices_[index].y = min.y;
+		vec_vertices_[index++].z = min.z;
+		vec_vertices_[index].x = max.x;
+		vec_vertices_[index].y = max.y;
+		vec_vertices_[index++].z = min.z;
+
+		vec_vertices_[index].x = min.x;
+		vec_vertices_[index].y = max.y;
+		vec_vertices_[index++].z = min.z;
+		vec_vertices_[index].x = max.x;
+		vec_vertices_[index].y = max.y;
+		vec_vertices_[index++].z = min.z;
+
+		vec_vertices_[index].x = min.x;
+		vec_vertices_[index].y = max.y;
+		vec_vertices_[index++].z = min.z;
+		vec_vertices_[index].x = min.x;
+		vec_vertices_[index].y = max.y;
+		vec_vertices_[index++].z = max.z;
+
+		vec_vertices_[index].x = min.x;
+		vec_vertices_[index].y = min.y;
+		vec_vertices_[index++].z = max.z;
+		vec_vertices_[index].x = max.x;
+		vec_vertices_[index].y = min.y;
+		vec_vertices_[index++].z = max.z;
+
+		vec_vertices_[index].x = min.x;
+		vec_vertices_[index].y = min.y;
+		vec_vertices_[index++].z = max.z;
+		vec_vertices_[index].x = min.x;
+		vec_vertices_[index].y = max.y;
+		vec_vertices_[index++].z = max.z;
+	}
+
+};
+#endif  // TANGO_GL_LINE_H_

app/android/jni/jni_interface.cpp

@@ -259,10 +259,10 @@ Java_com_introlab_rtabmap_RTABMapLib_setMeshTriangleSize(
 	return app.setMeshTriangleSize(value);
 }
 JNIEXPORT void JNICALL
-Java_com_introlab_rtabmap_RTABMapLib_setMinClusterSize(
+Java_com_introlab_rtabmap_RTABMapLib_setClusterRatio(
-		JNIEnv*, jobject, int value)
+		JNIEnv*, jobject, float value)
 {
-	return app.setMinClusterSize(value);
+	return app.setClusterRatio(value);
 }
 JNIEXPORT void JNICALL
 Java_com_introlab_rtabmap_RTABMapLib_setMaxGainRadius(
@@ -270,6 +270,12 @@ Java_com_introlab_rtabmap_RTABMapLib_setMaxGainRadius(
 {
 	return app.setMaxGainRadius(value);
 }
+JNIEXPORT void JNICALL
+Java_com_introlab_rtabmap_RTABMapLib_setRenderingTextureDecimation(
+		JNIEnv*, jobject, int value)
+{
+	return app.setRenderingTextureDecimation(value);
+}
 JNIEXPORT jint JNICALL
 Java_com_introlab_rtabmap_RTABMapLib_setMappingParameter(
     JNIEnv* env, jobject, jstring key, jstring value)

app/android/jni/point_cloud_drawable.cpp

@@ -32,6 +32,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #include "rtabmap/utilite/UConversion.h"
 #include <opencv2/imgproc/imgproc.hpp>
 #include "util.h"
+#include "pcl/common/transforms.h"
 
 #include <GLES2/gl2.h>
 
@@ -47,7 +48,8 @@ PointCloudDrawable::PointCloudDrawable(
 		vertex_buffers_(0),
 		textures_(0),
 		nPoints_(0),
-		pose_(1.0f),
+		pose_(rtabmap::Transform::getIdentity()),
+		poseGl_(1.0f),
 		visible_(true),
 		hasNormals_(false),
 		cloud_shader_program_(cloudShaderProgram),
@@ -64,7 +66,8 @@ PointCloudDrawable::PointCloudDrawable(
 		vertex_buffers_(0),
 		textures_(0),
 		nPoints_(0),
-		pose_(1.0f),
+		pose_(rtabmap::Transform::getIdentity()),
+		poseGl_(1.0f),
 		visible_(true),
 		hasNormals_(false),
 		cloud_shader_program_(cloudShaderProgram),
@@ -138,6 +141,8 @@ void PointCloudDrawable::updateCloud(const pcl::PointCloud<pcl::PointXYZRGB>::Pt
 	gain_ = gain;
 	verticesLowRes_.clear();
 	verticesLowLowRes_.clear();
+	aabbMinModel_ = aabbMinWorld_ = pcl::PointXYZ(1000,1000,1000);
+	aabbMaxModel_ = aabbMaxWorld_ = pcl::PointXYZ(-1000,-1000,-1000);
 
 	if (vertex_buffers_)
 	{
@@ -173,10 +178,13 @@ void PointCloudDrawable::updateCloud(const pcl::PointCloud<pcl::PointXYZRGB>::Pt
 		int oi_lowlow = 0;
 		for(unsigned int i=0; i<indices->size(); ++i)
 		{
-			vertices[i*4] = cloud->at(indices->at(i)).x;
+			const pcl::PointXYZRGB & pt = cloud->at(indices->at(i));
-			vertices[i*4+1] = cloud->at(indices->at(i)).y;
+			vertices[i*4] = pt.x;
-			vertices[i*4+2] = cloud->at(indices->at(i)).z;
+			vertices[i*4+1] = pt.y;
-			vertices[i*4+3] = cloud->at(indices->at(i)).rgb;
+			vertices[i*4+2] = pt.z;
+			vertices[i*4+3] = pt.rgb;
+
+			updateAABBMinMax(pt, aabbMinModel_, aabbMaxModel_);
 
 			if(cloud->isOrganized())
 			{
@@ -203,10 +211,13 @@ void PointCloudDrawable::updateCloud(const pcl::PointCloud<pcl::PointXYZRGB>::Pt
 		int oi_lowlow = 0;
 		for(unsigned int i=0; i<cloud->size(); ++i)
 		{
-			vertices[i*4] = cloud->at(i).x;
+			const pcl::PointXYZRGB & pt = cloud->at(i);
-			vertices[i*4+1] = cloud->at(i).y;
+			vertices[i*4] = pt.x;
-			vertices[i*4+2] = cloud->at(i).z;
+			vertices[i*4+1] = pt.y;
-			vertices[i*4+3] = cloud->at(i).rgb;
+			vertices[i*4+2] = pt.z;
+			vertices[i*4+3] = pt.rgb;
+
+			updateAABBMinMax(pt, aabbMinModel_, aabbMaxModel_);
 
 			if(cloud->isOrganized())
 			{
@@ -243,6 +254,8 @@ void PointCloudDrawable::updateMesh(const Mesh & mesh)
 {
 	UASSERT(mesh.cloud.get() && !mesh.cloud->empty());
 	nPoints_ = 0;
+	aabbMinModel_ = aabbMinWorld_ = pcl::PointXYZ(1000,1000,1000);
+	aabbMaxModel_ = aabbMaxWorld_ = pcl::PointXYZ(-1000,-1000,-1000);
 
 	if (vertex_buffers_)
 	{
@@ -306,12 +319,15 @@ void PointCloudDrawable::updateMesh(const Mesh & mesh)
 			vertices = std::vector<float>(mesh.indices->size()*9);
 			for(unsigned int i=0; i<mesh.indices->size(); ++i)
 			{
-				vertices[i*items] = mesh.cloud->at(mesh.indices->at(i)).x;
+				const pcl::PointXYZRGB & pt = mesh.cloud->at(mesh.indices->at(i));
-				vertices[i*items+1] = mesh.cloud->at(mesh.indices->at(i)).y;
+				vertices[i*items] = pt.x;
-				vertices[i*items+2] = mesh.cloud->at(mesh.indices->at(i)).z;
+				vertices[i*items+1] = pt.y;
+				vertices[i*items+2] = pt.z;
 
 				// rgb
-				vertices[i*items+3] = mesh.cloud->at(mesh.indices->at(i)).rgb;
+				vertices[i*items+3] = pt.rgb;
+
+				updateAABBMinMax(pt, aabbMinModel_, aabbMaxModel_);
 
 				// texture uv
 				int index = mesh.indices->at(i);
@@ -345,10 +361,13 @@ void PointCloudDrawable::updateMesh(const Mesh & mesh)
 			vertices = std::vector<float>(mesh.indices->size()*items);
 			for(unsigned int i=0; i<mesh.indices->size(); ++i)
 			{
-				vertices[i*items] = mesh.cloud->at(mesh.indices->at(i)).x;
+				const pcl::PointXYZRGB & pt = mesh.cloud->at(mesh.indices->at(i));
-				vertices[i*items+1] = mesh.cloud->at(mesh.indices->at(i)).y;
+				vertices[i*items] = pt.x;
-				vertices[i*items+2] = mesh.cloud->at(mesh.indices->at(i)).z;
+				vertices[i*items+1] = pt.y;
-				vertices[i*items+3] = mesh.cloud->at(mesh.indices->at(i)).rgb;
+				vertices[i*items+2] = pt.z;
+				vertices[i*items+3] = pt.rgb;
+
+				updateAABBMinMax(pt, aabbMinModel_, aabbMaxModel_);
 
 				if(hasNormals_)
 				{
@@ -401,12 +420,16 @@ void PointCloudDrawable::updateMesh(const Mesh & mesh)
 					UASSERT(oi < mesh.texCoords.size());
 					UASSERT(v.vertices[j] < mesh.cloud->size());
 
-					vertices[oi*items] = mesh.cloud->at(v.vertices[j]).x;
+					const pcl::PointXYZRGB & pt =  mesh.cloud->at(v.vertices[j]);
-					vertices[oi*items+1] = mesh.cloud->at(v.vertices[j]).y;
+
-					vertices[oi*items+2] = mesh.cloud->at(v.vertices[j]).z;
+					vertices[oi*items] = pt.x;
+					vertices[oi*items+1] = pt.y;
+					vertices[oi*items+2] = pt.z;
 
 					// rgb
-					vertices[oi*items+3] = mesh.cloud->at(v.vertices[j]).rgb;
+					vertices[oi*items+3] = pt.rgb;
+
+					updateAABBMinMax(pt, aabbMinModel_, aabbMaxModel_);
 
 					// texture uv
 					if(mesh.texCoords[oi][0]>=0.0f)
@@ -444,10 +467,14 @@ void PointCloudDrawable::updateMesh(const Mesh & mesh)
 			vertices = std::vector<float>(mesh.cloud->size()*items);
 			for(unsigned int i=0; i<mesh.cloud->size(); ++i)
 			{
-				vertices[i*items] = mesh.cloud->at(i).x;
+				const pcl::PointXYZRGB & pt =  mesh.cloud->at(i);
-				vertices[i*items+1] = mesh.cloud->at(i).y;
+
-				vertices[i*items+2] = mesh.cloud->at(i).z;
+				vertices[i*items] =pt.x;
-				vertices[i*items+3] = mesh.cloud->at(i).rgb;
+				vertices[i*items+1] = pt.y;
+				vertices[i*items+2] = pt.z;
+				vertices[i*items+3] = pt.rgb;
+
+				updateAABBMinMax(pt, aabbMinModel_, aabbMaxModel_);
 
 				if(hasNormals_)
 				{
@@ -513,15 +540,51 @@ void PointCloudDrawable::updateMesh(const Mesh & mesh)
 	{
 		updatePolygons(polygons, polygonsLowRes);
 	}
+
+	if(!pose_.isNull())
+	{
+		updateAABBWorld(pose_);
+	}
 }
 
 void PointCloudDrawable::setPose(const rtabmap::Transform & pose)
 {
 	UASSERT(!pose.isNull());
 
-	pose_ = glmFromTransform(pose);
+	if(pose_ != pose)
+	{
+		updateAABBWorld(pose);
+	}
+
+	pose_ = pose;
+	poseGl_ = glmFromTransform(pose);
 }
 
+void PointCloudDrawable::updateAABBWorld(const rtabmap::Transform & pose)
+{
+	pcl::PointCloud<pcl::PointXYZ> corners;
+	corners.resize(8);
+	corners.at(0) = pcl::PointXYZ(aabbMinModel_.x, aabbMinModel_.y, aabbMinModel_.z);
+	corners.at(1) = pcl::PointXYZ(aabbMinModel_.x, aabbMinModel_.y, aabbMaxModel_.z);
+	corners.at(2) = pcl::PointXYZ(aabbMinModel_.x, aabbMaxModel_.y, aabbMinModel_.z);
+	corners.at(3) = pcl::PointXYZ(aabbMaxModel_.x, aabbMinModel_.y, aabbMinModel_.z);
+	corners.at(4) = pcl::PointXYZ(aabbMaxModel_.x, aabbMaxModel_.y, aabbMaxModel_.z);
+	corners.at(5) = pcl::PointXYZ(aabbMaxModel_.x, aabbMaxModel_.y, aabbMinModel_.z);
+	corners.at(6) = pcl::PointXYZ(aabbMaxModel_.x, aabbMinModel_.y, aabbMaxModel_.z);
+	corners.at(7) = pcl::PointXYZ(aabbMinModel_.x, aabbMaxModel_.y, aabbMaxModel_.z);
+
+	pcl::PointCloud<pcl::PointXYZ> cornersTransformed;
+	pcl::transformPointCloud(corners, cornersTransformed, pose.toEigen3f());
+
+	aabbMinWorld_ = pcl::PointXYZ(1000,1000,1000);
+	aabbMaxWorld_ = pcl::PointXYZ(-1000,-1000,-1000);
+	for(unsigned int i=0; i<cornersTransformed.size(); ++i)
+	{
+		updateAABBMinMax(cornersTransformed.at(i), aabbMinWorld_, aabbMaxWorld_);
+	}
+}
+
+
 void PointCloudDrawable::Render(const glm::mat4 & projectionMatrix,
 		const glm::mat4 & viewMatrix,
 		bool meshRendering,
@@ -537,7 +600,7 @@ void PointCloudDrawable::Render(const glm::mat4 & projectionMatrix,
 			glUseProgram(texture_shader_program_);
 
 			GLuint mvp_handle = glGetUniformLocation(texture_shader_program_, "uMVP");
-			glm::mat4 mv_mat = viewMatrix * pose_;
+			glm::mat4 mv_mat = viewMatrix * poseGl_;
 			glm::mat4 mvp_mat = projectionMatrix * mv_mat;
 			glUniformMatrix4fv(mvp_handle, 1, GL_FALSE, glm::value_ptr(mvp_mat));
 
@@ -611,7 +674,7 @@ void PointCloudDrawable::Render(const glm::mat4 & projectionMatrix,
 			glUseProgram(cloud_shader_program_);
 
 			GLuint mvp_handle_ = glGetUniformLocation(cloud_shader_program_, "uMVP");
-			glm::mat4 mv_mat = viewMatrix * pose_;
+			glm::mat4 mv_mat = viewMatrix * poseGl_;
 			glm::mat4 mvp_mat = projectionMatrix * mv_mat;
 			glUniformMatrix4fv(mvp_handle_, 1, GL_FALSE, glm::value_ptr(mvp_mat));
 

app/android/jni/point_cloud_drawable.h

@@ -59,10 +59,15 @@ class PointCloudDrawable {
   void setPose(const rtabmap::Transform & pose);
   void setVisible(bool visible) {visible_=visible;}
   void setGain(float gain) {gain_ = gain;}
-  rtabmap::Transform getPose() const {return glmToTransform(pose_);}
+  rtabmap::Transform getPose() const {return pose_;}
+  const glm::mat4 & getPoseGl() const {return poseGl_;}
   bool isVisible() const {return visible_;}
   bool hasMesh() const {return polygons_.size()!=0;}
   bool hasTexture() const {return textures_ != 0;}
+  const pcl::PointXYZ & aabbMinModel() const {return aabbMinModel_;}
+  const pcl::PointXYZ & aabbMaxModel() const {return aabbMaxModel_;}
+  const pcl::PointXYZ & aabbMinWorld() const {return aabbMinWorld_;}
+  const pcl::PointXYZ & aabbMaxWorld() const {return aabbMaxWorld_;}
 
   // Update current point cloud data.
   //
@@ -78,6 +83,19 @@ class PointCloudDrawable {
 		  bool lighting = true,
 		  float distanceToCamSqr = 0.0f);
 
+ private:
+  template<class PointT>
+  void updateAABBMinMax(const PointT & pt, pcl::PointXYZ & min, pcl::PointXYZ & max)
+  {
+	  if(pt.x<min.x) min.x = pt.x;
+	  if(pt.y<min.y) min.y = pt.y;
+	  if(pt.z<min.z) min.z = pt.z;
+	  if(pt.x>max.x) max.x = pt.x;
+	  if(pt.y>max.y) max.y = pt.y;
+	  if(pt.z>max.z) max.z = pt.z;
+  }
+  void updateAABBWorld(const rtabmap::Transform & pose);
+
  private:
   // Vertex buffer of the point cloud geometry.
   GLuint vertex_buffers_;
@@ -87,7 +105,8 @@ class PointCloudDrawable {
   std::vector<GLuint> verticesLowRes_;
   std::vector<GLuint> verticesLowLowRes_;
   int nPoints_;
-  glm::mat4 pose_;
+  rtabmap::Transform pose_;
+  glm::mat4 poseGl_;
   bool visible_;
   bool hasNormals_;
   std::vector<unsigned int> organizedToDenseIndices_;
@@ -96,6 +115,11 @@ class PointCloudDrawable {
   GLuint texture_shader_program_;
 
   float gain_;
+
+  pcl::PointXYZ aabbMinModel_;
+  pcl::PointXYZ aabbMaxModel_;
+  pcl::PointXYZ aabbMinWorld_;
+  pcl::PointXYZ aabbMaxWorld_;
 };
 
 #endif  // TANGO_POINT_CLOUD_POINT_CLOUD_DRAWABLE_H_

app/android/jni/scene.cpp

@@ -20,6 +20,7 @@
 #include <rtabmap/utilite/ULogger.h>
 #include <rtabmap/utilite/UStl.h>
 #include <rtabmap/core/util3d_filtering.h>
+#include <pcl/common/transforms.h>
 
 #include <glm/gtx/transform.hpp>
 
@@ -153,6 +154,7 @@ Scene::Scene() :
 		axis_(0),
 		frustum_(0),
 		grid_(0),
+		box_(0),
 		trace_(0),
 		graph_(0),
 		graphVisible_(true),
@@ -168,6 +170,7 @@ Scene::Scene() :
 		meshRenderingTexture_(true),
 		pointSize_(5.0f),
 		frustumCulling_(true),
+		boundingBoxRendering_(false),
 		lighting_(true),
 		backfaceCulling_(true),
 		r_(0.0f),
@@ -199,6 +202,7 @@ void Scene::InitGLContent()
   frustum_ = new tango_gl::Frustum();
   trace_ = new tango_gl::Trace();
   grid_ = new tango_gl::Grid();
+  box_ = new BoundingBoxDrawable();
   currentPose_ = new rtabmap::Transform();
 
 
@@ -208,6 +212,8 @@ void Scene::InitGLContent()
   trace_->SetColor(kTraceColor);
   grid_->SetColor(kGridColor);
   grid_->SetPosition(-kHeightOffset);
+  box_->SetShader();
+  box_->SetColor(1,0,0);
 
   if(cloud_shader_program_ == 0)
   {
@@ -238,6 +244,7 @@ void Scene::DeleteResources() {
 	  delete trace_;
 	  delete grid_;
 	  delete currentPose_;
+	  delete box_;
 	}
 
 	if (cloud_shader_program_) {
@@ -287,6 +294,113 @@ void Scene::SetupViewPort(int w, int h) {
   glViewport(0, 0, w, h);
 }
 
+std::vector<glm::vec4> computeFrustumPlanes(const glm::mat4 & mat, bool normalize = true)
+{
+	// http://www.txutxi.com/?p=444
+	std::vector<glm::vec4> planes(6);
+
+	// Left Plane
+	// col4 + col1
+	planes[0].x = mat[0][3] + mat[0][0];
+	planes[0].y = mat[1][3] + mat[1][0];
+	planes[0].z = mat[2][3] + mat[2][0];
+	planes[0].w = mat[3][3] + mat[3][0];
+
+	// Right Plane
+	// col4 - col1
+	planes[1].x = mat[0][3] - mat[0][0];
+	planes[1].y = mat[1][3] - mat[1][0];
+	planes[1].z = mat[2][3] - mat[2][0];
+	planes[1].w = mat[3][3] - mat[3][0];
+
+	// Bottom Plane
+	// col4 + col2
+	planes[2].x = mat[0][3] + mat[0][1];
+	planes[2].y = mat[1][3] + mat[1][1];
+	planes[2].z = mat[2][3] + mat[2][1];
+	planes[2].w = mat[3][3] + mat[3][1];
+
+	// Top Plane
+	// col4 - col2
+	planes[3].x = mat[0][3] - mat[0][1];
+	planes[3].y = mat[1][3] - mat[1][1];
+	planes[3].z = mat[2][3] - mat[2][1];
+	planes[3].w = mat[3][3] - mat[3][1];
+
+	// Near Plane
+	// col4 + col3
+	planes[4].x = mat[0][3] + mat[0][2];
+	planes[4].y = mat[1][3] + mat[1][2];
+	planes[4].z = mat[2][3] + mat[2][2];
+	planes[4].w = mat[3][3] + mat[3][2];
+
+	// Far Plane
+	// col4 - col3
+	planes[5].x = mat[0][3] - mat[0][2];
+	planes[5].y = mat[1][3] - mat[1][2];
+	planes[5].z = mat[2][3] - mat[2][2];
+	planes[5].w = mat[3][3] - mat[3][2];
+
+	//if(normalize)
+	{
+		for(unsigned int i=0;i<planes.size(); ++i)
+		{
+			if(normalize)
+			{
+				float d = std::sqrt(planes[i].x * planes[i].x + planes[i].y * planes[i].y + planes[i].z * planes[i].z); // for normalizing the coordinates
+				planes[i].x/=d;
+				planes[i].y/=d;
+				planes[i].z/=d;
+				planes[i].w/=d;
+			}
+		}
+	}
+
+	return planes;
+}
+
+/**
+ * Tells whether or not b is intersecting f.
+ * http://www.txutxi.com/?p=584
+ * @param f Viewing frustum.
+ * @param b An axis aligned bounding box.
+ * @return True if b intersects f, false otherwise.
+ */
+bool intersectFrustumAABB(
+        const std::vector<glm::vec4> &planes,
+        const pcl::PointXYZ &boxMin,
+		const pcl::PointXYZ &boxMax)
+{
+  // Indexed for the 'index trick' later
+	const pcl::PointXYZ * box[] = {&boxMin, &boxMax};
+
+  // We only need to do 6 point-plane tests
+  for (unsigned int i = 0; i < planes.size(); ++i)
+  {
+    // This is the current plane
+    const glm::vec4 &p = planes[i];
+
+    // p-vertex selection (with the index trick)
+    // According to the plane normal we can know the
+    // indices of the positive vertex
+    const int px = p.x > 0.0f?1:0;
+    const int py = p.y > 0.0f?1:0;
+    const int pz = p.z > 0.0f?1:0;
+
+    // Dot product
+    // project p-vertex on plane normal
+    // (How far is p-vertex from the origin)
+    const float dp =
+        (p.x*box[px]->x) +
+        (p.y*box[py]->y) +
+        (p.z*box[pz]->z) + p.w;
+
+    // Doesn't intersect if it is behind the plane
+    if (dp < 0) {return false; }
+  }
+  return true;
+}
+
 //Should only be called in OpenGL thread!
 int Scene::Render() {
 	UASSERT(gesture_camera_ != 0);
@@ -353,7 +467,7 @@ int Scene::Render() {
 	}
 
 	float fov = 45.0f;
-	rtabmap::Transform openglCamera = GetOpenGLCameraPose(&fov)*rtabmap::Transform(0.0f, 0.0f, 3.0f, 0.0f, 0.0f, 0.0f);
+	rtabmap::Transform openglCamera = GetOpenGLCameraPose(&fov);//*rtabmap::Transform(0.0f, 0.0f, 3.0f, 0.0f, 0.0f, 0.0f);
 	// transform in same coordinate as frustum filtering
 	openglCamera *= rtabmap::Transform(
 		 0.0f,  0.0f,  1.0f, 0.0f,
@@ -363,50 +477,30 @@ int Scene::Render() {
 	int cloudDrawn=0;
 	if(mapRendering_ && frustumCulling_)
 	{
-		//Use camera frustum to cull nodes that don't need to be drawn
+		std::vector<glm::vec4> planes = computeFrustumPlanes(gesture_camera_->GetProjectionMatrix()*gesture_camera_->GetViewMatrix(), true);
-		pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>);
-		std::vector<int> ids(pointClouds_.size());
-
-		cloud->resize(pointClouds_.size());
-		ids.resize(pointClouds_.size());
-		int oi=0;
 		for(std::map<int, PointCloudDrawable*>::const_iterator iter=pointClouds_.begin(); iter!=pointClouds_.end(); ++iter)
 		{
-			if(!iter->second->getPose().isNull() && iter->second->isVisible())
+			if(iter->second->isVisible())
 			{
-				(*cloud)[oi] = pcl::PointXYZ(iter->second->getPose().x(), iter->second->getPose().y(), iter->second->getPose().z());
+				if(intersectFrustumAABB(planes,
-				ids[oi++] = iter->first;
+						iter->second->aabbMinWorld(),
-			}
+						iter->second->aabbMaxWorld()))
-		}
+				{
-		cloud->resize(oi);
+					if(boundingBoxRendering_)
-		ids.resize(oi);
+					{
+						box_->updateVertices(iter->second->aabbMinWorld(), iter->second->aabbMaxWorld());
+						box_->Render(gesture_camera_->GetProjectionMatrix(),
+											gesture_camera_->GetViewMatrix());
+					}
 
-		if(oi)
+					++cloudDrawn;
-		{
+					Eigen::Vector3f cloudToCamera(
-			pcl::IndicesPtr indices = rtabmap::util3d::frustumFiltering(
+							iter->second->getPose().x() - openglCamera.x(),
-					cloud,
+							iter->second->getPose().y() - openglCamera.y(),
-					pcl::IndicesPtr(new std::vector<int>),
+							iter->second->getPose().z() - openglCamera.z());
-					openglCamera,
+					float distanceToCameraSqr = cloudToCamera[0]*cloudToCamera[0] + cloudToCamera[1]*cloudToCamera[1] + cloudToCamera[2]*cloudToCamera[2];
-					fov*2.0f,
+					iter->second->Render(gesture_camera_->GetProjectionMatrix(), gesture_camera_->GetViewMatrix(), meshRendering_, pointSize_, meshRenderingTexture_, lighting_, distanceToCameraSqr);
-					fov*2.0f,
+				}
-					0.1f,
-					100.0f);
-
-			//LOGI("Frustum poses filtered = %d (showing %d/%d)",
-			//			(int)(pointClouds_.size()-indices->size()),
-			//			(int)indices->size(),
-			//			(int)pointClouds_.size());
-
-			for(unsigned int i=0; i<indices->size(); ++i)
-			{
-				++cloudDrawn;
-				std::map<int, PointCloudDrawable*>::const_iterator iter = pointClouds_.find(ids[indices->at(i)]);
-				Eigen::Vector3f cloudToCamera(
-						iter->second->getPose().x() - openglCamera.x(),
-						iter->second->getPose().y() - openglCamera.y(),
-						iter->second->getPose().z() - openglCamera.z());
-				float distanceToCameraSqr = cloudToCamera[0]*cloudToCamera[0] + cloudToCamera[1]*cloudToCamera[1] + cloudToCamera[2]*cloudToCamera[2];
-				iter->second->Render(gesture_camera_->GetProjectionMatrix(), gesture_camera_->GetViewMatrix(), meshRendering_, pointSize_, meshRenderingTexture_, lighting_, distanceToCameraSqr);
 			}
 		}
 	}
@@ -414,8 +508,20 @@ int Scene::Render() {
 	{
 		for(std::map<int, PointCloudDrawable*>::const_iterator iter=pointClouds_.begin(); iter!=pointClouds_.end(); ++iter)
 		{
-			if((mapRendering_ || iter->first < 0) && iter->second->isVisible())
+			if(!mapRendering_ && iter->first > 0)
 			{
+				break;
+			}
+
+			if(iter->second->isVisible())
+			{
+				if(boundingBoxRendering_)
+				{
+					box_->updateVertices(iter->second->aabbMinWorld(), iter->second->aabbMaxWorld());
+					box_->Render(gesture_camera_->GetProjectionMatrix(),
+										gesture_camera_->GetViewMatrix());
+				}
+
 				++cloudDrawn;
 				Eigen::Vector3f cloudToCamera(
 						iter->second->getPose().x() - openglCamera.x(),
@@ -475,8 +581,11 @@ void Scene::updateGraph(
 	}
 
 	//create
-	UASSERT(graph_shader_program_ != 0);
+	if(graphVisible_)
-	graph_ = new GraphDrawable(graph_shader_program_, poses, links);
+	{
+		UASSERT(graph_shader_program_ != 0);
+		graph_ = new GraphDrawable(graph_shader_program_, poses, links);
+	}
 }
 
 void Scene::setGraphVisible(bool visible)

app/android/jni/scene.h

@@ -37,6 +37,7 @@
 
 #include <point_cloud_drawable.h>
 #include <graph_drawable.h>
+#include <bounding_box_drawable.h>
 
 #include <pcl/point_cloud.h>
 #include <pcl/point_types.h>
@@ -127,6 +128,7 @@ class Scene {
   void setBackfaceCulling(bool enabled) {backfaceCulling_ = enabled;}
   void setBackgroundColor(float r, float g, float b) {r_=r; g_=g; b_=b;} // 0.0f <> 1.0f
 
+  bool isMapRendering() const {return mapRendering_;}
   bool isMeshRendering() const {return meshRendering_;}
   bool isMeshTexturing() const {return meshRendering_ && meshRenderingTexture_;}
   float getPointSize() const {return pointSize_;}
@@ -147,6 +149,9 @@ class Scene {
   // Ground grid.
   tango_gl::Grid* grid_;
 
+  // Bounding box
+  BoundingBoxDrawable * box_;
+
   // Trace of pose data.
   tango_gl::Trace* trace_;
   GraphDrawable * graph_;
@@ -170,6 +175,7 @@ class Scene {
   bool meshRenderingTexture_;
   float pointSize_;
   bool frustumCulling_;
+  bool boundingBoxRendering_;
   bool lighting_;
   bool backfaceCulling_;
   float r_;

app/android/jni/tango-gl/include/tango-gl/util.h

@@ -34,10 +34,17 @@
 #include "glm/gtx/matrix_decompose.hpp"
 
 #define LOG_TAG "rtabmap"
+#ifdef DISABLE_LOG
+#define LOGD(...) ;
+#define LOGI(...) ;
+#define LOGW(...) ;
+#define LOGE(...) ;
+#else
 #define LOGD(...) __android_log_print(ANDROID_LOG_DEBUG,LOG_TAG,__VA_ARGS__)
 #define LOGI(...) __android_log_print(ANDROID_LOG_INFO,LOG_TAG,__VA_ARGS__)
 #define LOGW(...) __android_log_print(ANDROID_LOG_WARN,LOG_TAG,__VA_ARGS__)
 #define LOGE(...) __android_log_print(ANDROID_LOG_ERROR,LOG_TAG,__VA_ARGS__)
+#endif
 
 #ifndef M_PI
 #define M_PI 3.1415926f

app/android/jni/util.h

@@ -51,7 +51,7 @@ public:
 		registerToEventsManager();
 	}
 protected:
-	virtual void handleEvent(UEvent * event)
+	virtual bool handleEvent(UEvent * event)
 	{
 		if(event->getClassName().compare("ULogEvent") == 0)
 		{
@@ -74,6 +74,7 @@ protected:
 			}
 
 		}
+		return false;
 	}
 };
 

app/android/res/layout/activity_settings.xml

@@ -37,6 +37,13 @@
    			android:entries="@array/pref_triangle_keys"
    			android:entryValues="@array/pref_triangle_values"
    			android:defaultValue="@string/pref_default_triangle"/>
+        <ListPreference 
+            android:key="@string/pref_key_rendering_texture_decimation"
+    		android:title="@string/pref_title_rendering_texture_decimation"
+    		android:summary="@string/pref_summary_rendering_texture_decimation"
+   			android:entries="@array/pref_rendering_texture_decimation_keys"
+   			android:entryValues="@array/pref_rendering_texture_decimation_values"
+   			android:defaultValue="@string/pref_default_rendering_texture_decimation"/>
         <SwitchPreference
             android:key="@string/pref_key_nodes_filtering"
             android:title="@string/pref_title_nodes_filtering"
@@ -269,12 +276,12 @@
    			android:entryValues="@array/pref_gain_max_radius_values"
    			android:defaultValue="@string/pref_default_gain_max_radius"/>
         <ListPreference 
-            android:key="@string/pref_key_min_cluster_size"
+            android:key="@string/pref_key_cluster_ratio"
-    		android:title="@string/pref_title_min_cluster_size"
+    		android:title="@string/pref_title_cluster_ratio"
-    		android:summary="@string/pref_summary_min_cluster_size"
+    		android:summary="@string/pref_summary_cluster_ratio"
-   			android:entries="@array/pref_min_cluster_size_values"
+   			android:entries="@array/pref_cluster_ratio_keys"
-   			android:entryValues="@array/pref_min_cluster_size_values"
+   			android:entryValues="@array/pref_cluster_ratio_values"
-   			android:defaultValue="@string/pref_default_min_cluster_size"/>
+   			android:defaultValue="@string/pref_default_cluster_ratio"/>
         
 	    <Preference android:title="@string/pref_title_reset_button"
 	                android:key="@string/pref_key_reset_button"/>

app/android/res/values/strings.xml

@@ -38,7 +38,7 @@
 
     <!-- Preference keys: BEGIN -->
     <string name="pref_key_tags">pref_key_tags</string>
-    <string name="pref_default_tags">rtabmap 3dscan</string>
+    <string name="pref_default_tags">rtabmap 3dscan tango</string>
     <string name="pref_key_rendering">pref_key_rendering</string>
     <string name="pref_default_rendering">2</string>
     <string name="pref_key_reset_button">pref_key_reset_button</string>
@@ -49,9 +49,11 @@
     <string name="pref_key_point_size">pref_key_point_size</string>
     <string name="pref_default_point_size">5</string>
     <string name="pref_key_angle">pref_key_angle</string>
-    <string name="pref_default_angle">15</string>
+    <string name="pref_default_angle">20</string>
     <string name="pref_key_triangle">pref_key_triangle</string>
     <string name="pref_default_triangle">2</string>
+    <string name="pref_key_rendering_texture_decimation">pref_key_rendering_texture_decimation</string>
+    <string name="pref_default_rendering_texture_decimation">4</string>
     <string name="pref_key_nodes_filtering">pref_key_nodes_filtering</string>
     <string name="pref_default_nodes_filtering">false</string>
     <string name="pref_key_append">pref_key_append</string>
@@ -114,17 +116,19 @@
     <string name="pref_default_opt_clean_white">true</string>
     <string name="pref_key_gain_max_radius">pref_key_gain_max_radius</string>
     <string name="pref_default_gain_max_radius">0.02</string>
-    <string name="pref_key_min_cluster_size">pref_key_min_cluster_size</string>
+    <string name="pref_key_cluster_ratio">pref_key_cluster_ratio</string>
-    <string name="pref_default_min_cluster_size">200</string>
+    <string name="pref_default_cluster_ratio">0.05</string>
     <!-- Preference keys: END -->
 
     <string name="pref_title_rendering">Rendering</string>
     <string name="pref_title_density">Point Cloud Density</string>
     <string name="pref_summary_density">Decrease density to reduce rendering time and memory. Tip: To apply a different density to current map: save the map, change density and re-open the same map to regenerate the point clouds at this density.</string>
     <string name="pref_title_angle">Mesh Angle Tolerance</string>
-    <string name="pref_summary_angle">Minimum polygon angle.</string>
+    <string name="pref_summary_angle">Minimum polygon angle. Increase to force scanning perpendicular to surfaces.</string>
     <string name="pref_title_triangle">Mesh Triangle Size</string>
     <string name="pref_summary_triangle">Size in pixels of the polygons created from the depth image.</string>
+    <string name="pref_title_rendering_texture_decimation">Texture Resolution</string>
+    <string name="pref_summary_rendering_texture_decimation">Resolution of the texture for online rendering. This doesn\'t affect Export results.</string>
     <string name="pref_title_depth">Max Depth</string>
     <string name="pref_summary_depth">Points over the maximum depth are not rendered.</string>
     <string name="pref_title_point_size">Point Size</string>
@@ -178,6 +182,9 @@
         <item>"1"</item>
     </string-array>
     <string-array name="pref_angle_keys">
+        <item>"60 deg"</item>
+        <item>"45 deg"</item>
+        <item>"35 deg"</item>
         <item>"30 deg"</item>
         <item>"25 deg"</item>
         <item>"20 deg"</item>
@@ -186,6 +193,9 @@
         <item>"5 deg"</item>
     </string-array>
     <string-array name="pref_angle_values">
+        <item>"60"</item>
+        <item>"45"</item>
+        <item>"35"</item>
         <item>"30"</item>
         <item>"25"</item>
         <item>"20"</item>
@@ -207,6 +217,18 @@
         <item>"3"</item>
         <item>"2"</item>
     </string-array>
+    <string-array name="pref_rendering_texture_decimation_keys">
+        <item>"Maximum"</item>
+        <item>"High"</item>
+        <item>"Low"</item>
+        <item>"Very Low"</item>
+    </string-array>
+    <string-array name="pref_rendering_texture_decimation_values">
+        <item>"1"</item>
+        <item>"2"</item>
+        <item>"4"</item>
+        <item>"8"</item>
+    </string-array>
 
     <string name="pref_title_mapping_sub">Mapping&#8230;</string>
     <string name="pref_title_mapping">Mapping</string>
@@ -614,8 +636,8 @@
     <string name="pref_title_general">General</string>
     <string name="pref_title_gain_max_radius">Color Correction Radius</string>
     <string name="pref_summary_gain_max_radius">Radius used to find pixel correspondences for Adjust Colors optimization.</string>
-    <string name="pref_title_min_cluster_size">Min Cluster Size</string>
+    <string name="pref_title_cluster_ratio">Noise Filtering Ratio</string>
-    <string name="pref_summary_min_cluster_size">Minimum number of polygons for a cluster to be kept after Noise Filtering optimization.</string>
+    <string name="pref_summary_cluster_ratio">Polygon clusters with size smaller than this ratio of the largest cluster are removed by the Noise Filtering optimization.</string>
 
     <string-array name="pref_gain_max_radius_keys">
         <item>"0.3 m"</item>
@@ -633,12 +655,33 @@
         <item>"0.02"</item>
         <item>"0.01"</item>
     </string-array>
-    <string-array name="pref_min_cluster_size_values">
+    <string-array name="pref_cluster_ratio_keys">
-        <item>"500"</item>
+        <item>"Keep Largest Only"</item>
-        <item>"200"</item>
+        <item>"0.9"</item>
-        <item>"100"</item>
+        <item>"0.8"</item>
-        <item>"50"</item>
+        <item>"0.7"</item>
-        <item>"10"</item>
+        <item>"0.6"</item>
+        <item>"0.5"</item>
+        <item>"0.4"</item>
+        <item>"0.3"</item>
+        <item>"0.2"</item>
+        <item>"0.1"</item>
+        <item>"0.05"</item>
+        <item>"0.01"</item>
+    </string-array>
+    <string-array name="pref_cluster_ratio_values">
+        <item>"1.0"</item>
+        <item>"0.9"</item>
+        <item>"0.8"</item>
+        <item>"0.7"</item>
+        <item>"0.6"</item>
+        <item>"0.5"</item>
+        <item>"0.4"</item>
+        <item>"0.3"</item>
+        <item>"0.2"</item>
+        <item>"0.1"</item>
+        <item>"0.05"</item>
+        <item>"0.01"</item>
     </string-array>
 
     <string name="pref_title_reset_button">Restore All Default Settings</string>

app/android/src/com/introlab/rtabmap/RTABMapActivity.java

@@ -33,6 +33,7 @@ import android.content.DialogInterface;
 import android.content.Intent;
 import android.content.ServiceConnection;
 import android.content.SharedPreferences;
+import android.content.pm.ApplicationInfo;
 import android.content.pm.PackageInfo;
 import android.content.pm.PackageManager;
 import android.content.pm.PackageManager.NameNotFoundException;
@@ -93,6 +94,7 @@ public class RTABMapActivity extends Activity implements OnClickListener {
 
 	// Tag for debug logging.
 	public static final String TAG = RTABMapActivity.class.getSimpleName();
+	public static boolean DISABLE_LOG = true;
 
 	// The minimum Tango Core version required from this application.
 	private static final int  MIN_TANGO_CORE_VERSION = 9377;
@@ -180,12 +182,12 @@ public class RTABMapActivity extends Activity implements OnClickListener {
 	private int mTotalLoopClosures = 0;
 	private boolean mMapIsEmpty = false;
 	private boolean mExportedOBJ = false;
+	int mMapNodes = 0;
 
 	private Toast mToast = null;
 	
 	private AlertDialog mMemoryWarningDialog = null;
 	
-	private Thread mMemStatusUpdateThread = null;
 	private String[] mStatusTexts = new String[16];
 
 	//Tango Service connection.
@@ -347,6 +349,8 @@ public class RTABMapActivity extends Activity implements OnClickListener {
                 public void onDisplayRemoved(int displayId) {}
             }, null);
         }
+        
+       	DISABLE_LOG =  !( 0 != ( getApplicationInfo().flags & ApplicationInfo.FLAG_DEBUGGABLE ) );
 	}
 
 	@Override
@@ -386,7 +390,7 @@ public class RTABMapActivity extends Activity implements OnClickListener {
 	protected void onPause() {
 		super.onPause();
 		
-		Log.i(TAG, "onPause()");
+		if(!DISABLE_LOG) Log.i(TAG, "onPause()");
 		mOnPause = true;
 		
 		// This deletes OpenGL context!
@@ -434,7 +438,7 @@ public class RTABMapActivity extends Activity implements OnClickListener {
 		// update preferences
 		try
 		{
-			Log.d(TAG, "update preferences...");
+			if(!DISABLE_LOG) Log.d(TAG, "update preferences...");
 			SharedPreferences sharedPref = PreferenceManager.getDefaultSharedPreferences(this);
 			mUpdateRate = sharedPref.getString(getString(R.string.pref_key_update_rate), getString(R.string.pref_default_update_rate));
 			mTimeThr = sharedPref.getString(getString(R.string.pref_key_time_thr), getString(R.string.pref_default_time_thr));
@@ -450,7 +454,7 @@ public class RTABMapActivity extends Activity implements OnClickListener {
 			boolean optimizeFromGraphEnd = sharedPref.getBoolean(getString(R.string.pref_key_optimize_end), Boolean.parseBoolean(getString(R.string.pref_default_optimize_end)));
 			String optimizer = sharedPref.getString(getString(R.string.pref_key_optimizer), getString(R.string.pref_default_optimizer));
 			
-			Log.d(TAG, "set mapping parameters");
+			if(!DISABLE_LOG) Log.d(TAG, "set mapping parameters");
 			RTABMapLib.setNodesFiltering(sharedPref.getBoolean(getString(R.string.pref_key_nodes_filtering), Boolean.parseBoolean(getString(R.string.pref_default_nodes_filtering))));
 			RTABMapLib.setAutoExposure(sharedPref.getBoolean(getString(R.string.pref_key_auto_exposure), Boolean.parseBoolean(getString(R.string.pref_default_auto_exposure))));
 			RTABMapLib.setRawScanSaved(sharedPref.getBoolean(getString(R.string.pref_key_raw_scan_saved), Boolean.parseBoolean(getString(R.string.pref_default_raw_scan_saved))));
@@ -472,16 +476,17 @@ public class RTABMapActivity extends Activity implements OnClickListener {
 			RTABMapLib.setMappingParameter("RGBD/OptimizeFromGraphEnd", String.valueOf(optimizeFromGraphEnd));
 			RTABMapLib.setMappingParameter("Optimizer/Strategy", optimizer);
 	
-			Log.d(TAG, "set exporting parameters...");
+			if(!DISABLE_LOG) Log.d(TAG, "set exporting parameters...");
 			RTABMapLib.setMeshDecimation(Integer.parseInt(sharedPref.getString(getString(R.string.pref_key_density), getString(R.string.pref_default_density))));
 			RTABMapLib.setMaxCloudDepth(Float.parseFloat(sharedPref.getString(getString(R.string.pref_key_depth), getString(R.string.pref_default_depth))));
 			RTABMapLib.setPointSize(Float.parseFloat(sharedPref.getString(getString(R.string.pref_key_point_size), getString(R.string.pref_default_point_size))));
 			RTABMapLib.setMeshAngleTolerance(Float.parseFloat(sharedPref.getString(getString(R.string.pref_key_angle), getString(R.string.pref_default_angle))));
 			RTABMapLib.setMeshTriangleSize(Integer.parseInt(sharedPref.getString(getString(R.string.pref_key_triangle), getString(R.string.pref_default_triangle))));
 			
-			Log.d(TAG, "set rendering parameters...");
+			if(!DISABLE_LOG) Log.d(TAG, "set rendering parameters...");
-			RTABMapLib.setMinClusterSize(Integer.parseInt(sharedPref.getString(getString(R.string.pref_key_min_cluster_size), getString(R.string.pref_default_min_cluster_size))));
+			RTABMapLib.setClusterRatio(Float.parseFloat(sharedPref.getString(getString(R.string.pref_key_cluster_ratio), getString(R.string.pref_default_cluster_ratio))));
 			RTABMapLib.setMaxGainRadius(Float.parseFloat(sharedPref.getString(getString(R.string.pref_key_gain_max_radius), getString(R.string.pref_default_gain_max_radius))));
+			RTABMapLib.setRenderingTextureDecimation(Integer.parseInt(sharedPref.getString(getString(R.string.pref_key_rendering_texture_decimation), getString(R.string.pref_default_rendering_texture_decimation))));
 	
 			if(mItemRenderingPointCloud != null)
 			{
@@ -511,14 +516,14 @@ public class RTABMapActivity extends Activity implements OnClickListener {
 			mToast.makeText(this, String.format("Error parsing preferences: "+e.getMessage()), mToast.LENGTH_LONG).show();
 		}
 
-		Log.i(TAG, String.format("onResume()"));
+		if(!DISABLE_LOG) Log.i(TAG, String.format("onResume()"));
 
 		if (Tango.hasPermission(this, Tango.PERMISSIONTYPE_MOTION_TRACKING)) {
 
 			mGLView.onResume();
 
 		} else {
-			Log.i(TAG, String.format("Asking for motion tracking permission"));
+			if(!DISABLE_LOG) Log.i(TAG, String.format("Asking for motion tracking permission"));
 			startActivityForResult(
 					Tango.getRequestPermissionIntent(Tango.PERMISSIONTYPE_MOTION_TRACKING),
 					Tango.TANGO_INTENT_ACTIVITYCODE);
@@ -530,7 +535,6 @@ public class RTABMapActivity extends Activity implements OnClickListener {
 	private void setCamera(int type)
 	{		
 		// for convenience, for a refresh of the memory used
-		stopUpdateStatusThread();
 		mStatusTexts[1] = getString(R.string.memory)+String.valueOf(Debug.getNativeHeapAllocatedSize()/(1024*1024));
 		mStatusTexts[2] = getString(R.string.free_memory)+String.valueOf(getFreeMemory());
 		updateStatusTexts();
@@ -618,7 +622,7 @@ public class RTABMapActivity extends Activity implements OnClickListener {
 
 	@Override
 	public boolean onCreateOptionsMenu(Menu menu) {
-		Log.i(TAG, "called onCreateOptionsMenu;");
+		if(!DISABLE_LOG) Log.i(TAG, "called onCreateOptionsMenu;");
 
 		MenuInflater inflater = getMenuInflater();
 		inflater.inflate(R.menu.optionmenu, menu);
@@ -734,7 +738,7 @@ public class RTABMapActivity extends Activity implements OnClickListener {
 				long memoryUsed = processMemoryUsed;
 				long memoryFree = getFreeMemory();
 				
-				if(memoryFree < 100)
+				if(memoryFree < 200)
 				{
 					mButtonPause.setChecked(true);
 					pauseMapping();
@@ -843,33 +847,43 @@ public class RTABMapActivity extends Activity implements OnClickListener {
 			final float rehearsalValue,
 			final float optimizationMaxError)
 	{
-		Log.i(TAG, String.format("updateStatsCallback()"));
+		if(!DISABLE_LOG) Log.i(TAG, String.format("updateStatsCallback()"));
 
 		final String[] statusTexts = new String[16];
-		if(mButtonPause!=null)
+		if(mButtonPause!=null && !mButtonPause.isChecked())
 		{
-			if(mButtonPause.isChecked())
+			String updateValue = mUpdateRate.compareTo("0")==0?"Max":mUpdateRate;
-			{
+			statusTexts[0] = getString(R.string.status)+(mItemDataRecorderMode!=null&&mItemDataRecorderMode.isChecked()?String.format("Recording (%s Hz)", updateValue):mItemLocalizationMode!=null && mItemLocalizationMode.isChecked()?String.format("Localization (%s Hz)", updateValue):String.format("Mapping (%s Hz)", updateValue));
-				statusTexts[0] = getString(R.string.status)+"Paused";
-			}
-			else if(mItemLocalizationMode!=null && mItemDataRecorderMode!=null)
-			{			
-				String updateValue = mUpdateRate.compareTo("0")==0?"Max":mUpdateRate;
-				statusTexts[0] = getString(R.string.status)+(mItemLocalizationMode.isChecked()?String.format("Localization (%s Hz)", updateValue):mItemDataRecorderMode.isChecked()?String.format("Recording (%s Hz)", updateValue):String.format("Mapping (%s Hz)", updateValue));
-			}
 		}
 		else
 		{
-			statusTexts[0] = getString(R.string.status);
+			statusTexts[0] = mStatusTexts[0];
 		}
 		
-		stopUpdateStatusThread();
-		
 		// getNativeHeapAllocatedSize() is too slow, so we need to use the estimate.
 		// Multiply by 3/2 to match getNativeHeapAllocatedSize()
 		final int adjustedMemoryUsed = (processMemoryUsed*3)/2;
 		
-		statusTexts[1] = getString(R.string.memory)+adjustedMemoryUsed; 
+		if(mButtonPause!=null)
+		{
+			if(!mButtonPause.isChecked())
+			{
+				statusTexts[1] = getString(R.string.memory)+adjustedMemoryUsed; 
+			}
+			else if(mState == State.STATE_PROCESSING)
+			{
+				// This request is long to do, only do it when processing.
+				statusTexts[1] = getString(R.string.memory)+String.valueOf(Debug.getNativeHeapAllocatedSize()/(1024*1024));
+			}
+			else
+			{
+				statusTexts[1] = mStatusTexts[1];
+			}
+		}
+		else
+		{
+			statusTexts[1] = mStatusTexts[1];
+		}
 		statusTexts[2] = getString(R.string.free_memory)+getFreeMemory();
 	
 		
@@ -878,6 +892,8 @@ public class RTABMapActivity extends Activity implements OnClickListener {
 			++mTotalLoopClosures;
 		}
 		
+		mMapNodes = nodes;
+		
 		int index = 4;
 		statusTexts[index++] = getString(R.string.nodes)+nodes+" (" + nodesDrawn + " shown)";
 		statusTexts[index++] = getString(R.string.words)+words;
@@ -903,7 +919,7 @@ public class RTABMapActivity extends Activity implements OnClickListener {
 			int status, 
 			String msg)
 	{
-		Log.i(TAG, String.format("rtabmapInitEventsUI() status=%d msg=%s", status, msg));
+		if(!DISABLE_LOG) Log.i(TAG, String.format("rtabmapInitEventsUI() status=%d msg=%s", status, msg));
 
 		if(mButtonPause!=null)
 		{
@@ -911,9 +927,9 @@ public class RTABMapActivity extends Activity implements OnClickListener {
 			{
 				mStatusTexts[0] = getString(R.string.status)+(status == 1 && msg.isEmpty()?"Paused":msg);
 			}
-			else
+			else if(mItemLocalizationMode!=null && mItemDataRecorderMode!=null)
 			{
-				mStatusTexts[0] = getString(R.string.status)+(status == 1 && msg.isEmpty()?(mItemLocalizationMode!=null&&mItemLocalizationMode.isChecked()?"Localization":mItemDataRecorderMode!=null&&mItemDataRecorderMode.isChecked()?"Recording":"Mapping"):msg);
+				mStatusTexts[0] = getString(R.string.status)+(status == 1 && msg.isEmpty()?(mItemDataRecorderMode!=null&&mItemDataRecorderMode.isChecked()?"Recording":mItemLocalizationMode!=null&&mItemLocalizationMode.isChecked()?"Localization":"Mapping"):msg);
 			}
 			
 			mStatusTexts[1] = getString(R.string.memory)+String.valueOf(Debug.getNativeHeapAllocatedSize()/(1024*1024));
@@ -927,7 +943,7 @@ public class RTABMapActivity extends Activity implements OnClickListener {
 			final int status, 
 			final String msg)
 	{
-		Log.i(TAG, String.format("rtabmapInitEventCallback()"));
+		if(!DISABLE_LOG) Log.i(TAG, String.format("rtabmapInitEventCallback()"));
 
 		runOnUiThread(new Runnable() {
 			public void run() {
@@ -940,7 +956,7 @@ public class RTABMapActivity extends Activity implements OnClickListener {
 			int count, 
 			int max)
 	{
-		Log.i(TAG, String.format("updateProgressionUI() count=%d max=%s", count, max));
+		if(!DISABLE_LOG) Log.i(TAG, String.format("updateProgressionUI() count=%d max=%s", count, max));
 
 		mExportProgressDialog.setMax(max);
 		mExportProgressDialog.setProgress(count);
@@ -951,7 +967,7 @@ public class RTABMapActivity extends Activity implements OnClickListener {
 			final int count, 
 			final int max)
 	{
-		Log.i(TAG, String.format("updateProgressionCallback()"));
+		if(!DISABLE_LOG) Log.i(TAG, String.format("updateProgressionCallback()"));
 
 		runOnUiThread(new Runnable() {
 			public void run() {
@@ -1105,49 +1121,12 @@ public class RTABMapActivity extends Activity implements OnClickListener {
 		workingThread.start();
 	}
 		
-	public void startUpdateStatusThread()
-	{
-		if(mMemStatusUpdateThread == null)
-		{
-			mMemStatusUpdateThread = new Thread() {
-	
-	        	@Override
-	        	public void run() {
-	        		try {
-	        			while (!isInterrupted()) {
-	        				Thread.sleep(1000);
-	        				if(mState != RTABMapActivity.State.STATE_VISUALIZING)
-	        				{
-		        				runOnUiThread(new Runnable() {
-		        					@Override
-		        					public void run() {
-		        						mStatusTexts[1] = getString(R.string.memory)+String.valueOf(Debug.getNativeHeapAllocatedSize()/(1024*1024));
-		        						mStatusTexts[2] = getString(R.string.free_memory)+String.valueOf(getFreeMemory());
-		        						updateStatusTexts();
-		        					}
-		        				});
-	        				}
-	        			}
-	        		} catch (InterruptedException e) {
-	        		}
-	        	}
-	        };
-	        mMemStatusUpdateThread.start();
-		}
-	}
-	
-	public void stopUpdateStatusThread()
-	{
-		if(mMemStatusUpdateThread != null)
-		{
-			Thread tmp = mMemStatusUpdateThread;
-			mMemStatusUpdateThread = null;
-			tmp.interrupt();
-		}
-	}
-
 	private void updateState(State state)
 	{	
+		if(mState == State.STATE_VISUALIZING && state == State.STATE_IDLE && mMapNodes > 100)
+		{
+			mToast.makeText(getActivity(), String.format("Re-adding %d online clouds, this may take some time...", mMapNodes), mToast.LENGTH_LONG).show();
+		}
 		mState = state;
 		switch(state)
 		{
@@ -1164,7 +1143,6 @@ public class RTABMapActivity extends Activity implements OnClickListener {
 			mItemReset.setEnabled(false);
 			mItemModes.setEnabled(false);
 			mButtonPause.setVisibility(View.INVISIBLE);
-			startUpdateStatusThread();
 			break;
 		case STATE_VISUALIZING:
 			mButtonLighting.setVisibility(View.VISIBLE);
@@ -1180,7 +1158,6 @@ public class RTABMapActivity extends Activity implements OnClickListener {
 			mItemModes.setEnabled(true);
 			mButtonPause.setVisibility(View.INVISIBLE);
 			mItemDataRecorderMode.setEnabled(mButtonPause.isChecked());
-			stopUpdateStatusThread();
 			break;
 		default:
 			mButtonLighting.setVisibility(View.INVISIBLE);
@@ -1197,7 +1174,6 @@ public class RTABMapActivity extends Activity implements OnClickListener {
 			mButtonPause.setVisibility(View.VISIBLE);
 			mItemDataRecorderMode.setEnabled(mButtonPause.isChecked());
 			RTABMapLib.postExportation(false);
-			stopUpdateStatusThread();
 			break;
 		}
 	}
@@ -1259,7 +1235,7 @@ public class RTABMapActivity extends Activity implements OnClickListener {
 	}
 
 	public boolean onOptionsItemSelected(MenuItem item) {
-		Log.i(TAG, "called onOptionsItemSelected; selected item: " + item);
+		if(!DISABLE_LOG) Log.i(TAG, "called onOptionsItemSelected; selected item: " + item);
 		int itemId = item.getItemId();
 		if (itemId == R.id.post_processing_standard)
 		{
@@ -1500,6 +1476,7 @@ public class RTABMapActivity extends Activity implements OnClickListener {
 			mTotalLoopClosures = 0;
 			
 			int index = 4;
+			mMapNodes = 0;
 			mStatusTexts[index++] = getString(R.string.nodes)+0;
 			mStatusTexts[index++] = getString(R.string.words)+0;
 			mStatusTexts[index++] = getString(R.string.database_size)+0;
@@ -1538,6 +1515,7 @@ public class RTABMapActivity extends Activity implements OnClickListener {
 					// reset
 					mTotalLoopClosures = 0;
 					int index = 4;
+					mMapNodes = 0;
 					mStatusTexts[index++] = getString(R.string.nodes)+0;
 					mStatusTexts[index++] = getString(R.string.words)+0;
 					mStatusTexts[index++] = getString(R.string.database_size)+0;
@@ -1756,6 +1734,8 @@ public class RTABMapActivity extends Activity implements OnClickListener {
 		Thread exportThread = new Thread(new Runnable() {
 			public void run() {
 
+				final long startTime = System.currentTimeMillis()/1000;
+
 				final boolean success = RTABMapLib.exportMesh(
 						tmpPath,
 						cloudVoxelSize,
@@ -1784,10 +1764,12 @@ public class RTABMapActivity extends Activity implements OnClickListener {
 									mToast.makeText(getActivity(), String.format("Cloud assembled and voxelized at %s m.", cloudVoxelSizeStr), mToast.LENGTH_LONG).show();
 								}
 								
+								final long endTime = System.currentTimeMillis()/1000;
+
 								// Visualize the result?
 								AlertDialog d = new AlertDialog.Builder(getActivity())
 								.setCancelable(false)
-								.setTitle("Export Successful!")
+								.setTitle("Export Successful! (" + (endTime-startTime) + " sec)")
 								.setMessage(Html.fromHtml("Do you want visualize the result before saving to file or sharing to <a href=\"https://sketchfab.com/about\">Sketchfab</a>?"))
 								.setPositiveButton("Yes", new DialogInterface.OnClickListener() {
 									public void onClick(DialogInterface dialog, int which) {
@@ -1869,7 +1851,6 @@ public class RTABMapActivity extends Activity implements OnClickListener {
 		}
 		mProgressDialog.show();
 		updateState(State.STATE_PROCESSING);
-		startUpdateStatusThread();
 		Thread saveThread = new Thread(new Runnable() {
 			public void run() {
 				RTABMapLib.save(newDatabasePath); // save
@@ -2059,16 +2040,6 @@ public class RTABMapActivity extends Activity implements OnClickListener {
 	{
 		mOpenedDatabasePath = mWorkingDirectory + fileName;
 		
-		final String tmpDatabase = mWorkingDirectory+RTABMAP_TMP_DB;
-		(new File(tmpDatabase)).delete();
-		try{
-			copy(new File(mOpenedDatabasePath), new File(tmpDatabase));
-		}
-		catch(IOException e)
-		{
-			mToast.makeText(getActivity(), String.format("Failed to create temp database from %s!", mOpenedDatabasePath), mToast.LENGTH_LONG).show();
-		}
-
 		SharedPreferences sharedPref = PreferenceManager.getDefaultSharedPreferences(getActivity());
 		final boolean databaseInMemory = sharedPref.getBoolean(getString(R.string.pref_key_db_in_memory), Boolean.parseBoolean(getString(R.string.pref_default_db_in_memory)));		
 		
@@ -2081,6 +2052,19 @@ public class RTABMapActivity extends Activity implements OnClickListener {
 		Thread openThread = new Thread(new Runnable() {
 			public void run() {
 
+				final String tmpDatabase = mWorkingDirectory+RTABMAP_TMP_DB;
+				(new File(tmpDatabase)).delete();
+				try{
+					copy(new File(mOpenedDatabasePath), new File(tmpDatabase));
+				}
+				catch(IOException e)
+				{
+					mToast.makeText(getActivity(), String.format("Failed to create temp database from %s!", mOpenedDatabasePath), mToast.LENGTH_LONG).show();
+					updateState(State.STATE_IDLE);
+					mProgressDialog.dismiss();
+					return;
+				}
+				
 				final int status = RTABMapLib.openDatabase(tmpDatabase, databaseInMemory, optimize);
 				
 				runOnUiThread(new Runnable() {
@@ -2113,12 +2097,10 @@ public class RTABMapActivity extends Activity implements OnClickListener {
 						else
 						{
 							// creating meshes...
-							startUpdateStatusThread();
-							
 							if(!mItemTrajectoryMode.isChecked())
 							{
 								mProgressDialog.setTitle("Loading");
-								mProgressDialog.setMessage(String.format("Database \"%s\" loaded. Please wait while creating point clouds and meshes...", fileName));
+								mProgressDialog.setMessage(String.format("Database \"%s\" loaded. Please wait while rendering point clouds and meshes...", fileName));
 								mProgressDialog.show();
 							}
 						}

app/android/src/com/introlab/rtabmap/RTABMapLib.java

@@ -80,8 +80,9 @@ public class RTABMapLib
     public static native void setMeshDecimation(int value);
     public static native void setMeshAngleTolerance(float value);
     public static native void setMeshTriangleSize(int value);
-    public static native void setMinClusterSize(int value);
+    public static native void setClusterRatio(float value);
     public static native void setMaxGainRadius(float value);
+    public static native void setRenderingTextureDecimation(int value);
     public static native int setMappingParameter(String key, String value);
 
     public static native void resetMapping();

app/android/src/com/introlab/rtabmap/Renderer.java

@@ -96,36 +96,49 @@ public class Renderer implements GLSurfaceView.Renderer {
 				mTextManager.Draw(mtrxProjectionAndView);
 			}
 
-			mActivity.runOnUiThread(new Runnable() {
+			if(value != 0 && mProgressDialog != null && mProgressDialog.isShowing())
-				public void run() {
-					if(value != 0 && mProgressDialog != null && mProgressDialog.isShowing())
-					{
-						Log.i("RTABMapActivity", "Renderer: dismiss dialog, value received=" + String.valueOf(value));
-						mProgressDialog.dismiss();
-						mActivity.stopUpdateStatusThread();
-					}
-					if(value==-1 && mToast!=null)
-					{
-						mToast.makeText(mActivity, String.format("Out of Memory!"), Toast.LENGTH_SHORT).show();
-					}
-					else if(value==-2 && mToast!=null)
-					{
-						mToast.makeText(mActivity, String.format("Rendering Error!"), Toast.LENGTH_SHORT).show();
-					}
-				} 
-			});
-		}
-		catch(final Exception e)
-		{
-			if(mToast!=null)
 			{
 				mActivity.runOnUiThread(new Runnable() {
 					public void run() {
-						mToast.makeText(mActivity, String.format("Rendering error! %s", e.getMessage()), Toast.LENGTH_SHORT).show();
+						if(!RTABMapActivity.DISABLE_LOG) Log.i("RTABMapActivity", "Renderer: dismiss dialog, value received=" + String.valueOf(value));
+						mProgressDialog.dismiss();
 					}
-
 				});
 			}
+			if(value==-1)
+			{
+				mActivity.runOnUiThread(new Runnable() {
+					public void run() {
+						if(mToast!=null)
+						{
+							mToast.makeText(mActivity, String.format("Out of Memory!"), Toast.LENGTH_SHORT).show();
+						}
+					}
+				});
+			}
+			else if(value==-2)
+			{
+				mActivity.runOnUiThread(new Runnable() {
+					public void run() {
+						if(mToast!=null)
+						{
+							mToast.makeText(mActivity, String.format("Rendering Error!"), Toast.LENGTH_SHORT).show();
+						}
+					}
+				});
+			}
+		}
+		catch(final Exception e)
+		{
+			mActivity.runOnUiThread(new Runnable() {
+				public void run() {
+					if(mToast!=null)
+					{
+						mToast.makeText(mActivity, String.format("Rendering error! %s", e.getMessage()), Toast.LENGTH_SHORT).show();
+					}
+				}
+
+			});
 		}
 	}
 

app/android/src/com/introlab/rtabmap/SettingsActivity.java

@@ -33,6 +33,7 @@ public class SettingsActivity extends PreferenceActivity implements OnSharedPref
         ((Preference)findPreference(getString(R.string.pref_key_point_size))).setSummary("("+((ListPreference)findPreference(getString(R.string.pref_key_point_size))).getEntry() + ") "+getString(R.string.pref_summary_point_size));
         ((Preference)findPreference(getString(R.string.pref_key_angle))).setSummary("("+((ListPreference)findPreference(getString(R.string.pref_key_angle))).getEntry() + ") "+getString(R.string.pref_summary_angle));
         ((Preference)findPreference(getString(R.string.pref_key_triangle))).setSummary("("+((ListPreference)findPreference(getString(R.string.pref_key_triangle))).getEntry() + ") "+getString(R.string.pref_summary_triangle));
+        ((Preference)findPreference(getString(R.string.pref_key_rendering_texture_decimation))).setSummary("("+((ListPreference)findPreference(getString(R.string.pref_key_rendering_texture_decimation))).getEntry() + ") "+getString(R.string.pref_summary_rendering_texture_decimation));
           
         ((Preference)findPreference(getString(R.string.pref_key_update_rate))).setSummary("("+((ListPreference)findPreference(getString(R.string.pref_key_update_rate))).getEntry() + ") "+getString(R.string.pref_summary_update_rate));
         ((Preference)findPreference(getString(R.string.pref_key_time_thr))).setSummary("("+((ListPreference)findPreference(getString(R.string.pref_key_time_thr))).getEntry() + ") "+getString(R.string.pref_summary_time_thr));
@@ -55,7 +56,7 @@ public class SettingsActivity extends PreferenceActivity implements OnSharedPref
         ((Preference)findPreference(getString(R.string.pref_key_opt_depth))).setSummary("("+((ListPreference)findPreference(getString(R.string.pref_key_opt_depth))).getEntry() + ") "+getString(R.string.pref_summary_opt_depth));
         ((Preference)findPreference(getString(R.string.pref_key_opt_color_radius))).setSummary("("+((ListPreference)findPreference(getString(R.string.pref_key_opt_color_radius))).getEntry() + ") "+getString(R.string.pref_summary_opt_color_radius));
     
-        ((Preference)findPreference(getString(R.string.pref_key_min_cluster_size))).setSummary("("+((ListPreference)findPreference(getString(R.string.pref_key_min_cluster_size))).getEntry() + ") "+getString(R.string.pref_summary_min_cluster_size));
+        ((Preference)findPreference(getString(R.string.pref_key_cluster_ratio))).setSummary("("+((ListPreference)findPreference(getString(R.string.pref_key_cluster_ratio))).getEntry() + ") "+getString(R.string.pref_summary_cluster_ratio));
         ((Preference)findPreference(getString(R.string.pref_key_gain_max_radius))).setSummary("("+((ListPreference)findPreference(getString(R.string.pref_key_gain_max_radius))).getEntry() + ") "+getString(R.string.pref_summary_gain_max_radius));
     }
     
@@ -68,6 +69,7 @@ public class SettingsActivity extends PreferenceActivity implements OnSharedPref
         	if(key.compareTo(getString(R.string.pref_key_point_size))==0) pref.setSummary("("+((ListPreference)pref).getEntry() + ") "+getString(R.string.pref_summary_point_size));
         	if(key.compareTo(getString(R.string.pref_key_angle))==0) pref.setSummary("("+((ListPreference)pref).getEntry() + ") "+getString(R.string.pref_summary_angle));
         	if(key.compareTo(getString(R.string.pref_key_triangle))==0) pref.setSummary("("+((ListPreference)pref).getEntry() + ") "+getString(R.string.pref_summary_triangle));
+        	if(key.compareTo(getString(R.string.pref_key_rendering_texture_decimation))==0) pref.setSummary("("+((ListPreference)pref).getEntry() + ") "+getString(R.string.pref_summary_rendering_texture_decimation));
         	
         	if(key.compareTo(getString(R.string.pref_key_update_rate))==0) pref.setSummary("("+((ListPreference)pref).getEntry() + ") "+getString(R.string.pref_summary_update_rate));
         	if(key.compareTo(getString(R.string.pref_key_time_thr))==0) pref.setSummary("("+((ListPreference)pref).getEntry() + ") "+getString(R.string.pref_summary_time_thr));
@@ -90,7 +92,7 @@ public class SettingsActivity extends PreferenceActivity implements OnSharedPref
         	if(key.compareTo(getString(R.string.pref_key_opt_depth))==0) pref.setSummary("("+((ListPreference)pref).getEntry() + ") "+getString(R.string.pref_summary_opt_depth));
         	if(key.compareTo(getString(R.string.pref_key_opt_color_radius))==0) pref.setSummary("("+((ListPreference)pref).getEntry() + ") "+getString(R.string.pref_summary_opt_color_radius));
         	
-        	if(key.compareTo(getString(R.string.pref_key_min_cluster_size))==0) pref.setSummary("("+((ListPreference)pref).getEntry() + ") "+getString(R.string.pref_summary_min_cluster_size));
+        	if(key.compareTo(getString(R.string.pref_key_cluster_ratio))==0) pref.setSummary("("+((ListPreference)pref).getEntry() + ") "+getString(R.string.pref_summary_cluster_ratio));
         	if(key.compareTo(getString(R.string.pref_key_gain_max_radius))==0) pref.setSummary("("+((ListPreference)pref).getEntry() + ") "+getString(R.string.pref_summary_gain_max_radius));
         
         }

app/android/src/com/introlab/rtabmap/SketchfabActivity.java

@@ -213,6 +213,7 @@ public class SketchfabActivity extends Activity implements OnClickListener {
 			mAuthDialog = new Dialog(this);
 			mAuthDialog.setContentView(R.layout.auth_dialog);
 			web = (WebView)mAuthDialog.findViewById(R.id.webv);
+			web.setWebContentsDebuggingEnabled(!RTABMapActivity.DISABLE_LOG);
 			web.getSettings().setJavaScriptEnabled(true);
 			String auth_url = AUTHORIZE_PATH+"?redirect_uri="+REDIRECT_URI+"&response_type=token&client_id="+CLIENT_ID;
 			Log.i(RTABMapActivity.TAG, "Auhorize url="+auth_url);

app/src/ObjDeletionHandler.h

@@ -50,13 +50,14 @@ signals:
 	void objDeletionEventReceived(int);
 
 protected:
-	virtual void handleEvent(UEvent * event)
+	virtual bool handleEvent(UEvent * event)
 	{
 		if(event->getClassName().compare("UObjDeletedEvent") == 0 &&
 		   event->getCode() == _watchedId)
 		{
 			emit objDeletionEventReceived(_watchedId);
 		}
+		return false;
 	}
 private:
 	int _watchedId;

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"));
+		cv::Mat compressedImage;
-		rtabmap::CompressionThread ctDepth(depthOrRightImage, std::string(".png"));
+		cv::Mat compressedDepth;
-		rtabmap::CompressionThread ctLaserScan(laserScan);
+		cv::Mat compressedScan;
-		rtabmap::CompressionThread ctUserData(data.userDataRaw());
+		cv::Mat compressedUserData;
-		if(!image.empty())
+		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(),
+						compressedImage,
-						ctDepth.getCompressedData(),
+						compressedDepth,
 						stereoCameraModel,
 						id,
 						0,
-						ctUserData.getCompressedData()):
+						compressedUserData):
 				SensorData(
-						ctLaserScan.getCompressedData(),
+						compressedScan,
 						LaserScanInfo(maxLaserScanMaxPts, data.laserScanInfo().maxRange(), data.laserScanInfo().localTransform()),
-						ctImage.getCompressedData(),
+						compressedImage,
-						ctDepth.getCompressedData(),
+						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());
+		cv::Mat compressedScan;
-		rtabmap::CompressionThread ctLaserScan(laserScan);
+		cv::Mat compressedUserData;
-		if(!data.userDataRaw().empty() && !isIntermediateNode)
+		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);

examples/RGBDMapping/MapBuilder.h

@@ -249,7 +249,7 @@ protected slots:
 		processingStatistics_ = false;
 	}
 
-	virtual void handleEvent(UEvent * event)
+	virtual bool handleEvent(UEvent * event)
 	{
 		if(event->getClassName().compare("RtabmapEvent") == 0)
 		{
@@ -273,6 +273,7 @@ protected slots:
 				QMetaObject::invokeMethod(this, "processOdometry", Q_ARG(rtabmap::OdometryEvent, *odomEvent));
 			}
 		}
+		return false;
 	}
 
 protected:

examples/WifiMapping/MapBuilder.h

@@ -249,7 +249,7 @@ protected slots:
 		processingStatistics_ = false;
 	}
 
-	virtual void handleEvent(UEvent * event)
+	virtual bool handleEvent(UEvent * event)
 	{
 		if(event->getClassName().compare("RtabmapEvent") == 0)
 		{
@@ -273,6 +273,7 @@ protected slots:
 				QMetaObject::invokeMethod(this, "processOdometry", Q_ARG(rtabmap::OdometryEvent, *odomEvent));
 			}
 		}
+		return false;
 	}
 
 protected:

examples/WifiMapping/MapBuilderWifi.h

@@ -66,7 +66,7 @@ public:
 	}
 
 protected:
-	virtual void handleEvent(UEvent * event)
+	virtual bool handleEvent(UEvent * event)
 	{
 		if(event->getClassName().compare("UserDataEvent") == 0)
 		{
@@ -84,7 +84,7 @@ protected:
 				wifiLevels_.insert(std::make_pair(stamp, level));
 			}
 		}
-		MapBuilder::handleEvent(event);
+		return MapBuilder::handleEvent(event);
 	}
 
 protected slots:

guilib/include/rtabmap/gui/CalibrationDialog.h

@@ -86,7 +86,7 @@ private slots:
 
 protected:
 	virtual void closeEvent(QCloseEvent* event);
-	virtual void handleEvent(UEvent * event);
+	virtual bool handleEvent(UEvent * event);
 
 private:
 	float getArea(const std::vector<cv::Point2f> & corners, const cv::Size & boardSize);

guilib/include/rtabmap/gui/CameraViewer.h

@@ -56,7 +56,7 @@ public:
 public slots:
 	void showImage(const rtabmap::SensorData & data);
 protected:
-	virtual void handleEvent(UEvent * event);
+	virtual bool handleEvent(UEvent * event);
 
 private:
 	ImageView* imageView_;

guilib/include/rtabmap/gui/ConsoleWidget.h

@@ -61,7 +61,7 @@ private slots:
 	void updateTextEditBufferSize();
 
 protected:
-	virtual void handleEvent(UEvent * anEvent);
+	virtual bool handleEvent(UEvent * anEvent);
 
 private:
 	Ui_consoleWidget * _ui;

guilib/include/rtabmap/gui/DataRecorder.h

@@ -61,7 +61,7 @@ public slots:
 	void showImage(const cv::Mat & image, const cv::Mat & depth);
 protected:
 	virtual void closeEvent(QCloseEvent* event);
-	void handleEvent(UEvent * event);
+	bool handleEvent(UEvent * event);
 
 private:
 	UMutex memoryMutex_;

guilib/include/rtabmap/gui/MainWindow.h

@@ -114,7 +114,7 @@ public slots:
 
 protected:
 	virtual void closeEvent(QCloseEvent* event);
-	virtual void handleEvent(UEvent* anEvent);
+	virtual bool handleEvent(UEvent* anEvent);
 	virtual void showEvent(QShowEvent* anEvent);
 	virtual void moveEvent(QMoveEvent* anEvent);
 	virtual void resizeEvent(QResizeEvent* anEvent);

guilib/include/rtabmap/gui/OdometryViewer.h

@@ -64,7 +64,7 @@ public slots:
 	virtual void clear();
 
 protected:
-	virtual void handleEvent(UEvent * event);
+	virtual bool handleEvent(UEvent * event);
 
 private slots:
 	void reset();

guilib/src/CalibrationDialog.cpp

@@ -265,7 +265,7 @@ void CalibrationDialog::closeEvent(QCloseEvent* event)
 	}
 }
 
-void CalibrationDialog::handleEvent(UEvent * event)
+bool CalibrationDialog::handleEvent(UEvent * event)
 {
 	if(!processingData_)
 	{
@@ -282,6 +282,7 @@ void CalibrationDialog::handleEvent(UEvent * event)
 			}
 		}
 	}
+	return false;
 }
 
 void CalibrationDialog::processImages(const cv::Mat & imageLeft, const cv::Mat & imageRight, const QString & cameraName)

guilib/src/CameraViewer.cpp

@@ -184,7 +184,7 @@ void CameraViewer::showImage(const rtabmap::SensorData & data)
 	processingImages_ = false;
 }
 
-void CameraViewer::handleEvent(UEvent * event)
+bool CameraViewer::handleEvent(UEvent * event)
 {
 	if(!pause_->isChecked())
 	{
@@ -205,6 +205,7 @@ void CameraViewer::handleEvent(UEvent * event)
 			}
 		}
 	}
+	return false;
 }
 
 } /* namespace rtabmap */

guilib/src/ConsoleWidget.cpp

@@ -62,7 +62,7 @@ ConsoleWidget::~ConsoleWidget()
 	delete _ui;
 }
 
-void ConsoleWidget::handleEvent(UEvent * anEvent)
+bool ConsoleWidget::handleEvent(UEvent * anEvent)
 {
 	// WARNING, don't put a log message here! otherwise it could be recursively called.
 	if(anEvent->getClassName().compare("ULogEvent") == 0)
@@ -100,6 +100,7 @@ void ConsoleWidget::handleEvent(UEvent * anEvent)
 		}
 
 	}
+	return false;
 }
 
 void ConsoleWidget::appendMsg(const QString & msg, int level)

guilib/src/DataRecorder.cpp

@@ -167,7 +167,7 @@ void DataRecorder::closeEvent(QCloseEvent* event)
 	event->accept();
 }
 
-void DataRecorder::handleEvent(UEvent * event)
+bool DataRecorder::handleEvent(UEvent * event)
 {
 	if(memory_)
 	{
@@ -192,6 +192,7 @@ void DataRecorder::handleEvent(UEvent * event)
 			}
 		}
 	}
+	return false;
 }
 
 } /* namespace rtabmap */

guilib/src/MainWindow.cpp

@@ -722,7 +722,7 @@ void MainWindow::closeEvent(QCloseEvent* event)
 	UDEBUG("");
 }
 
-void MainWindow::handleEvent(UEvent* anEvent)
+bool MainWindow::handleEvent(UEvent* anEvent)
 {
 	if(anEvent->getClassName().compare("RtabmapEvent") == 0)
 	{
@@ -856,6 +856,7 @@ void MainWindow::handleEvent(UEvent* anEvent)
 			}
 		}
 	}
+	return false;
 }
 
 void MainWindow::processCameraInfo(const rtabmap::CameraInfo & info)

guilib/src/OdometryViewer.cpp

@@ -470,7 +470,7 @@ void OdometryViewer::processData(const rtabmap::OdometryEvent & odom)
 	processingData_ = false;
 }
 
-void OdometryViewer::handleEvent(UEvent * event)
+bool OdometryViewer::handleEvent(UEvent * event)
 {
 	if(!processingData_ && this->isVisible())
 	{
@@ -485,6 +485,7 @@ void OdometryViewer::handleEvent(UEvent * event)
 			}
 		}
 	}
+	return false;
 }
 
 } /* namespace rtabmap */

guilib/src/PreferencesDialog.cpp

@@ -606,6 +606,7 @@ PreferencesDialog::PreferencesDialog(QWidget * parent) :
 	_ui->general_checkBox_keepBinaryData->setObjectName(Parameters::kMemBinDataKept().c_str());
 	_ui->general_checkBox_keepDescriptors->setObjectName(Parameters::kMemRawDescriptorsKept().c_str());
 	_ui->general_checkBox_saveDepth16bits->setObjectName(Parameters::kMemSaveDepth16Format().c_str());
+	_ui->general_checkBox_compressionParallelized->setObjectName(Parameters::kMemCompressionParallelized().c_str());
 	_ui->general_checkBox_reduceGraph->setObjectName(Parameters::kMemReduceGraph().c_str());
 	_ui->general_checkBox_keepNotLinkedNodes->setObjectName(Parameters::kMemNotLinkedNodesKept().c_str());
 	_ui->general_spinBox_maxStMemSize->setObjectName(Parameters::kMemSTMSize().c_str());

guilib/src/TexturingState.h

@@ -40,7 +40,7 @@ class TexturingState : public QObject, public ProgressState
 	Q_OBJECT
 
 public:
-	TexturingState(ProgressDialog * dialog): dialog_(dialog), canceled_(false)
+	TexturingState(ProgressDialog * dialog): dialog_(dialog)
 	{
 		connect(dialog_, SIGNAL(canceled()), this, SLOT(cancel()));
 	}
@@ -53,18 +53,17 @@ public:
 			dialog_->incrementStep();
 		}
 		QApplication::processEvents();
-		return !canceled_;
+		return !isCanceled();
 	}
 
 public slots:
 	void cancel()
 	{
-		canceled_ = true;
+		setCanceled(true);
 	}
 
 private:
 	ProgressDialog * dialog_;
-	bool canceled_;
 };
 
 }

guilib/src/ui/preferencesDialog.ui

@@ -63,7 +63,7 @@
           <property name="geometry">
            <rect>
             <x>0</x>
-            <y>0</y>
+            <y>-182</y>
             <width>673</width>
             <height>2718</height>
            </rect>
@@ -86,7 +86,7 @@
                 <enum>QFrame::Raised</enum>
                </property>
                <property name="currentIndex">
-                <number>21</number>
+                <number>8</number>
                </property>
                <widget class="QWidget" name="page_22">
                 <layout class="QVBoxLayout" name="verticalLayout_29" stretch="0,1">
@@ -6077,7 +6077,7 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag
                         </property>
                        </widget>
                       </item>
-                      <item row="14" column="0">
+                      <item row="15" column="0">
                        <widget class="QSpinBox" name="general_spinBox_laserScanDownsample">
                         <property name="minimumSize">
                          <size>
@@ -6116,7 +6116,7 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag
                         </property>
                        </widget>
                       </item>
-                      <item row="12" column="1">
+                      <item row="13" column="1">
                        <widget class="QLabel" name="label_retrieved_13">
                         <property name="text">
                          <string>Image pre decimation. This option can be used to reduce image size 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).</string>
@@ -6188,7 +6188,7 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag
                         </property>
                        </widget>
                       </item>
-                      <item row="13" column="0">
+                      <item row="14" column="0">
                        <widget class="QSpinBox" name="spinBox_imagePostDecimation">
                         <property name="minimum">
                          <number>-16</number>
@@ -6234,7 +6234,7 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag
                         </property>
                        </widget>
                       </item>
-                      <item row="13" column="1">
+                      <item row="14" column="1">
                        <widget class="QLabel" name="label_retrieved_6">
                         <property name="text">
                          <string>Image post decimation. This option can be used to save images in lower resolution (size/decimation). It is done on 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).</string>
@@ -6260,7 +6260,7 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag
                         </property>
                        </widget>
                       </item>
-                      <item row="14" column="1">
+                      <item row="15" column="1">
                        <widget class="QLabel" name="label_retrieved_8">
                         <property name="text">
                          <string>If &gt; 1, downsample the laser scans when creating a location. This feature can be used to save laser scans already downsampled.</string>
@@ -6283,7 +6283,7 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag
                         </property>
                        </widget>
                       </item>
-                      <item row="12" column="0">
+                      <item row="13" column="0">
                        <widget class="QSpinBox" name="spinBox_imagePreDecimation">
                         <property name="minimum">
                          <number>-16</number>
@@ -6293,20 +6293,7 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag
                         </property>
                        </widget>
                       </item>
-                      <item row="15" column="1">
+                      <item row="16" column="0">
-                       <widget class="QLabel" name="label_retrieved_14">
-                        <property name="text">
-                         <string>If &gt; 0 and laser scans are 3D without normals, normals will be computed with K search neighbors when creating a signature.</string>
-                        </property>
-                        <property name="wordWrap">
-                         <bool>true</bool>
-                        </property>
-                        <property name="textInteractionFlags">
-                         <set>Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse</set>
-                        </property>
-                       </widget>
-                      </item>
-                      <item row="15" column="0">
                        <widget class="QSpinBox" name="general_spinBox_laserScanNormalK">
                         <property name="minimumSize">
                          <size>
@@ -6325,6 +6312,42 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag
                         </property>
                        </widget>
                       </item>
+                      <item row="16" column="1">
+                       <widget class="QLabel" name="label_retrieved_14">
+                        <property name="text">
+                         <string>If &gt; 0 and laser scans are 3D without normals, normals will be computed with K search neighbors when creating a signature.</string>
+                        </property>
+                        <property name="wordWrap">
+                         <bool>true</bool>
+                        </property>
+                        <property name="textInteractionFlags">
+                         <set>Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse</set>
+                        </property>
+                       </widget>
+                      </item>
+                      <item row="12" column="1">
+                       <widget class="QLabel" name="label_retrieved_15">
+                        <property name="text">
+                         <string>Multi-threaded compression.</string>
+                        </property>
+                        <property name="wordWrap">
+                         <bool>true</bool>
+                        </property>
+                        <property name="textInteractionFlags">
+                         <set>Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse</set>
+                        </property>
+                       </widget>
+                      </item>
+                      <item row="12" column="0">
+                       <widget class="QCheckBox" name="general_checkBox_compressionParallelized">
+                        <property name="text">
+                         <string/>
+                        </property>
+                        <property name="checked">
+                         <bool>false</bool>
+                        </property>
+                       </widget>
+                      </item>
                      </layout>
                     </item>
                     <item>

tools/Calibration/main.cpp

@@ -129,12 +129,14 @@ int main(int argc, char * argv[])
 	UINFO("Using device %d", device);
 	UINFO("Stereo: %s", stereo?"true":"false");
 
-	bool switchImages = false;
+	QApplication app(argc, argv);
+	rtabmap::CalibrationDialog dialog(stereo, ".");
 
 	rtabmap::Camera * camera = 0;
 	if(driver == -1)
 	{
 		camera = new rtabmap::CameraVideo(device);
+		dialog.setStereoMode(stereo);
 	}
 	else if(driver == 0)
 	{
@@ -183,8 +185,9 @@ int main(int argc, char * argv[])
 			UERROR("Not built with Freenect2 support...");
 			exit(-1);
 		}
-		switchImages = true;
 		camera = new rtabmap::CameraFreenect2(0, rtabmap::CameraFreenect2::kTypeColorIR);
+		dialog.setSwitchedImages(true);
+		dialog.setStereoMode(stereo, "rgb", "depth");
 	}
 	else if(driver == 6)
 	{
@@ -194,6 +197,7 @@ int main(int argc, char * argv[])
 			exit(-1);
 		}
 		camera = new rtabmap::CameraStereoDC1394();
+		dialog.setStereoMode(stereo);
 	}
 	else if(driver == 7)
 	{
@@ -203,6 +207,7 @@ int main(int argc, char * argv[])
 			exit(-1);
 		}
 		camera = new rtabmap::CameraStereoFlyCapture2();
+		dialog.setStereoMode(stereo);
 	}
 	else
 	{
@@ -222,8 +227,6 @@ int main(int argc, char * argv[])
 		cameraThread = new rtabmap::CameraThread(camera);
 	}
 
-	QApplication app(argc, argv);
-	rtabmap::CalibrationDialog dialog(stereo, ".", switchImages);
 	dialog.registerToEventsManager();
 
 	dialog.show();

utilite/include/rtabmap/utilite/UEventsHandler.h

@@ -147,10 +147,15 @@ protected:
      * events are received. If a handling function 
      * takes too much time, the events list can grow 
      * faster than it is emptied. The event can be
-     * modified but must not be deleted.
+     * modified.
+     * @return "true" to notify UEventsManager that this handler took ownership of the
+     *         event (meaning it must delete it). The event will
+     *         not be dispatched to next handlers.
+     * @return "false" to let event be dispatched to next handlers (default behavior). UEventsManager
+     *         will take care of deleting the event.
      *
      */
-    virtual void handleEvent(UEvent * event) = 0;
+    virtual bool handleEvent(UEvent * event) = 0;
 
 protected:
     /**

utilite/include/rtabmap/utilite/UEventsManager.h

@@ -175,7 +175,7 @@ private:
     /*
 	 * This method dispatches an event to all handlers.
 	 */
-    virtual void dispatchEvent(UEvent * event, const UEventsSender * sender);
+    virtual bool dispatchEvent(UEvent * event, const UEventsSender * sender);
 
     /*
      * This method is used to add an events 

utilite/src/UEventsManager.cpp

@@ -190,13 +190,15 @@ void UEventsManager::dispatchEvents()
 	// Past events to handlers
 	for(it=eventsBuf.begin(); it!=eventsBuf.end(); ++it)
 	{
-		dispatchEvent(it->first, it->second);
+		if(!dispatchEvent(it->first, it->second))
-		delete it->first;
+		{
+			delete it->first;
+		}
 	}
     eventsBuf.clear();
 }
 
-void UEventsManager::dispatchEvent(UEvent * event, const UEventsSender * sender)
+bool UEventsManager::dispatchEvent(UEvent * event, const UEventsSender * sender)
 {
 	std::list<UEventsHandler*> handlers;
 
@@ -213,7 +215,9 @@ void UEventsManager::dispatchEvent(UEvent * event, const UEventsSender * sender)
 		handlers = handlers_;
 	}
 
-	for(std::list<UEventsHandler*>::iterator it=handlers.begin(); it!=handlers.end(); ++it)
+	bool handled = false;
+
+	for(std::list<UEventsHandler*>::iterator it=handlers.begin(); it!=handlers.end() && !handled; ++it)
 	{
 		// Check if the handler is still in the
 		// handlers_ list (may be changed if addHandler() or
@@ -228,13 +232,14 @@ void UEventsManager::dispatchEvent(UEvent * event, const UEventsSender * sender)
 			{
 				// To be able to add/remove an handler in a handleEvent call (without a deadlock)
 				// @see _addHandler(), _removeHandler()
-				handler->handleEvent(event);
+				handled = handler->handleEvent(event);
 			}
 
 			handlersMutex_.lock();
 		}
 	}
 	handlersMutex_.unlock();
+	return handled;
 }
 
 void UEventsManager::_addHandler(UEventsHandler* handler)
@@ -309,8 +314,10 @@ void UEventsManager::_postEvent(UEvent * event, bool async, const UEventsSender
     	}
     	else
     	{
-    		dispatchEvent(event, sender);
+    		if(!dispatchEvent(event, sender))
-    		delete event;
+    		{
+    			delete event;
+    		}
     	}
     }
     else