Tango: 0.12.0 (performance optimization, time and memory)

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="36"
+        android:versionCode="37"
         android:versionName="@RTABMAP_VERSION@">
 
     <uses-permission android:name="android.permission.CAMERA" />

app/android/jni/CameraTango.cpp

@@ -40,7 +40,7 @@ namespace rtabmap {
 const int kVersionStringLength = 128;
 const int holeSize = 5;
 const float maxDepthError = 0.10;
-const int scanDownsampling = 10;
+const int scanDownsampling = 1;
 
 // Callbacks
 void onPointCloudAvailableRouter(void* context, const TangoPointCloud* point_cloud)
@@ -97,7 +97,10 @@ void onTangoEventAvailableRouter(void* context, const TangoEvent* event)
 //////////////////////////////
 // CameraTango
 //////////////////////////////
-CameraTango::CameraTango(int decimation, bool autoExposure, bool publishRawScan) :
+const float CameraTango::bilateralFilteringSigmaS = 2.0f;
+const float CameraTango::bilateralFilteringSigmaR = 0.075f;
+
+CameraTango::CameraTango(int decimation, bool autoExposure, bool publishRawScan, bool smoothing) :
 		Camera(0),
 		tango_config_(0),
 		firstFrame_(true),
@@ -105,6 +108,7 @@ CameraTango::CameraTango(int decimation, bool autoExposure, bool publishRawScan)
 		decimation_(decimation),
 		autoExposure_(autoExposure),
 		rawScanPublished_(publishRawScan),
+		smoothing_(smoothing),
 		cloudStamp_(0),
 		tangoColorType_(0),
 		tangoColorStamp_(0),
@@ -599,23 +603,41 @@ SensorData CameraTango::captureImage(CameraInfo * info)
 					scanData.at(oi++) = pt;
 				}
 
-				int pixel_x, pixel_y;
+				int pixel_x_l, pixel_y_l, pixel_x_h, pixel_y_h;
 				// get the coordinate on image plane.
-				pixel_x = static_cast<int>((depthModel.fx()) * (pt.x / pt.z) + depthModel.cx());
-				pixel_y = static_cast<int>((depthModel.fy()) * (pt.y / pt.z) + depthModel.cy());
+				pixel_x_l = static_cast<int>((depthModel.fx()) * (pt.x / pt.z) + depthModel.cx());
+				pixel_y_l = static_cast<int>((depthModel.fy()) * (pt.y / pt.z) + depthModel.cy());
+				pixel_x_h = static_cast<int>((depthModel.fx()) * (pt.x / pt.z) + depthModel.cx() + 0.5f);
+				pixel_y_h = static_cast<int>((depthModel.fy()) * (pt.y / pt.z) + depthModel.cy() + 0.5f);
 				unsigned short depth_value(pt.z * 1000.0f);
 
-				if(pixel_x>=0 && pixel_x<depth.cols &&
-				   pixel_y>0 && pixel_y<depth.rows &&
+				bool pixelSet = false;
+				if(pixel_x_l>=0 && pixel_x_l<depth.cols &&
+				   pixel_y_l>0 && pixel_y_l<depth.rows &&
 				   depth_value)
 				{
-					unsigned short & depthPixel = depth.at<unsigned short>(pixel_y, pixel_x);
+					unsigned short & depthPixel = depth.at<unsigned short>(pixel_y_l, pixel_x_l);
 					if(depthPixel == 0 || depthPixel > depth_value)
 					{
 						depthPixel = depth_value;
-						pixelsSet += 1;
+						pixelSet = true;
 					}
 				}
+				if(pixel_x_h>=0 && pixel_x_h<depth.cols &&
+				   pixel_y_h>0 && pixel_y_h<depth.rows &&
+				   depth_value)
+				{
+					unsigned short & depthPixel = depth.at<unsigned short>(pixel_y_h, pixel_x_h);
+					if(depthPixel == 0 || depthPixel > depth_value)
+					{
+						depthPixel = depth_value;
+						pixelSet = true;
+					}
+				}
+				if(pixelSet)
+				{
+					pixelsSet += 1;
+				}
 			}
 
 			if(oi)
@@ -690,6 +712,14 @@ SensorData CameraTango::captureImage(CameraInfo * info)
 				model.setImageSize(sizet);
 			}
 
+			if(smoothing_)
+			{
+				//UTimer t;
+				depth = rtabmap::util2d::fastBilateralFiltering(depth, bilateralFilteringSigmaS, bilateralFilteringSigmaR);
+				data.setDepthOrRightRaw(depth);
+				//LOGD("Bilateral filtering, time=%fs", t.ticks());
+			}
+
 			if(rawScanPublished_)
 			{
 				data = SensorData(scan, LaserScanInfo(cloud.total()/scanDownsampling, 0, model.localTransform()), rgb, depth, model, this->getNextSeqID(), rgbStamp);

app/android/jni/CameraTango.h

@@ -71,7 +71,11 @@ private:
 
 class CameraTango : public Camera, public UThread, public UEventsSender {
 public:
-	CameraTango(int decimation, bool autoExposure, bool publishRawScan);
+	static const float bilateralFilteringSigmaS;
+	static const float bilateralFilteringSigmaR;
+
+public:
+	CameraTango(int decimation, bool autoExposure, bool publishRawScan, bool smoothing);
 	virtual ~CameraTango();
 
 	virtual bool init(const std::string & calibrationFolder = ".", const std::string & cameraName = "");
@@ -81,6 +85,7 @@ public:
 	const CameraModel & getCameraModel() const {return model_;}
 	rtabmap::Transform tangoPoseToTransform(const TangoPoseData * tangoPose) const;
 	void setDecimation(int value) {decimation_ = value;}
+	void setSmoothing(bool enabled) {smoothing_ = enabled;}
 	void setAutoExposure(bool enabled) {autoExposure_ = enabled;}
 	void setRawScanPublished(bool enabled) {rawScanPublished_ = enabled;}
 	void setScreenRotation(TangoSupportRotation colorCameraToDisplayRotation) {colorCameraToDisplayRotation_ = colorCameraToDisplayRotation;}
@@ -107,6 +112,7 @@ private:
 	int decimation_;
 	bool autoExposure_;
 	bool rawScanPublished_;
+	bool smoothing_;
 	cv::Mat cloud_;
 	double cloudStamp_;
 	cv::Mat tangoColor_;

app/android/jni/RTABMapApp.cpp

@@ -148,6 +148,7 @@ RTABMapApp::RTABMapApp() :
 		trajectoryMode_(false),
 		autoExposure_(true),
 		rawScanSaved_(false),
+		smoothing_(true),
 		fullResolution_(false),
 		appendMode_(true),
 		maxCloudDepth_(0.0),
@@ -168,6 +169,8 @@ RTABMapApp::RTABMapApp() :
 		totalPolygons_(0),
 		lastDrawnCloudsCount_(0),
 		renderingTime_(0.0f),
+		processMemoryUsedBytes(0),
+		processGPUMemoryUsedBytes(0),
 		visualizingMesh_(false),
 		exportedMeshUpdated_(false),
 		exportedMesh_(new pcl::TextureMesh),
@@ -206,6 +209,8 @@ void RTABMapApp::onCreate(JNIEnv* env, jobject caller_activity)
 	totalPolygons_ = 0;
 	lastDrawnCloudsCount_ = 0;
 	renderingTime_ = 0.0f;
+	processMemoryUsedBytes = 0;
+	processGPUMemoryUsedBytes = 0;
 
 	if(camera_)
 	{
@@ -227,7 +232,7 @@ void RTABMapApp::onCreate(JNIEnv* env, jobject caller_activity)
 
 	this->registerToEventsManager();
 
-	camera_ = new rtabmap::CameraTango(fullResolution_?1:2, autoExposure_, rawScanSaved_);
+	camera_ = new rtabmap::CameraTango(fullResolution_?1:2, autoExposure_, rawScanSaved_, smoothing_);
 }
 
 void RTABMapApp::setScreenRotation(int displayRotation, int cameraRotation)
@@ -563,7 +568,7 @@ int RTABMapApp::Render()
 				renderingTime_ = fpsTime.elapsed();
 			}
 
-			return notifyCameraStarted;
+			return notifyCameraStarted?1:0;
 		}
 		else
 		{
@@ -613,6 +618,8 @@ int RTABMapApp::Render()
 				totalPolygons_ = 0;
 				lastDrawnCloudsCount_ = 0;
 				renderingTime_ = 0.0f;
+				processMemoryUsedBytes = 0;
+				processGPUMemoryUsedBytes = 0;
 			}
 
 			// Did we lose OpenGL context? If so, recreate the context;
@@ -622,6 +629,7 @@ int RTABMapApp::Render()
 				boost::mutex::scoped_lock  lock(meshesMutex_);
 				if(added.size() != createdMeshes_.size())
 				{
+					processGPUMemoryUsedBytes = 0;
 					for(std::map<int, Mesh>::iterator iter=createdMeshes_.begin(); iter!=createdMeshes_.end(); ++iter)
 					{
 						if(!main_scene_.hasCloud(iter->first))
@@ -633,10 +641,24 @@ int RTABMapApp::Render()
 							cv::Mat texture;
 							if(main_scene_.isMeshTexturing())
 							{
-								texture = rtabmap::uncompressImage(rtabmap_->getMemory()->getImageCompressed(iter->first));
+								cv::Mat textureRaw;
+								textureRaw = rtabmap::uncompressImage(rtabmap_->getMemory()->getImageCompressed(iter->first));
+								if(!textureRaw.empty())
+								{
+									cv::Size reducedSize(textureRaw.cols/(textureRaw.cols>1000?4:2), textureRaw.rows/(textureRaw.cols>1000?4:2));
+									LOGD("resize image from %dx%d to %dx%d", textureRaw.cols, textureRaw.rows, reducedSize.width, reducedSize.height);
+									cv::resize(textureRaw, texture, reducedSize, 0, 0, CV_INTER_AREA);
+								}
 							}
 							main_scene_.addMesh(iter->first, iter->second, texture, opengl_world_T_rtabmap_world*iter->second.pose);
 							main_scene_.setCloudVisible(iter->first, iter->second.visible);
+
+							long estimateGPUMem = 0;
+							estimateGPUMem += iter->second.cloud->size()*16; // 3*float + 1 float rgb
+							estimateGPUMem += iter->second.indices->size()*4; // int
+							estimateGPUMem += iter->second.polygons.size()*4*3; // 3 indices per polygon
+
+							processGPUMemoryUsedBytes += estimateGPUMem + (texture.empty()?0:iter->second.polygons.size()*3*8+texture.total());
 						}
 					}
 				}
@@ -659,11 +681,16 @@ int RTABMapApp::Render()
 						{
 							for(std::map<int, rtabmap::Signature>::const_iterator jter=iter->getSignatures().begin(); jter!=iter->getSignatures().end(); ++jter)
 							{
+								bool dataDetected = false;
 								if(!jter->second.sensorData().imageRaw().empty() &&
 								   !jter->second.sensorData().depthRaw().empty())
 								{
-									uInsert(bufferedSensorData, std::make_pair(jter->first, jter->second.sensorData()));
-									uInsert(rawPoses_, std::make_pair(jter->first, jter->second.getPose()));
+									if(!localizationMode_)
+									{
+										uInsert(bufferedSensorData, std::make_pair(jter->first, jter->second.sensorData()));
+										uInsert(rawPoses_, std::make_pair(jter->first, jter->second.getPose()));
+										dataDetected = true;
+									}
 								}
 								else if(totalPoints_ == 0 &&
 										!jter->second.sensorData().imageCompressed().empty() &&
@@ -676,6 +703,19 @@ int RTABMapApp::Render()
 										LOGI("Detecting that we are loading a database");
 									}
 									notifyDataLoaded = true;
+									dataDetected = true;
+								}
+								if(dataDetected)
+								{
+									processMemoryUsedBytes += jter->second.sensorData().imageCompressed().total();
+									processMemoryUsedBytes += jter->second.sensorData().depthOrRightCompressed().total();
+									processMemoryUsedBytes += jter->second.sensorData().laserScanCompressed().total();
+									processMemoryUsedBytes += jter->second.getWords().size()*4*8;
+									processMemoryUsedBytes += jter->second.getWords3().size()*4*4;
+									if(!jter->second.getWordsDescriptors().empty())
+									{
+										processMemoryUsedBytes += jter->second.getWordsDescriptors().size()*(4+jter->second.getWordsDescriptors().begin()->second.total());
+									}
 								}
 							}
 						}
@@ -755,15 +795,6 @@ int RTABMapApp::Render()
 								cv::Mat tmpA, depth;
 								data.uncompressData(&tmpA, &depth);
 
-								if(notifyDataLoaded && optimizeOpenedDatabase_)
-								{
-									// do post-processing bilateral filtering now
-									UTimer t;
-									depth = rtabmap::util2d::fastBilateralFiltering(depth, 2.0f, 0.075f);
-									data.setDepthOrRightRaw(depth);
-									LOGI("Bilateral filtering of %d, time=%fs", id, t.ticks());
-								}
-
 								if(!data.imageRaw().empty() && !data.depthRaw().empty())
 								{
 									// Voxelize and filter depending on the previous cloud?
@@ -795,7 +826,22 @@ int RTABMapApp::Render()
 											inserted.first->second.visible = true;
 											inserted.first->second.cameraModel = data.cameraModels()[0];
 											inserted.first->second.gain = 1.0f;
-											main_scene_.addMesh(id, inserted.first->second, main_scene_.isMeshTexturing()?data.imageRaw():cv::Mat(), iter->second);
+											cv::Mat texture;
+											if(main_scene_.isMeshTexturing())
+											{
+												cv::Size reducedSize(data.imageRaw().cols/(data.imageRaw().cols>1000?4:2), data.imageRaw().rows/(data.imageRaw().cols>1000?4:2));
+												LOGD("resize image from %dx%d to %dx%d", data.imageRaw().cols, data.imageRaw().rows, reducedSize.width, reducedSize.height);
+												cv::resize(data.imageRaw(), texture, reducedSize, 0, 0, CV_INTER_AREA);
+											}
+											main_scene_.addMesh(id, inserted.first->second, texture, iter->second);
+
+											long estimateCPUMem = 0;
+											estimateCPUMem += inserted.first->second.cloud->size()*16; // 3*float + 1 float rgb
+											estimateCPUMem += inserted.first->second.indices->size()*4; // int
+											estimateCPUMem += inserted.first->second.polygons.size()*4*3; // 3 indices per polygon
+
+											processMemoryUsedBytes += estimateCPUMem;
+											processGPUMemoryUsedBytes += estimateCPUMem + (texture.empty()?0:inserted.first->second.polygons.size()*3*8+texture.total());
 										}
 										else
 										{
@@ -1166,6 +1212,18 @@ void RTABMapApp::setFullResolution(bool enabled)
 	}
 }
 
+void RTABMapApp::setSmoothing(bool enabled)
+{
+	if(smoothing_ != enabled)
+	{
+		smoothing_ = enabled;
+		if(camera_)
+		{
+			camera_->setSmoothing(smoothing_);
+		}
+	}
+}
+
 void RTABMapApp::setAppendMode(bool enabled)
 {
 	if(appendMode_ != enabled)
@@ -1464,6 +1522,7 @@ cv::Mat RTABMapApp::mergeTextures(pcl::TextureMesh & mesh, int textureSize) cons
 bool RTABMapApp::exportMesh(
 		const std::string & filePath,
 		float cloudVoxelSize,
+		bool regenerateCloud,
 		bool meshing,
 		int textureSize,
 		int normalK,
@@ -2118,18 +2177,34 @@ bool RTABMapApp::exportMesh(
 				pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZRGB>);
 				pcl::IndicesPtr indices(new std::vector<int>);
 				float gain = 1.0f;
-				if(jter != createdMeshes_.end())
-				{
-					cloud = jter->second.cloud;
-					indices = jter->second.indices;
-					gain = jter->second.gain;
-				}
-				else
+				if(regenerateCloud)
 				{
+					if(jter != createdMeshes_.end())
+					{
+						gain = jter->second.gain;
+					}
 					rtabmap::SensorData data = rtabmap_->getMemory()->getNodeData(iter->first, true);
 					if(!data.imageRaw().empty() && !data.depthRaw().empty())
 					{
-						cloud = rtabmap::util3d::cloudRGBFromSensorData(data, meshDecimation_, maxCloudDepth_, 0, indices.get());
+						// full resolution
+						cloud = rtabmap::util3d::cloudRGBFromSensorData(data, 1, maxCloudDepth_, 0, indices.get());
+					}
+				}
+				else
+				{
+					if(jter != createdMeshes_.end())
+					{
+						cloud = jter->second.cloud;
+						indices = jter->second.indices;
+						gain = jter->second.gain;
+					}
+					else
+					{
+						rtabmap::SensorData data = rtabmap_->getMemory()->getNodeData(iter->first, true);
+						if(!data.imageRaw().empty() && !data.depthRaw().empty())
+						{
+							cloud = rtabmap::util3d::cloudRGBFromSensorData(data, meshDecimation_, maxCloudDepth_, 0, indices.get());
+						}
 					}
 				}
 				if(cloud->size() && indices->size())
@@ -2317,7 +2392,7 @@ int RTABMapApp::postProcessing(int approach)
 			}
 
 			// bilateral filtering
-			if(approach == -1 || approach == 7)
+			if(approach == 7)
 			{
 				bilateralFilteringOnNextRender_ = true;
 			}
@@ -2467,7 +2542,7 @@ void RTABMapApp::handleEvent(UEvent * event)
 				jclass clazz = env->GetObjectClass(RTABMapActivity);
 				if(clazz)
 				{
-					jmethodID methodID = env->GetMethodID(clazz, "updateStatsCallback", "(IIIIFIIIIIIFIFIFF)V" );
+					jmethodID methodID = env->GetMethodID(clazz, "updateStatsCallback", "(IIIIFIIIIIIIFIFIFF)V" );
 					if(methodID)
 					{
 						env->CallVoidMethod(RTABMapActivity, methodID,
@@ -2478,6 +2553,7 @@ void RTABMapApp::handleEvent(UEvent * event)
 								updateTime,
 								loopClosureId,
 								highestHypId,
+								(int)((processMemoryUsedBytes+processGPUMemoryUsedBytes)/(1024*1024)),
 								databaseMemoryUsed,
 								inliers,
 								matches,

app/android/jni/RTABMapApp.h

@@ -34,9 +34,9 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #include <tango_client_api.h>  // NOLINT
 #include <tango-gl/util.h>
 
-#include <scene.h>
-#include <CameraTango.h>
-#include <util.h>
+#include "scene.h"
+#include "CameraTango.h"
+#include "util.h"
 
 #include <rtabmap/core/RtabmapThread.h>
 #include <rtabmap/utilite/UEventsHandler.h>
@@ -128,6 +128,7 @@ class RTABMapApp : public UEventsHandler {
   void setAutoExposure(bool enabled);
   void setRawScanSaved(bool enabled);
   void setFullResolution(bool enabled);
+  void setSmoothing(bool enabled);
   void setAppendMode(bool enabled);
   void setDataRecorderMode(bool enabled);
   void setMaxCloudDepth(float value);
@@ -144,6 +145,7 @@ class RTABMapApp : public UEventsHandler {
   bool exportMesh(
 		  const std::string & filePath,
 		  float cloudVoxelSize,
+		  bool regenerateCloud,
 		  bool meshing,
 		  int textureSize,
 		  int normalK,
@@ -179,6 +181,7 @@ class RTABMapApp : public UEventsHandler {
   bool trajectoryMode_;
   bool autoExposure_;
   bool rawScanSaved_;
+  bool smoothing_;
   bool fullResolution_;
   bool appendMode_;
   float maxCloudDepth_;
@@ -202,6 +205,8 @@ class RTABMapApp : public UEventsHandler {
   int totalPolygons_;
   int lastDrawnCloudsCount_;
   float renderingTime_;
+  long processMemoryUsedBytes;
+  long processGPUMemoryUsedBytes;
 
   bool visualizingMesh_;
   bool exportedMeshUpdated_;

app/android/jni/jni_interface.cpp

@@ -217,6 +217,12 @@ Java_com_introlab_rtabmap_RTABMapLib_setFullResolution(
 	return app.setFullResolution(enabled);
 }
 JNIEXPORT void JNICALL
+Java_com_introlab_rtabmap_RTABMapLib_setSmoothing(
+		JNIEnv*, jobject, bool enabled)
+{
+	return app.setSmoothing(enabled);
+}
+JNIEXPORT void JNICALL
 Java_com_introlab_rtabmap_RTABMapLib_setAppendMode(
 		JNIEnv*, jobject, bool enabled)
 {
@@ -295,6 +301,7 @@ Java_com_introlab_rtabmap_RTABMapLib_exportMesh(
 		JNIEnv* env, jobject,
 		jstring filePath,
 		float cloudVoxelSize,
+		bool regenerateCloud,
 		bool meshing,
 		int textureSize,
 		int normalK,
@@ -313,6 +320,7 @@ Java_com_introlab_rtabmap_RTABMapLib_exportMesh(
 	return app.exportMesh(
 			filePathC,
 			cloudVoxelSize,
+			regenerateCloud,
 			meshing,
 			textureSize,
 			normalK,

app/android/jni/point_cloud_drawable.cpp

@@ -226,7 +226,7 @@ void PointCloudDrawable::updateMesh(const Mesh & mesh, const cv::Mat & texture)
 		}
 	}
 
-	LOGD("Creating cloud buffer %d", vertex_buffers_);
+	//LOGD("Creating cloud buffer %d", vertex_buffers_);
 	std::vector<float> vertices;
 	int totalPoints = 0;
 	std::vector<pcl::Vertices> polygons = mesh.polygons;
@@ -238,7 +238,7 @@ void PointCloudDrawable::updateMesh(const Mesh & mesh, const cv::Mat & texture)
 		totalPoints = mesh.indices->size();
 		if(textures_ && polygons.size())
 		{
-			LOGD("Organized mesh with texture");
+			//LOGD("Organized mesh with texture");
 			int items = hasNormals_?9:6;
 			vertices = std::vector<float>(mesh.indices->size()*9);
 			for(unsigned int i=0; i<mesh.indices->size(); ++i)
@@ -268,7 +268,7 @@ void PointCloudDrawable::updateMesh(const Mesh & mesh, const cv::Mat & texture)
 		}
 		else
 		{
-			LOGD("Organized mesh");
+			//LOGD("Organized mesh");
 			int items = hasNormals_?7:4;
 			vertices = std::vector<float>(mesh.indices->size()*items);
 			for(unsigned int i=0; i<mesh.indices->size(); ++i)
@@ -295,8 +295,8 @@ void PointCloudDrawable::updateMesh(const Mesh & mesh, const cv::Mat & texture)
 		totalPoints = mesh.cloud->size();
 		if(textures_ && polygons.size() && mesh.normals->size())
 		{
-			LOGD("Dense mesh with texture (%d texCoords %d points %d polygons %dx%d)",
-					(int)mesh.texCoords.size(), (int)mesh.cloud->size(), (int)mesh.polygons.size(), texture.cols, texture.rows);
+			//LOGD("Dense mesh with texture (%d texCoords %d points %d polygons %dx%d)",
+			//		(int)mesh.texCoords.size(), (int)mesh.cloud->size(), (int)mesh.polygons.size(), texture.cols, texture.rows);
 
 			// Texturing issue:
 			//  tex_coordinates should be linked to points, not
@@ -355,7 +355,7 @@ void PointCloudDrawable::updateMesh(const Mesh & mesh, const cv::Mat & texture)
 		}
 		else
 		{
-			LOGD("Dense mesh");
+			//LOGD("Dense mesh");
 			int items = hasNormals_?7:4;
 			organizedToDenseIndices_ = std::vector<unsigned int>(mesh.cloud->size(), -1);
 			vertices = std::vector<float>(mesh.cloud->size()*items);
@@ -392,9 +392,12 @@ void PointCloudDrawable::updateMesh(const Mesh & mesh, const cv::Mat & texture)
 
 	if(textures_ && textureUpdate)
 	{
-		GLint maxTextureSize = 0;
-		glGetIntegerv(GL_MAX_TEXTURE_SIZE, &maxTextureSize);
-		LOGI("maxTextureSize=%d", maxTextureSize);
+		//GLint maxTextureSize = 0;
+		//glGetIntegerv(GL_MAX_TEXTURE_SIZE, &maxTextureSize);
+		//LOGI("maxTextureSize=%d", maxTextureSize);
+		//GLint maxTextureUnits = 0;
+		//glGetIntegerv(GL_MAX_TEXTURE_IMAGE_UNITS, &maxTextureUnits);
+		//LOGW("maxTextureUnits=%d", maxTextureUnits);
 
 		// gen texture from image
 		glBindTexture(GL_TEXTURE_2D, textures_);
@@ -402,6 +405,11 @@ void PointCloudDrawable::updateMesh(const Mesh & mesh, const cv::Mat & texture)
 		glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_MAG_FILTER, GL_LINEAR);
 		cv::Mat rgbImage;
 		cv::cvtColor(texture, rgbImage, CV_BGR2RGB);
+
+		glPixelStorei(GL_UNPACK_ALIGNMENT, 1);
+		//glPixelStorei(GL_UNPACK_ROW_LENGTH, 0);
+		//glPixelStorei(GL_UNPACK_SKIP_PIXELS, 0);
+		//glPixelStorei(GL_UNPACK_SKIP_ROWS, 0);
 		glTexImage2D(GL_TEXTURE_2D, 0, GL_RGB, rgbImage.cols, rgbImage.rows, 0, GL_RGB, GL_UNSIGNED_BYTE, rgbImage.data);
 
 		GLint error = glGetError();

app/android/res/layout/activity_rtabmap.xml

@@ -23,263 +23,6 @@
         android:layout_height="fill_parent"
         android:layout_gravity="top" />
     
-    <LinearLayout
-        android:id="@+id/status_layout"
-        android:layout_width="wrap_content"
-        android:layout_height="wrap_content"
-        android:layout_alignParentTop="true"
-        android:layout_alignParentLeft="true"
-        android:orientation="vertical"
-        android:paddingLeft="5dp" >
-        
-        <LinearLayout
-            android:layout_width="wrap_content"
-            android:layout_height="wrap_content"
-            android:orientation="horizontal" >
-
-            <TextView
-                android:layout_width="wrap_content"
-                android:layout_height="wrap_content"
-                android:text="@string/status" />
-
-            <TextView
-                android:id="@+id/status"
-                android:layout_width="wrap_content"
-                android:layout_height="wrap_content" />
-        </LinearLayout>
-        
-        <LinearLayout
-            android:layout_width="wrap_content"
-            android:layout_height="wrap_content"
-            android:orientation="horizontal" >
-
-            <TextView
-                android:layout_width="wrap_content"
-                android:layout_height="wrap_content"
-                android:text="@string/memory" />
-
-            <TextView
-                android:id="@+id/memory"
-                android:layout_width="wrap_content"
-                android:layout_height="wrap_content" />
-        </LinearLayout>
-        
-        <LinearLayout
-            android:layout_width="wrap_content"
-            android:layout_height="wrap_content"
-            android:orientation="horizontal" >
-
-            <TextView
-                android:layout_width="wrap_content"
-                android:layout_height="wrap_content"
-                android:text="@string/free_memory" />
-
-            <TextView
-                android:id="@+id/free_memory"
-                android:layout_width="wrap_content"
-                android:layout_height="wrap_content" />
-        </LinearLayout>
-        
-   </LinearLayout>
-   <LinearLayout
-        android:id="@+id/debug_layout"
-        android:layout_width="wrap_content"
-        android:layout_height="wrap_content"
-        android:layout_alignParentLeft="true"
-        android:orientation="vertical"
-        android:layout_below="@+id/status_layout"
-        android:paddingLeft="5dp" >
-               
-        <LinearLayout
-            android:layout_width="wrap_content"
-            android:layout_height="wrap_content"
-            android:orientation="horizontal" >
-
-            <TextView
-                android:layout_width="wrap_content"
-                android:layout_height="wrap_content"
-                android:text="@string/nodes" />
-
-            <TextView
-                android:id="@+id/nodes"
-                android:layout_width="wrap_content"
-                android:layout_height="wrap_content" />
-        </LinearLayout>
-        
-        <LinearLayout
-            android:layout_width="wrap_content"
-            android:layout_height="wrap_content"
-            android:orientation="horizontal" >
-
-            <TextView
-                android:layout_width="wrap_content"
-                android:layout_height="wrap_content"
-                android:text="@string/words" />
-
-            <TextView
-                android:id="@+id/words"
-                android:layout_width="wrap_content"
-                android:layout_height="wrap_content" />
-        </LinearLayout>
-        
-        <LinearLayout
-            android:layout_width="wrap_content"
-            android:layout_height="wrap_content"
-            android:orientation="horizontal" >
-
-            <TextView
-                android:layout_width="wrap_content"
-                android:layout_height="wrap_content"
-                android:text="@string/database_size" />
-
-            <TextView
-                android:id="@+id/database_size"
-                android:layout_width="wrap_content"
-                android:layout_height="wrap_content" />
-        </LinearLayout>
-        
-        
-         <LinearLayout
-            android:layout_width="wrap_content"
-            android:layout_height="wrap_content"
-            android:orientation="horizontal" >
-
-            <TextView
-                android:layout_width="wrap_content"
-                android:layout_height="wrap_content"
-                android:text="@string/points" />
-
-            <TextView
-                android:id="@+id/points"
-                android:layout_width="wrap_content"
-                android:layout_height="wrap_content" />
-        </LinearLayout>
-        <LinearLayout
-            android:layout_width="wrap_content"
-            android:layout_height="wrap_content"
-            android:orientation="horizontal" >
-
-            <TextView
-                android:layout_width="wrap_content"
-                android:layout_height="wrap_content"
-                android:text="@string/polygons" />
-
-            <TextView
-                android:id="@+id/polygons"
-                android:layout_width="wrap_content"
-                android:layout_height="wrap_content" />
-        </LinearLayout>
-        
-         <LinearLayout
-            android:layout_width="wrap_content"
-            android:layout_height="wrap_content"
-            android:orientation="horizontal" >
-
-            <TextView
-                android:layout_width="wrap_content"
-                android:layout_height="wrap_content"
-                android:text="@string/update_time" />
-
-            <TextView
-                android:id="@+id/update_time"
-                android:layout_width="wrap_content"
-                android:layout_height="wrap_content" />
-        </LinearLayout>
-        <LinearLayout
-            android:layout_width="wrap_content"
-            android:layout_height="wrap_content"
-            android:orientation="horizontal" >
-
-            <TextView
-                android:layout_width="wrap_content"
-                android:layout_height="wrap_content"
-                android:text="@string/features" />
-
-            <TextView
-                android:id="@+id/features"
-                android:layout_width="wrap_content"
-                android:layout_height="wrap_content" />
-        </LinearLayout>
-        <LinearLayout
-            android:layout_width="wrap_content"
-            android:layout_height="wrap_content"
-            android:orientation="horizontal" >
-
-            <TextView
-                android:layout_width="wrap_content"
-                android:layout_height="wrap_content"
-                android:text="@string/rehearsal" />
-
-            <TextView
-                android:id="@+id/rehearsal"
-                android:layout_width="wrap_content"
-                android:layout_height="wrap_content" />
-        </LinearLayout>
-        <LinearLayout
-            android:layout_width="wrap_content"
-            android:layout_height="wrap_content"
-            android:orientation="horizontal" >
-
-            <TextView
-                android:layout_width="wrap_content"
-                android:layout_height="wrap_content"
-                android:text="@string/total_loop" />
-
-            <TextView
-                android:id="@+id/total_loop"
-                android:layout_width="wrap_content"
-                android:layout_height="wrap_content" />
-        </LinearLayout>
-
-        <LinearLayout
-            android:layout_width="wrap_content"
-            android:layout_height="wrap_content"
-            android:orientation="horizontal" >
-
-            <TextView
-                android:layout_width="wrap_content"
-                android:layout_height="wrap_content"
-                android:text="@string/inliers" />
-
-            <TextView
-                android:id="@+id/inliers"
-                android:layout_width="wrap_content"
-                android:layout_height="wrap_content" />
-
-        </LinearLayout>
-        <LinearLayout
-            android:layout_width="wrap_content"
-            android:layout_height="wrap_content"
-            android:orientation="horizontal" >
-
-            <TextView
-                android:layout_width="wrap_content"
-                android:layout_height="wrap_content"
-                android:text="@string/hypothesis" />
-
-            <TextView
-                android:id="@+id/hypothesis"
-                android:layout_width="wrap_content"
-                android:layout_height="wrap_content" />
-        </LinearLayout>
-        <LinearLayout
-            android:layout_width="wrap_content"
-            android:layout_height="wrap_content"
-            android:orientation="horizontal" >
-
-            <TextView
-                android:layout_width="wrap_content"
-                android:layout_height="wrap_content"
-                android:text="@string/fps" />
-
-            <TextView
-                android:id="@+id/fps"
-                android:layout_width="wrap_content"
-                android:layout_height="wrap_content" />
-        </LinearLayout>
-           
-    </LinearLayout>
-
     <ToggleButton
         android:id="@+id/backface_button"
         android:layout_width="100dp"

app/android/res/layout/activity_settings.xml

@@ -68,6 +68,11 @@
 	            android:title="@string/pref_title_resolution"
 	            android:summary="@string/pref_summary_resolution"
 	            android:defaultValue="@string/pref_default_resolution"/>
+		    <SwitchPreference
+		        android:key="@string/pref_key_smoothing"
+		        android:title="@string/pref_title_smoothing"
+		        android:summary="@string/pref_summary_smoothing"
+		        android:defaultValue="@string/pref_default_smoothing"/>
 	  
 		     <PreferenceCategory
 	        	android:title="@string/pref_title_mapping_core">

app/android/res/menu/optionmenu.xml

@@ -11,7 +11,12 @@
                      
        <item android:id="@+id/export" android:showAsAction="ifRoom" android:title="Export">
           <menu>
-              <item android:id="@+id/export_point_cloud" android:title="Point Cloud" />
+              <item android:id="@+id/export_point_cloud_menu" android:title="Point Cloud">
+                  <menu>
+                  	<item android:id="@+id/export_point_cloud" android:title="Current Density" />
+                  	<item android:id="@+id/export_point_cloud_highrez" android:title="Max Density" />
+                  </menu>
+              </item>
               <item android:id="@+id/export_mesh_menu" android:title="Raw Mesh..." >
 	              <menu>
 		              <item android:id="@+id/export_mesh" android:title="Colored Mesh" />
@@ -49,6 +54,7 @@
        <item android:id="@+id/menu_rendering_settings" android:title="Visibility...">
         <menu >
         <group android:id="@+id/group_rendering_visibility" android:checkableBehavior="all">
+            <item android:id="@+id/status" android:checked="true" android:title="Status" />
             <item android:id="@+id/debug" android:checked="false" android:title="Debug" />
             <item android:id="@+id/map_shown" android:checked="true" android:title="Map Visible" />
             <item android:id="@+id/odom_shown" android:checked="true" android:title="Odom Visible" />

app/android/res/values/strings.xml

@@ -18,7 +18,7 @@
     <string name="light_on">Lighting</string>
     <string name="light_off">Lighting</string>
     <string name="close_visualization">Close Visualization</string>
-    <string name="save_to_file">Save to File&#8230;</string>
+    <string name="save_to_file">Export to File&#8230;</string>
     <string name="share_to_sketchfab">Share to Sketchfab&#8230;</string>
     <string name="start">Start</string>
     <string name="nodes">"Nodes (WM): "</string>
@@ -30,22 +30,22 @@
     <string name="total_loop">"Loop closures: "</string>
     <string name="inliers">"Inliers: "</string>
     <string name="features">"Features: "</string>
-    <string name="rehearsal">"Rehearsal: "</string>
+    <string name="rehearsal">"Rehearsal (%): "</string>
     <string name="polygons">"Polygons: "</string>
     <string name="memory">"Used Memory (MB): "</string>
-    <string name="hypothesis">"Hypothesis: "</string>
+    <string name="hypothesis">"Hypothesis (%): "</string>
     <string name="fps">"FPS (rendering): "</string>
 
     <!-- Preference keys: BEGIN -->
     <string name="pref_key_tags">pref_key_tags</string>
-    <string name="pref_default_tags">rtabmap</string>
+    <string name="pref_default_tags">rtabmap 3dscan</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>
     <string name="pref_key_density">pref_key_density</string>
     <string name="pref_default_density">1</string>
     <string name="pref_key_depth">pref_key_depth</string>
-    <string name="pref_default_depth">0</string>
+    <string name="pref_default_depth">2.5</string>
     <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>
@@ -60,6 +60,8 @@
     <string name="pref_default_auto_exposure">true</string>
     <string name="pref_key_resolution">pref_key_resolution</string>
     <string name="pref_default_resolution">false</string>
+    <string name="pref_key_smoothing">pref_key_smoothing</string>
+    <string name="pref_default_smoothing">true</string>
     
     <string name="pref_key_update_rate">pref_key_update_rate</string>
     <string name="pref_default_update_rate">1</string>
@@ -93,7 +95,7 @@
     <string name="pref_default_db_in_memory">true</string>
     
     <string name="pref_key_cloud_voxel">pref_key_cloud_voxel</string>
-    <string name="pref_default_cloud_voxel">0.01</string>
+    <string name="pref_default_cloud_voxel">0</string>
     <string name="pref_key_texture_size">pref_key_texture_size</string>
     <string name="pref_default_texture_size">4096</string>
     <string name="pref_key_normal_k">pref_key_normal_k</string>
@@ -145,17 +147,25 @@
     <string-array name="pref_depth_keys">
         <item>"No Limit"</item>
         <item>"5"</item>
+        <item>"4.5"</item>
         <item>"4"</item>
+        <item>"3.5"</item>
         <item>"3"</item>
+        <item>"2.5"</item>
         <item>"2"</item>
+        <item>"1.5"</item>
         <item>"1"</item>
     </string-array>
     <string-array name="pref_depth_values">
         <item>"0"</item>
         <item>"5"</item>
+        <item>"4.5"</item>
         <item>"4"</item>
+        <item>"3.5"</item>
         <item>"3"</item>
+        <item>"2.5"</item>
         <item>"2"</item>
+        <item>"1.5"</item>
         <item>"1"</item>
     </string-array>
     <string-array name="pref_point_size_values">
@@ -208,6 +218,8 @@
     <string name="pref_summary_auto_exposure">Adjust camera exposure depending on the lighting to get always maximum contrast. This may change texture color between scanned images. Color correction option in Post-Processing can help to uniformize colors. May not work on some devices.</string>
     <string name="pref_title_resolution">HD Mode</string>
     <string name="pref_summary_resolution">Save HD images if you want very detailed textures. More memory will be required.</string>
+    <string name="pref_title_smoothing">Smoothing</string>
+    <string name="pref_summary_smoothing">Smooth the point clouds.</string>
     <string name="pref_title_update_rate">Update Rate</string>
     <string name="pref_summary_update_rate">Rate at which a new node is added to map.</string>
     <string name="pref_title_time_thr">Time Limit</string>
@@ -460,7 +472,7 @@
         <item>"0"</item>
     </string-array>
 
-    <string name="pref_title_export_sub">Exporting...</string>
+    <string name="pref_title_export_sub">Exporting&#8230;</string>
     <string name="pref_title_export">Exporting</string>
     <string name="pref_summary_export">Advanced parameters used when exporting the map.</string>
     <string name="pref_title_cloud_voxel">Voxel Size</string>

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

@@ -36,6 +36,10 @@ import android.content.pm.PackageManager;
 import android.content.pm.PackageManager.NameNotFoundException;
 import android.content.res.Configuration;
 import android.graphics.Bitmap;
+import android.graphics.Canvas;
+import android.graphics.Color;
+import android.graphics.Paint;
+import android.graphics.Typeface;
 import android.hardware.Camera;
 import android.graphics.Point;
 import android.hardware.display.DisplayManager;
@@ -52,6 +56,7 @@ import android.os.IBinder;
 import android.preference.PreferenceManager;
 import android.text.Editable;
 import android.text.InputType;
+import android.text.TextPaint;
 import android.util.Log;
 import android.view.Display;
 import android.view.Menu;
@@ -93,6 +98,7 @@ public class RTABMapActivity extends Activity implements OnClickListener {
 	public static final String RTABMAP_TMP_DB = "rtabmap.tmp.db";
 	public static final String RTABMAP_TMP_DIR = "tmp/";
 	public static final String RTABMAP_TMP_FILENAME = "map";
+	public static final String RTABMAP_SDCARD_PATH = "/sdcard/";
 
 	public static final String RTABMAP_AUTH_TOKEN_KEY = "com.introlab.rtabmap.AUTH_TOKEN";
 	public static final String RTABMAP_FILENAME_KEY = "com.introlab.rtabmap.FILENAME";
@@ -136,6 +142,8 @@ public class RTABMapActivity extends Activity implements OnClickListener {
 	private MenuItem mItemRenderingMesh;
 	private MenuItem mItemRenderingTextureMesh;
 	private MenuItem mItemDataRecorderMode;
+	private MenuItem mItemStatusVisibility;
+	private MenuItem mItemDebugVisibility;
 
 	private ToggleButton mButtonFirst;
 	private ToggleButton mButtonThird;
@@ -149,6 +157,7 @@ public class RTABMapActivity extends Activity implements OnClickListener {
 
 	private String mOpenedDatabasePath = "";
 	private String mWorkingDirectory = "";
+	private String mWorkingDirectoryHuman = "";
 
 	private String mUpdateRate;
 	private String mTimeThr;
@@ -157,15 +166,16 @@ public class RTABMapActivity extends Activity implements OnClickListener {
 	private String mMinInliers;
 	private String mMaxOptimizationError;
 
-	private LinearLayout mLayoutDebug;
-
 	private int mTotalLoopClosures = 0;
 	private boolean mMapIsEmpty = false;
 	private boolean mExportedOBJ = false;
 
 	private Toast mToast = null;
 	
+	private AlertDialog mMemoryWarningDialog = null;
+	
 	private Thread mMemStatusUpdateThread = null;
+	private String[] mStatusTexts = new String[16];
 
 	//Tango Service connection.
 	ServiceConnection mTangoServiceConnection = new ServiceConnection() {
@@ -254,12 +264,10 @@ public class RTABMapActivity extends Activity implements OnClickListener {
 		mRenderer = new Renderer(this);
 		mGLView.setRenderer(mRenderer);
 
-		mLayoutDebug = (LinearLayout) findViewById(R.id.debug_layout);
-		mLayoutDebug.setVisibility(LinearLayout.GONE);
-
 		mProgressDialog = new ProgressDialog(this);
 		mProgressDialog.setCanceledOnTouchOutside(false);
 		mRenderer.setProgressDialog(mProgressDialog);
+		mRenderer.setToast(mToast);
 
 		// Check if the Tango Core is out dated.
 		if (!CheckTangoCoreVersion(MIN_TANGO_CORE_VERSION)) {
@@ -270,6 +278,7 @@ public class RTABMapActivity extends Activity implements OnClickListener {
 
 		mOpenedDatabasePath = "";
 		mWorkingDirectory = "";
+		mWorkingDirectoryHuman = "";
 		mTotalLoopClosures = 0;
 
 		if(Environment.getExternalStorageState().compareTo(Environment.MEDIA_MOUNTED)==0)
@@ -278,6 +287,7 @@ public class RTABMapActivity extends Activity implements OnClickListener {
 			mWorkingDirectory = extStore.getAbsolutePath() + "/" + getString(R.string.app_name) + "/";
 			extStore = new File(mWorkingDirectory);
 			extStore.mkdirs();
+			mWorkingDirectoryHuman = RTABMAP_SDCARD_PATH + getString(R.string.app_name) + "/";
 		}
 		else
 		{
@@ -407,6 +417,7 @@ public class RTABMapActivity extends Activity implements OnClickListener {
 			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))));
 			RTABMapLib.setFullResolution(sharedPref.getBoolean(getString(R.string.pref_key_resolution), Boolean.parseBoolean(getString(R.string.pref_default_resolution))));
+			RTABMapLib.setSmoothing(sharedPref.getBoolean(getString(R.string.pref_key_smoothing), Boolean.parseBoolean(getString(R.string.pref_default_smoothing))));
 			RTABMapLib.setAppendMode(sharedPref.getBoolean(getString(R.string.pref_key_append), Boolean.parseBoolean(getString(R.string.pref_default_append))));
 			RTABMapLib.setMappingParameter("Rtabmap/DetectionRate", mUpdateRate);
 			RTABMapLib.setMappingParameter("Rtabmap/TimeThr", mTimeThr);
@@ -480,6 +491,12 @@ 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();
+		
 		RTABMapLib.setCamera(type);
 		mButtonFirst.setChecked(type==0);
 		mButtonThird.setChecked(type==1);
@@ -584,6 +601,8 @@ public class RTABMapActivity extends Activity implements OnClickListener {
 		mItemRenderingMesh = menu.findItem(R.id.mesh);
 		mItemRenderingTextureMesh = menu.findItem(R.id.texture_mesh);
 		mItemDataRecorderMode = menu.findItem(R.id.data_recorder);
+		mItemStatusVisibility = menu.findItem(R.id.status);
+		mItemDebugVisibility = menu.findItem(R.id.debug);
 		mItemSave.setEnabled(false);
 		mItemExport.setEnabled(false);
 		mItemOpen.setEnabled(false);
@@ -633,58 +652,107 @@ public class RTABMapActivity extends Activity implements OnClickListener {
 		activityManager.getMemoryInfo(mi);
 		return mi.availMem / 0x100000L; // MB
 	}
-
-	private void updateStatsUI(
-			int nodes, 
-			int words, 
-			int points, 
-			int polygons,
-			float updateTime, 
-			int loopClosureId,
-			int highestHypId,
-			int databaseMemoryUsed,
-			int inliers,
-			int matches,
-			int featuresExtracted,
-			float hypothesis,
-			int nodesDrawn,
-			float fps,
-			int rejected,
-			float rehearsalValue,
-			float optimizationMaxError)
+	
+	private void updateStatusTexts()
 	{
-		if(mButtonPause!=null)
+		if(mItemStatusVisibility != null && mItemDebugVisibility != null)
 		{
-			if(mButtonPause.isChecked())
+			if(mItemStatusVisibility.isChecked() && mItemDebugVisibility.isChecked())
 			{
-				((TextView)findViewById(R.id.status)).setText("Paused");
+				mRenderer.updateTexts(mStatusTexts);
+			}
+			else if(mItemStatusVisibility.isChecked())
+			{
+				mRenderer.updateTexts(Arrays.copyOfRange(mStatusTexts, 0, 3));
+			}
+			else if(mItemDebugVisibility.isChecked())
+			{
+				mRenderer.updateTexts(Arrays.copyOfRange(mStatusTexts, 4, mStatusTexts.length));
 			}
 			else
 			{
-				String updateValue = mUpdateRate.compareTo("0")==0?"Max":mUpdateRate;
-				((TextView)findViewById(R.id.status)).setText(mItemLocalizationMode.isChecked()?String.format("Localization (%s Hz)", updateValue):mItemDataRecorderMode.isChecked()?String.format("Recording (%s Hz)", updateValue):String.format("Mapping (%s Hz)", updateValue));
+				mRenderer.updateTexts(null);
+			}
+		}
+	}
+
+	private void updateStatsUI(
+			int processMemoryUsed,
+			int loopClosureId,
+			int inliers,
+			int matches,
+			int rejected,
+			float optimizationMaxError,
+			String[] statusTexts)
+	{
+		mStatusTexts = statusTexts;
+		updateStatusTexts();
+
+		if(mButtonPause!=null)
+		{
+			if(!mButtonPause.isChecked())
+			{	
+				//check if we are low in memory
+				long memoryUsed = processMemoryUsed;
+				long memoryFree = getFreeMemory();
+				
+				if(memoryFree < 100)
+				{
+					mButtonPause.setChecked(true);
+					pauseMapping();
+					
+					if(mMemoryWarningDialog!=null)
+					{
+						mMemoryWarningDialog.dismiss();
+						mMemoryWarningDialog = null;
+					}
+					
+					mMemoryWarningDialog = new AlertDialog.Builder(getActivity())
+					.setTitle("Memory is full!")
+					.setCancelable(false)
+					.setMessage(String.format("Scanning has been paused because free memory is too "
+							+ "low (%d MB). You should be able to save the database but some post-processing and exporting options may fail. "
+							+ "\n\nNote that for large environments, you can save multiple databases and "
+							+ "merge them with RTAB-Map Desktop version.", memoryUsed))
+					.setPositiveButton("Ok", new DialogInterface.OnClickListener() {
+						public void onClick(DialogInterface dialog, int which) {
+							mMemoryWarningDialog = null;
+						}
+					})
+					.create();
+					mMemoryWarningDialog.show();
+				}
+				else if(mMemoryWarningDialog == null && memoryUsed*3 > memoryFree && (mItemDataRecorderMode == null || !mItemDataRecorderMode.isChecked()))
+				{
+					mMemoryWarningDialog = new AlertDialog.Builder(getActivity())
+					.setTitle("Warning: Memory is almost full!")
+					.setCancelable(false)
+					.setMessage(String.format("Free memory (%d MB) should be at least 3 times the "
+							+ "memory used (%d MB) so that some post-processing and exporting options "
+							+ "have enough memory to work correctly. If you just want to save the database "
+							+ "after scanning, you can continue until the next warning.\n\n"
+							+ "Note that showing only point clouds reduces memory needed for rendering.", memoryFree, memoryUsed))
+					.setPositiveButton("Pause", new DialogInterface.OnClickListener() {
+						public void onClick(DialogInterface dialog, int which) {
+							mButtonPause.setChecked(true);
+							pauseMapping();
+						}
+					})
+					.setNeutralButton("Continue", new DialogInterface.OnClickListener() {
+						public void onClick(DialogInterface dialog, int which) {
+						}
+					})
+					.create();
+					mMemoryWarningDialog.show();
+				}
 			}
 		}
 
-		((TextView)findViewById(R.id.memory)).setText(String.valueOf(Debug.getNativeHeapAllocatedSize()/(1024*1024)));
-		((TextView)findViewById(R.id.free_memory)).setText(String.valueOf(getFreeMemory()));
-		((TextView)findViewById(R.id.points)).setText(String.valueOf(points));
-		((TextView)findViewById(R.id.polygons)).setText(String.valueOf(polygons));
-		((TextView)findViewById(R.id.nodes)).setText(String.format("%d (%d shown)", nodes, nodesDrawn));
-		((TextView)findViewById(R.id.words)).setText(String.valueOf(words));
-		((TextView)findViewById(R.id.database_size)).setText(String.valueOf(databaseMemoryUsed));
-		((TextView)findViewById(R.id.inliers)).setText(String.valueOf(inliers));
-		((TextView)findViewById(R.id.features)).setText(String.format("%d / %s", featuresExtracted, mMaxFeatures.compareTo("0")==0?"No Limit":mMaxFeatures.compareTo("-1")==0?"Disabled":mMaxFeatures));
-		((TextView)findViewById(R.id.rehearsal)).setText(String.format("%.3f", rehearsalValue));
-		((TextView)findViewById(R.id.update_time)).setText(String.format("%.3f / %s", updateTime, mTimeThr.compareTo("0")==0?"No Limit":mTimeThr));
-		((TextView)findViewById(R.id.hypothesis)).setText(String.format("%.3f / %s (%d)", hypothesis, mLoopThr, loopClosureId>0?loopClosureId:highestHypId));
-		((TextView)findViewById(R.id.fps)).setText(String.format("%.3f Hz", fps));
+		
 		if(mButtonPause!=null && !mButtonPause.isChecked())
 		{
 			if(loopClosureId > 0)
 			{
-				++mTotalLoopClosures;
-
 				mToast.setText(String.format("Loop closure detected! (%d/%d inliers)", inliers, matches));
 				mToast.show();
 			}
@@ -701,7 +769,6 @@ public class RTABMapActivity extends Activity implements OnClickListener {
 				mToast.show();
 			}
 		}
-		((TextView)findViewById(R.id.total_loop)).setText(String.valueOf(mTotalLoopClosures));
 	}
 
 	// called from jni
@@ -713,10 +780,11 @@ public class RTABMapActivity extends Activity implements OnClickListener {
 			final float updateTime, 
 			final int loopClosureId,
 			final int highestHypId,
+			final int processMemoryUsed,
 			final int databaseMemoryUsed,
 			final int inliers,
 			final int matches,
-			final int features,
+			final int featuresExtracted,
 			final float hypothesis,
 			final int nodesDrawn,
 			final float fps,
@@ -726,10 +794,57 @@ public class RTABMapActivity extends Activity implements OnClickListener {
 	{
 		Log.i(TAG, String.format("updateStatsCallback()"));
 
+		final String[] statusTexts = new String[16];
+		if(mButtonPause!=null)
+		{
+			if(mButtonPause.isChecked())
+			{
+				statusTexts[0] = getString(R.string.status)+"Paused";
+			}
+			else
+			{			
+				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);
+		}
+
+		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; 
+		statusTexts[2] = getString(R.string.free_memory)+getFreeMemory();
+	
+		
+		if(loopClosureId > 0)
+		{
+			++mTotalLoopClosures;
+		}
+		
+		int index = 4;
+		statusTexts[index++] = getString(R.string.nodes)+nodes+" (" + nodesDrawn + " shown)";
+		statusTexts[index++] = getString(R.string.words)+words;
+		statusTexts[index++] = getString(R.string.database_size)+databaseMemoryUsed;
+		statusTexts[index++] = getString(R.string.points)+points;
+		statusTexts[index++] = getString(R.string.polygons)+polygons;
+		statusTexts[index++] = getString(R.string.update_time)+(int)(updateTime) + " / " + (mTimeThr.compareTo("0")==0?"No Limit":mTimeThr);
+		statusTexts[index++] = getString(R.string.features)+featuresExtracted +" / " + (mMaxFeatures.compareTo("0")==0?"No Limit":mMaxFeatures.compareTo("-1")==0?"Disabled":mMaxFeatures);
+		statusTexts[index++] = getString(R.string.rehearsal)+(int)(rehearsalValue*100.0f);
+		statusTexts[index++] = getString(R.string.total_loop)+mTotalLoopClosures;
+		statusTexts[index++] = getString(R.string.inliers)+inliers;
+		statusTexts[index++] = getString(R.string.hypothesis)+(int)(hypothesis*100.0f) +" / " + (int)(Float.parseFloat(mLoopThr)*100.0f) + " (" + (loopClosureId>0?loopClosureId:highestHypId)+")";
+		statusTexts[index++] = getString(R.string.fps)+(int)fps+" Hz";
+	
 		runOnUiThread(new Runnable() {
-			public void run() {
-				updateStatsUI(nodes, words, points, polygons, updateTime, loopClosureId, highestHypId, databaseMemoryUsed, inliers, matches, features, hypothesis, nodesDrawn, fps, rejected, rehearsalValue, optimizationMaxError);
-			} 
+				public void run() {
+					updateStatsUI(adjustedMemoryUsed, loopClosureId, inliers, matches, rejected, optimizationMaxError, statusTexts);
+				} 
 		});
 	}
 
@@ -743,14 +858,16 @@ public class RTABMapActivity extends Activity implements OnClickListener {
 		{
 			if(mButtonPause.isChecked())
 			{
-				((TextView)findViewById(R.id.status)).setText(
-						status == 1 && msg.isEmpty()?"Paused":msg);
+				mStatusTexts[0] = getString(R.string.status)+(status == 1 && msg.isEmpty()?"Paused":msg);
 			}
 			else
 			{
-				((TextView)findViewById(R.id.status)).setText(
-						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()?(mItemLocalizationMode!=null&&mItemLocalizationMode.isChecked()?"Localization":mItemDataRecorderMode!=null&&mItemDataRecorderMode.isChecked()?"Recording":"Mapping"):msg);
 			}
+			
+			mStatusTexts[1] = getString(R.string.memory)+String.valueOf(Debug.getNativeHeapAllocatedSize()/(1024*1024));
+			mStatusTexts[2] = getString(R.string.free_memory)+String.valueOf(getFreeMemory());
+			updateStatusTexts();
 		}
 	}
 
@@ -901,6 +1018,47 @@ 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)
 	{	
@@ -920,32 +1078,7 @@ public class RTABMapActivity extends Activity implements OnClickListener {
 			mItemReset.setEnabled(false);
 			mItemModes.setEnabled(false);
 			mButtonPause.setVisibility(View.INVISIBLE);
-			if(mMemStatusUpdateThread == null)
-			{
-				mMemStatusUpdateThread = new Thread() {
-		
-		        	@Override
-		        	public void run() {
-		        		try {
-		        			while (!isInterrupted()) {
-		        				Thread.sleep(1000);
-		        				if(mState == RTABMapActivity.State.STATE_PROCESSING)
-		        				{
-			        				runOnUiThread(new Runnable() {
-			        					@Override
-			        					public void run() {
-			        						((TextView)findViewById(R.id.memory)).setText(String.valueOf(Debug.getNativeHeapAllocatedSize()/(1024*1024)));
-			        						((TextView)findViewById(R.id.free_memory)).setText(String.valueOf(getFreeMemory()));
-			        					}
-			        				});
-		        				}
-		        			}
-		        		} catch (InterruptedException e) {
-		        		}
-		        	}
-		        };
-		        mMemStatusUpdateThread.start();
-			}
+			startUpdateStatusThread();
 			break;
 		case STATE_VISUALIZING:
 			mButtonLighting.setVisibility(View.VISIBLE);
@@ -961,12 +1094,7 @@ public class RTABMapActivity extends Activity implements OnClickListener {
 			mItemModes.setEnabled(true);
 			mButtonPause.setVisibility(View.INVISIBLE);
 			mItemDataRecorderMode.setEnabled(mButtonPause.isChecked());
-			if(mMemStatusUpdateThread != null)
-			{
-				Thread tmp = mMemStatusUpdateThread;
-				mMemStatusUpdateThread = null;
-				tmp.interrupt();
-			}
+			stopUpdateStatusThread();
 			break;
 		default:
 			mButtonLighting.setVisibility(View.INVISIBLE);
@@ -983,12 +1111,7 @@ public class RTABMapActivity extends Activity implements OnClickListener {
 			mButtonPause.setVisibility(View.VISIBLE);
 			mItemDataRecorderMode.setEnabled(mButtonPause.isChecked());
 			RTABMapLib.postExportation(false);
-			if(mMemStatusUpdateThread != null)
-			{
-				Thread tmp = mMemStatusUpdateThread;
-				mMemStatusUpdateThread = null;
-				tmp.interrupt();
-			}
+			stopUpdateStatusThread();
 			break;
 		}
 	}
@@ -1000,11 +1123,17 @@ public class RTABMapActivity extends Activity implements OnClickListener {
 		if(mButtonPause.isChecked())
 		{
 			RTABMapLib.setPausedMapping(true);
-			((TextView)findViewById(R.id.status)).setText("Paused");
+			
+			mStatusTexts[0] = getString(R.string.status)+"Paused";
+			mStatusTexts[1] = getString(R.string.memory)+String.valueOf(Debug.getNativeHeapAllocatedSize()/(1024*1024));
+			mStatusTexts[2] = getString(R.string.free_memory)+String.valueOf(getFreeMemory());
+			updateStatusTexts();
+			
 			mMapIsEmpty = false;
 			mDateOnPause = new Date();
 
-			if(!mOnPause && !mItemLocalizationMode.isChecked() && !mItemDataRecorderMode.isChecked())
+			long memoryFree = getFreeMemory();
+			if(!mOnPause && !mItemLocalizationMode.isChecked() && !mItemDataRecorderMode.isChecked() && memoryFree >= 100)
 			{
 				// Do standard post processing?
 				new AlertDialog.Builder(getActivity())
@@ -1025,8 +1154,13 @@ public class RTABMapActivity extends Activity implements OnClickListener {
 		}
 		else
 		{
+			if(mMemoryWarningDialog != null)
+			{
+				mMemoryWarningDialog.dismiss();
+				mMemoryWarningDialog=null;
+			}
 			RTABMapLib.setPausedMapping(false);
-			((TextView)findViewById(R.id.status)).setText(mItemLocalizationMode.isChecked()?"Localization":mItemDataRecorderMode.isChecked()?"Recording":"Mapping");
+			
 			if(mItemDataRecorderMode.isChecked())
 			{
 				mToast.makeText(getActivity(), String.format("Data Recorder Mode: no map is created, only raw data is recorded."), mToast.LENGTH_LONG).show();
@@ -1154,17 +1288,15 @@ public class RTABMapActivity extends Activity implements OnClickListener {
 			});
 			workingThread.start();
 		}
+		else if(itemId == R.id.status)
+		{
+			item.setChecked(!item.isChecked());
+			updateStatusTexts();
+		}
 		else if(itemId == R.id.debug)
 		{
 			item.setChecked(!item.isChecked());
-			if(!item.isChecked())
-			{
-				mLayoutDebug.setVisibility(LinearLayout.GONE);
-			}
-			else
-			{
-				mLayoutDebug.setVisibility(LinearLayout.VISIBLE);
-			}
+			updateStatusTexts();
 		}
 		else if(itemId == R.id.mesh || itemId == R.id.texture_mesh || itemId == R.id.point_cloud)
 		{
@@ -1256,62 +1388,7 @@ public class RTABMapActivity extends Activity implements OnClickListener {
 							.setMessage("Do you want to overwrite the existing file?")
 							.setPositiveButton("Yes", new DialogInterface.OnClickListener() {
 								public void onClick(DialogInterface dialog, int which) {
-
-									final String newDatabasePath = mWorkingDirectory + fileName + ".db";
-									mProgressDialog.setTitle("Saving");
-									if(mOpenedDatabasePath.equals(newDatabasePath))
-									{
-										mProgressDialog.setMessage(String.format("Please wait while updating \"%s\"...", newDatabasePath));
-									}
-									else
-									{
-										mProgressDialog.setMessage(String.format("Please wait while saving \"%s\"...", newDatabasePath));
-									}
-									mProgressDialog.show();
-									updateState(State.STATE_PROCESSING);
-									Thread saveThread = new Thread(new Runnable() {
-										public void run() {
-											RTABMapLib.save(newDatabasePath); // save
-											runOnUiThread(new Runnable() {
-												public void run() {
-													if(mOpenedDatabasePath.equals(newDatabasePath))
-													{
-														mToast.makeText(getActivity(), String.format("Database \"%s\" updated.", newDatabasePath), mToast.LENGTH_LONG).show();
-													}
-													else
-													{
-														mToast.makeText(getActivity(), String.format("Database saved to \"%s\".", newDatabasePath), mToast.LENGTH_LONG).show();
-
-														Intent intent = new Intent(getActivity(), RTABMapActivity.class);
-														// use System.currentTimeMillis() to have a unique ID for the pending intent
-														PendingIntent pIntent = PendingIntent.getActivity(getActivity(), (int) System.currentTimeMillis(), intent, 0);
-
-														// build notification
-														// the addAction re-use the same intent to keep the example short
-														Notification n  = new Notification.Builder(getActivity())
-														.setContentTitle(getString(R.string.app_name))
-														.setContentText(newDatabasePath + " saved!")
-														.setSmallIcon(R.drawable.ic_launcher)
-														.setContentIntent(pIntent)
-														.setAutoCancel(true).build();
-
-
-														NotificationManager notificationManager = 
-																(NotificationManager) getSystemService(NOTIFICATION_SERVICE);
-
-														notificationManager.notify(0, n); 
-													}
-													if(!mItemDataRecorderMode.isChecked())
-													{
-														mOpenedDatabasePath = newDatabasePath;
-													}
-													mProgressDialog.dismiss();
-													updateState(State.STATE_IDLE);
-												}
-											});
-										} 
-									});
-									saveThread.start();
+									saveDatabase(fileName);
 								}
 							})
 							.setNegativeButton("No", new DialogInterface.OnClickListener() {
@@ -1323,61 +1400,7 @@ public class RTABMapActivity extends Activity implements OnClickListener {
 						}
 						else
 						{
-							final String newDatabasePath = mWorkingDirectory + fileName + ".db";
-							mProgressDialog.setTitle("Saving");
-							if(mOpenedDatabasePath.equals(newDatabasePath))
-							{
-								mProgressDialog.setMessage(String.format("Please wait while updating \"%s\"...", mOpenedDatabasePath));
-							}
-							else
-							{
-								mProgressDialog.setMessage(String.format("Please wait while saving \"%s\"...", newDatabasePath));
-							}
-							mProgressDialog.show();
-							updateState(State.STATE_PROCESSING);
-							Thread saveThread = new Thread(new Runnable() {
-								public void run() {
-									RTABMapLib.save(newDatabasePath); // save
-									runOnUiThread(new Runnable() {
-										public void run() {
-											if(mOpenedDatabasePath.equals(newDatabasePath))
-											{
-												mToast.makeText(getActivity(), String.format("Database \"%s\" updated.", newDatabasePath), mToast.LENGTH_LONG).show();
-											}
-											else
-											{
-												mToast.makeText(getActivity(), String.format("Database saved to \"%s\".", newDatabasePath), mToast.LENGTH_LONG).show();
-
-												Intent intent = new Intent(getActivity(), RTABMapActivity.class);
-												// use System.currentTimeMillis() to have a unique ID for the pending intent
-												PendingIntent pIntent = PendingIntent.getActivity(getActivity(), (int) System.currentTimeMillis(), intent, 0);
-
-												// build notification
-												// the addAction re-use the same intent to keep the example short
-												Notification n  = new Notification.Builder(getActivity())
-												.setContentTitle(getString(R.string.app_name))
-												.setContentText(newDatabasePath + " saved!")
-												.setSmallIcon(R.drawable.ic_launcher)
-												.setContentIntent(pIntent)
-												.setAutoCancel(true).build();
-
-
-												NotificationManager notificationManager = 
-														(NotificationManager) getSystemService(NOTIFICATION_SERVICE);
-
-												notificationManager.notify(0, n); 
-											}
-											if(!mItemDataRecorderMode.isChecked())
-											{
-												mOpenedDatabasePath = newDatabasePath;
-											}
-											mProgressDialog.dismiss();
-											updateState(State.STATE_IDLE);
-										}
-									});
-								} 
-							});
-							saveThread.start();
+							saveDatabase(fileName);
 						}
 					}
 				}
@@ -1388,17 +1411,22 @@ public class RTABMapActivity extends Activity implements OnClickListener {
 		}
 		else if(itemId == R.id.reset)
 		{
-			((TextView)findViewById(R.id.points)).setText(String.valueOf(0));
-			((TextView)findViewById(R.id.polygons)).setText(String.valueOf(0));
-			((TextView)findViewById(R.id.nodes)).setText(String.valueOf(0));
-			((TextView)findViewById(R.id.words)).setText(String.valueOf(0));
-			((TextView)findViewById(R.id.inliers)).setText(String.valueOf(0));
-			((TextView)findViewById(R.id.features)).setText(String.valueOf(0));
-			((TextView)findViewById(R.id.update_time)).setText(String.valueOf(0));
-			((TextView)findViewById(R.id.hypothesis)).setText(String.valueOf(0));
-			((TextView)findViewById(R.id.fps)).setText(String.valueOf(0));
 			mTotalLoopClosures = 0;
-			((TextView)findViewById(R.id.total_loop)).setText(String.valueOf(mTotalLoopClosures));
+			
+			int index = 4;
+			mStatusTexts[index++] = getString(R.string.nodes)+0;
+			mStatusTexts[index++] = getString(R.string.words)+0;
+			mStatusTexts[index++] = getString(R.string.database_size)+0;
+			mStatusTexts[index++] = getString(R.string.points)+0;
+			mStatusTexts[index++] = getString(R.string.polygons)+0;
+			mStatusTexts[index++] = getString(R.string.update_time)+0;
+			mStatusTexts[index++] = getString(R.string.features)+0;
+			mStatusTexts[index++] = getString(R.string.rehearsal)+0;
+			mStatusTexts[index++] = getString(R.string.total_loop)+0;
+			mStatusTexts[index++] = getString(R.string.inliers)+0;
+			mStatusTexts[index++] = getString(R.string.hypothesis)+0;
+			mStatusTexts[index++] = getString(R.string.fps)+0;
+			updateStatusTexts();
 
 			mOpenedDatabasePath = "";
 			SharedPreferences sharedPref = PreferenceManager.getDefaultSharedPreferences(this);
@@ -1422,17 +1450,21 @@ public class RTABMapActivity extends Activity implements OnClickListener {
 			.setPositiveButton("Yes", new DialogInterface.OnClickListener() {
 				public void onClick(DialogInterface dialog, int which) {           	  
 					// reset
-					((TextView)findViewById(R.id.points)).setText(String.valueOf(0));
-					((TextView)findViewById(R.id.polygons)).setText(String.valueOf(0));
-					((TextView)findViewById(R.id.nodes)).setText(String.valueOf(0));
-					((TextView)findViewById(R.id.words)).setText(String.valueOf(0));
-					((TextView)findViewById(R.id.inliers)).setText(String.valueOf(0));
-					((TextView)findViewById(R.id.features)).setText(String.valueOf(0));
-					((TextView)findViewById(R.id.update_time)).setText(String.valueOf(0));
-					((TextView)findViewById(R.id.hypothesis)).setText(String.valueOf(0));
-					((TextView)findViewById(R.id.fps)).setText(String.valueOf(0));
 					mTotalLoopClosures = 0;
-					((TextView)findViewById(R.id.total_loop)).setText(String.valueOf(mTotalLoopClosures));
+					int index = 4;
+					mStatusTexts[index++] = getString(R.string.nodes)+0;
+					mStatusTexts[index++] = getString(R.string.words)+0;
+					mStatusTexts[index++] = getString(R.string.database_size)+0;
+					mStatusTexts[index++] = getString(R.string.points)+0;
+					mStatusTexts[index++] = getString(R.string.polygons)+0;
+					mStatusTexts[index++] = getString(R.string.update_time)+0;
+					mStatusTexts[index++] = getString(R.string.features)+0;
+					mStatusTexts[index++] = getString(R.string.rehearsal)+0;
+					mStatusTexts[index++] = getString(R.string.total_loop)+0;
+					mStatusTexts[index++] = getString(R.string.inliers)+0;
+					mStatusTexts[index++] = getString(R.string.hypothesis)+0;
+					mStatusTexts[index++] = getString(R.string.fps)+0;
+					updateStatusTexts();
 
 					mItemDataRecorderMode.setChecked(!dataRecorderOldState);
 					RTABMapLib.setDataRecorderMode(mItemDataRecorderMode.isChecked());
@@ -1467,7 +1499,8 @@ public class RTABMapActivity extends Activity implements OnClickListener {
 			})
 			.show();
 		}
-		else if(itemId == R.id.export_point_cloud || 
+		else if(itemId == R.id.export_point_cloud ||
+				itemId == R.id.export_point_cloud_highrez ||
 				itemId == R.id.export_mesh ||
 				itemId == R.id.export_mesh_texture ||
 				itemId == R.id.export_optimized_mesh ||
@@ -1476,14 +1509,14 @@ public class RTABMapActivity extends Activity implements OnClickListener {
 			final boolean isOBJ = itemId == R.id.export_mesh_texture || itemId == R.id.export_optimized_mesh_texture;
 			final String extension = isOBJ? ".obj" : ".ply";
 
-			final int polygons = Integer.parseInt(((TextView)findViewById(R.id.polygons)).getText().toString());
-
-			final boolean meshing = itemId != R.id.export_point_cloud;
+			final boolean meshing = itemId != R.id.export_point_cloud && itemId != R.id.export_point_cloud_highrez;
+			final boolean regenerateCloud = itemId == R.id.export_point_cloud_highrez;
 			final boolean optimized = itemId == R.id.export_optimized_mesh || itemId == R.id.export_optimized_mesh_texture;
 
 			// get Export settings
 			SharedPreferences sharedPref = PreferenceManager.getDefaultSharedPreferences(this);
-			final float cloudVoxelSize = Float.parseFloat(sharedPref.getString(getString(R.string.pref_key_cloud_voxel), getString(R.string.pref_default_cloud_voxel)));
+			final String cloudVoxelSizeStr = sharedPref.getString(getString(R.string.pref_key_cloud_voxel), getString(R.string.pref_default_cloud_voxel));
+			final float cloudVoxelSize = Float.parseFloat(cloudVoxelSizeStr);
 			final int textureSize = isOBJ?Integer.parseInt(sharedPref.getString(getString(R.string.pref_key_texture_size), getString(R.string.pref_default_texture_size))):0;
 			final int normalK = Integer.parseInt(sharedPref.getString(getString(R.string.pref_key_normal_k), getString(R.string.pref_default_normal_k)));
 			final float maxTextureDistance = Float.parseFloat(sharedPref.getString(getString(R.string.pref_key_max_texture_distance), getString(R.string.pref_default_max_texture_distance)));
@@ -1511,6 +1544,7 @@ public class RTABMapActivity extends Activity implements OnClickListener {
 					final boolean success = RTABMapLib.exportMesh(
 							tmpPath,
 							cloudVoxelSize,
+							regenerateCloud,
 							meshing,
 							textureSize,
 							normalK,
@@ -1527,6 +1561,11 @@ public class RTABMapActivity extends Activity implements OnClickListener {
 						public void run() {
 							if(success)
 							{
+								if(!meshing && cloudVoxelSize>0.0f)
+								{
+									mToast.makeText(getActivity(), String.format("Cloud assembled and voxelized at %s m.", cloudVoxelSizeStr), mToast.LENGTH_LONG).show();
+								}
+								
 								// Visualize the result?
 								new AlertDialog.Builder(getActivity())
 								.setCancelable(false)
@@ -1563,6 +1602,10 @@ public class RTABMapActivity extends Activity implements OnClickListener {
 												saveOnDevice();
 											}
 										})
+										.setNeutralButton("Cancel", new DialogInterface.OnClickListener() {
+											public void onClick(DialogInterface dialog, int which) {
+											}
+										})
 										.show();
 									}
 								})
@@ -1596,11 +1639,11 @@ public class RTABMapActivity extends Activity implements OnClickListener {
 				builder.setTitle("Choose Your File (*.db)");
 				builder.setItems(filesWithSize, new DialogInterface.OnClickListener() {
 					public void onClick(DialogInterface dialog, final int which) {
-						
-						// Smooth and adjust color now?
+												
+						// Adjust color now?
 						new AlertDialog.Builder(getActivity())
 						.setTitle("Opening database...")
-						.setMessage("Do you want to smooth and adjust colors now?\nThis can be done later under Optimize menu.")
+						.setMessage("Do you want to adjust colors now?\nThis can be done later under Optimize menu.")
 						.setPositiveButton("Yes", new DialogInterface.OnClickListener() {
 							public void onClick(DialogInterface dialog, int whichIn) {
 								openDatabase(files[which], true);
@@ -1633,6 +1676,67 @@ public class RTABMapActivity extends Activity implements OnClickListener {
 		return true;
 	}
 	
+	private void saveDatabase(String fileName)
+	{
+		final String newDatabasePath = mWorkingDirectory + fileName + ".db";
+		final String newDatabasePathHuman = mWorkingDirectoryHuman + fileName + ".db";
+		mProgressDialog.setTitle("Saving");
+		if(mOpenedDatabasePath.equals(newDatabasePath))
+		{
+			mProgressDialog.setMessage(String.format("Please wait while updating \"%s\"...", newDatabasePathHuman));
+		}
+		else
+		{
+			mProgressDialog.setMessage(String.format("Please wait while saving \"%s\"...", newDatabasePathHuman));
+		}
+		mProgressDialog.show();
+		updateState(State.STATE_PROCESSING);
+		startUpdateStatusThread();
+		Thread saveThread = new Thread(new Runnable() {
+			public void run() {
+				RTABMapLib.save(newDatabasePath); // save
+				runOnUiThread(new Runnable() {
+					public void run() {
+						if(mOpenedDatabasePath.equals(newDatabasePath))
+						{
+							mToast.makeText(getActivity(), String.format("Database \"%s\" updated.", newDatabasePathHuman), mToast.LENGTH_LONG).show();
+						}
+						else
+						{
+							mToast.makeText(getActivity(), String.format("Database saved to \"%s\".", newDatabasePathHuman), mToast.LENGTH_LONG).show();
+
+							Intent intent = new Intent(getActivity(), RTABMapActivity.class);
+							// use System.currentTimeMillis() to have a unique ID for the pending intent
+							PendingIntent pIntent = PendingIntent.getActivity(getActivity(), (int) System.currentTimeMillis(), intent, 0);
+
+							// build notification
+							// the addAction re-use the same intent to keep the example short
+							Notification n  = new Notification.Builder(getActivity())
+							.setContentTitle(getString(R.string.app_name))
+							.setContentText(newDatabasePathHuman + " saved!")
+							.setSmallIcon(R.drawable.ic_launcher)
+							.setContentIntent(pIntent)
+							.setAutoCancel(true).build();
+
+
+							NotificationManager notificationManager = 
+									(NotificationManager) getSystemService(NOTIFICATION_SERVICE);
+
+							notificationManager.notify(0, n); 
+						}
+						if(!mItemDataRecorderMode.isChecked())
+						{
+							mOpenedDatabasePath = newDatabasePath;
+						}
+						mProgressDialog.dismiss();
+						updateState(State.STATE_IDLE);
+					}
+				});
+			} 
+		});
+		saveThread.start();
+	}
+	
 	private void saveOnDevice()
 	{
 		AlertDialog.Builder builder = new AlertDialog.Builder(this);
@@ -1705,6 +1809,7 @@ public class RTABMapActivity extends Activity implements OnClickListener {
 	{
 		final String extension  = mExportedOBJ?".obj":".ply";
 		final String path = mWorkingDirectory + fileName + extension;
+		final String pathHuman = mWorkingDirectoryHuman + fileName + extension;
 		
 		boolean success = true;
 		if(mExportedOBJ)
@@ -1726,28 +1831,28 @@ public class RTABMapActivity extends Activity implements OnClickListener {
 				copy(fromOBJFile,toOBJFile);
 				copy(fromMTLFile,toMTLFile);
 				copy(fromJPGFile,toJPGFile);
-				mToast.makeText(getActivity(), String.format("Mesh \"%s\" (with texture \"%s\" and \"%s\") successfully exported!", path, fileName + ".jpg", fileName + ".mtl"), mToast.LENGTH_LONG).show();
+				mToast.makeText(getActivity(), String.format("Mesh \"%s\" (with texture \"%s\" and \"%s\") successfully exported!", pathHuman, fileName + ".jpg", fileName + ".mtl"), mToast.LENGTH_LONG).show();
 			}
 			catch(IOException e)
 			{
-				mToast.makeText(getActivity(), String.format("Exporting mesh \"%s\" (with texture \"%s\" and \"%s\") failed! Error=%s", path, fileName + ".jpg", fileName + ".mtl", e.getMessage()), mToast.LENGTH_LONG).show();
+				mToast.makeText(getActivity(), String.format("Exporting mesh \"%s\" (with texture \"%s\" and \"%s\") failed! Error=%s", pathHuman, fileName + ".jpg", fileName + ".mtl", e.getMessage()), mToast.LENGTH_LONG).show();
 				success = false;
 			}
 		}
 		else
 		{
-			File toPLYFile = new File(mWorkingDirectory + fileName + extension);
+			File toPLYFile = new File(path);
 			toPLYFile.delete();
 			File fromPLYFile = new File(mWorkingDirectory + RTABMAP_TMP_DIR + RTABMAP_TMP_FILENAME + extension);
 			
 			try
 			{
 				copy(fromPLYFile,toPLYFile);
-				mToast.makeText(getActivity(), String.format("Mesh/point cloud \"%s\" successfully exported!", path), mToast.LENGTH_LONG).show();
+				mToast.makeText(getActivity(), String.format("Mesh/point cloud \"%s\" successfully exported!", pathHuman), mToast.LENGTH_LONG).show();
 			}
 			catch(Exception e)
 			{
-				mToast.makeText(getActivity(), String.format("Exporting mesh/point cloud \"%s\" failed! Error=%s", path, e.getMessage()), mToast.LENGTH_LONG).show();
+				mToast.makeText(getActivity(), String.format("Exporting mesh/point cloud \"%s\" failed! Error=%s", pathHuman, e.getMessage()), mToast.LENGTH_LONG).show();
 				success=false;
 			}
 		}
@@ -1762,7 +1867,7 @@ public class RTABMapActivity extends Activity implements OnClickListener {
 			// the addAction re-use the same intent to keep the example short
 			Notification n  = new Notification.Builder(getActivity())
 			.setContentTitle(getString(R.string.app_name))
-			.setContentText(path + " exported!")
+			.setContentText(pathHuman + " exported!")
 			.setSmallIcon(R.drawable.ic_launcher)
 			.setContentIntent(pIntent)
 			.setAutoCancel(true).build();
@@ -1775,19 +1880,11 @@ public class RTABMapActivity extends Activity implements OnClickListener {
 		}
 	}
 	
-	private void openDatabase(String fileName, boolean optimize)
+	private void openDatabase(final String fileName, final boolean optimize)
 	{
 		mOpenedDatabasePath = mWorkingDirectory + fileName;
-
-		if(!mItemTrajectoryMode.isChecked())
-		{
-			mProgressDialog.setTitle("Loading");
-			mProgressDialog.setMessage(String.format("Database \"%s\" loaded. Please wait while creating point clouds and meshes...", fileName));
-			mProgressDialog.show();
-			updateState(State.STATE_PROCESSING);
-		}
 		
-		String tmpDatabase = mWorkingDirectory+RTABMAP_TMP_DB;
+		final String tmpDatabase = mWorkingDirectory+RTABMAP_TMP_DB;
 		(new File(tmpDatabase)).delete();
 		try{
 			copy(new File(mOpenedDatabasePath), new File(tmpDatabase));
@@ -1798,32 +1895,62 @@ public class RTABMapActivity extends Activity implements OnClickListener {
 		}
 
 		SharedPreferences sharedPref = PreferenceManager.getDefaultSharedPreferences(getActivity());
-		boolean databaseInMemory = sharedPref.getBoolean(getString(R.string.pref_key_db_in_memory), Boolean.parseBoolean(getString(R.string.pref_default_db_in_memory)));		
-		int status = RTABMapLib.openDatabase(tmpDatabase, databaseInMemory, optimize);
-		setCamera(1);
-		updateState(State.STATE_IDLE);
+		final boolean databaseInMemory = sharedPref.getBoolean(getString(R.string.pref_key_db_in_memory), Boolean.parseBoolean(getString(R.string.pref_default_db_in_memory)));		
 		
-		if(status == -1)
-		{
-			new AlertDialog.Builder(getActivity())
-			.setCancelable(false)
-			.setTitle("Error")
-			.setMessage("The map is loaded but optimization of the map's graph has "
-					+ "failed, so the map cannot be shown. Change the Graph Optimizer approach used"
-					+ " or enable/disable if the graph is optimized from graph "
-					+ "end in \"Settings -> Mapping...\" and try opening again.")
-			.setPositiveButton("Open Settings", new DialogInterface.OnClickListener() {
-				public void onClick(DialogInterface dialog, int which) {
-					Intent intent = new Intent(getActivity(), SettingsActivity.class);
-					startActivity(intent);
-				}
-			})
-			.setNegativeButton("Close", new DialogInterface.OnClickListener() {
-				public void onClick(DialogInterface dialog, int which) {
-				}
-			})
-			.show();
-		}
+		
+		mProgressDialog.setTitle("Loading");
+		mProgressDialog.setMessage(String.format("Opening database \"%s\"...", fileName));
+		mProgressDialog.show();
+		updateState(State.STATE_PROCESSING);
+		
+		Thread openThread = new Thread(new Runnable() {
+			public void run() {
+
+				final int status = RTABMapLib.openDatabase(tmpDatabase, databaseInMemory, optimize);
+				
+				runOnUiThread(new Runnable() {
+					public void run() {
+						setCamera(1);
+						updateState(State.STATE_IDLE);
+						mProgressDialog.dismiss();
+						if(status == -1)
+						{
+							new AlertDialog.Builder(getActivity())
+							.setCancelable(false)
+							.setTitle("Error")
+							.setMessage("The map is loaded but optimization of the map's graph has "
+									+ "failed, so the map cannot be shown. Change the Graph Optimizer approach used"
+									+ " or enable/disable if the graph is optimized from graph "
+									+ "end in \"Settings -> Mapping...\" and try opening again.")
+							.setPositiveButton("Open Settings", new DialogInterface.OnClickListener() {
+								public void onClick(DialogInterface dialog, int which) {
+									Intent intent = new Intent(getActivity(), SettingsActivity.class);
+									startActivity(intent);
+								}
+							})
+							.setNegativeButton("Close", new DialogInterface.OnClickListener() {
+								public void onClick(DialogInterface dialog, int which) {
+								}
+							})
+							.show();
+						}
+						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.show();
+							}
+						}
+					}
+				});
+			} 
+		});
+		openThread.start();	
 	}
 	
 	public void copy(File src, File dst) throws IOException {

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

@@ -70,6 +70,7 @@ public class RTABMapLib
     public static native void setAutoExposure(boolean enabled);
     public static native void setRawScanSaved(boolean enabled);
     public static native void setFullResolution(boolean enabled);
+    public static native void setSmoothing(boolean enabled);
     public static native void setAppendMode(boolean enabled);
     public static native void setDataRecorderMode(boolean enabled);
     public static native void setMaxCloudDepth(float value);
@@ -88,6 +89,7 @@ public class RTABMapLib
     public static native boolean exportMesh(
     		String filePath,
     		float cloudVoxelSize,
+    		boolean regenerateCloud,
     		boolean meshing,
     		int textureSize,
     		int normalK,

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

@@ -16,10 +16,14 @@
 
 package com.introlab.rtabmap;
 
+import java.util.Vector;
+import java.util.concurrent.locks.ReentrantLock;
+
 import android.app.Activity;
 import android.app.ProgressDialog;
-import android.content.Context;
+import android.opengl.GLES20;
 import android.opengl.GLSurfaceView;
+import android.opengl.Matrix;
 import android.util.Log;
 import android.widget.Toast;
 
@@ -30,56 +34,161 @@ import javax.microedition.khronos.opengles.GL10;
 // ground grid, camera frustum, camera axis, and trajectory based on the Tango
 // device's pose.
 public class Renderer implements GLSurfaceView.Renderer {
-	
-	private static Activity mActivity;
-	  public Renderer(Activity c) {
-		  mActivity = c;
-	  }
 
-	private ProgressDialog mProgressDialog;
+	private final float[] mtrxProjection = new float[16];
+	private final float[] mtrxView = new float[16];
+	private final float[] mtrxProjectionAndView = new float[16];
+
+	private TextManager mTextManager = null;
+	private float mSurfaceHeight = 0.0f;
 	
+	private Vector<TextObject> mTexts;
+
+	private static RTABMapActivity mActivity;
+	public Renderer(RTABMapActivity c) {
+		mActivity = c;
+	}
+
+	private ProgressDialog mProgressDialog = null;
+	private Toast mToast = null;
+	
+	private boolean mTextChanged = false;
+	private ReentrantLock mTextLock = new ReentrantLock();
+
 	public void setProgressDialog(ProgressDialog progressDialog)
 	{
 		mProgressDialog = progressDialog;
 	}
-	
+
+	public void setToast(Toast toast)
+	{
+		mToast = toast;
+	}
+
 	// Render loop of the Gl context.
-	public void onDrawFrame(GL10 gl) {
+	public void onDrawFrame(GL10 useGLES20instead) {
+
 		try
 		{
 			final int value = RTABMapLib.render();
+
+			if(mTextManager!=null)
+			{
+				if(mTextChanged)
+				{
+					mTextChanged = false;
+					Vector<TextObject> txtcollection = new Vector<TextObject>();
+					
+					mTextLock.lock();
+				    try {
+				    	if(mTexts.size() > 0)
+				    	{
+				    		txtcollection.addAll(mTexts);
+				    	}
+				    } finally {
+				    	mTextLock.unlock();
+				    }
+				    
+					// Prepare the text for rendering
+					mTextManager.PrepareDraw(txtcollection);
+				}
+				
+				mTextManager.Draw(mtrxProjectionAndView);
+			}
+
 			mActivity.runOnUiThread(new Runnable() {
 				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)
+					if(value==-1 && mToast!=null)
 					{
-						Toast.makeText(mActivity, String.format("Out of Memory!"), Toast.LENGTH_LONG).show();
+						mToast.makeText(mActivity, String.format("Out of Memory!"), Toast.LENGTH_LONG).show();
 					}
 				} 
 			});
 		}
 		catch(final Exception e)
 		{
-			mActivity.runOnUiThread(new Runnable() {
-				public void run() {
-					Toast.makeText(mActivity, String.format("Rendering error! %s", e.getMessage()), Toast.LENGTH_LONG).show();
-				}
+			if(mToast!=null)
+			{
+				mActivity.runOnUiThread(new Runnable() {
+					public void run() {
+						mToast.makeText(mActivity, String.format("Rendering error! %s", e.getMessage()), Toast.LENGTH_LONG).show();
+					}
 
-			});
+				});
+			}
 		}
 	}
 
-  // Called when the surface size changes.
-  public void onSurfaceChanged(GL10 gl, int width, int height) {
-	  RTABMapLib.setupGraphic(width, height);
-  }
+	// Called when the surface size changes.
+	public void onSurfaceChanged(GL10 useGLES20instead, int width, int height) {
+		
+		RTABMapLib.setupGraphic(width, height);
+		
+		mSurfaceHeight = (float)height;
 
-  // Called when the surface is created or recreated.
-  public void onSurfaceCreated(GL10 gl, EGLConfig config) {
-	  RTABMapLib.initGlContent();
-  }
+		// Clear our matrices
+		for(int i=0;i<16;i++)
+		{
+			mtrxProjection[i] = 0.0f;
+			mtrxView[i] = 0.0f;
+			mtrxProjectionAndView[i] = 0.0f;
+		}
+
+		// Setup our screen width and height for normal sprite translation.
+		Matrix.orthoM(mtrxProjection, 0, 0f, width, 0.0f, height, 0, 50);
+
+		// Set the camera position (View matrix)
+		Matrix.setLookAtM(mtrxView, 0, 0f, 0f, 1f, 0f, 0f, 0f, 0f, 1.0f, 0.0f);
+
+		// Calculate the projection and view transformation
+		Matrix.multiplyMM(mtrxProjectionAndView, 0, mtrxProjection, 0, mtrxView, 0);
+	}
+
+	// Called when the surface is created or recreated.
+	public void onSurfaceCreated(GL10 useGLES20instead, EGLConfig config) {
+
+		RTABMapLib.initGlContent();
+		
+		// Create our text manager
+		mTextManager = new TextManager(mActivity);
+		
+		GLES20.glEnable(GLES20.GL_BLEND);
+	    GLES20.glBlendFunc(GLES20.GL_ONE, GLES20.GL_ONE_MINUS_SRC_ALPHA);
+	}
+	
+	public void updateTexts(String[] texts)
+	{
+		if(mTextManager != null && mSurfaceHeight > 0.0f)
+		{
+			Vector<TextObject> textObjects = new Vector<TextObject>();
+			float offset = mSurfaceHeight-mTextManager.getMaxTextHeight();
+			if(texts != null)
+			{
+				for(int i=0;i<texts.length; ++i)
+				{
+					if(texts[i]!=null && texts[i].length()>0)
+					{
+						TextObject txt = new TextObject(texts[i], 0, offset);
+						textObjects.add(txt);
+					}
+					offset-=mTextManager.getMaxTextHeight();
+				}
+			}
+	
+			mTextLock.lock();
+			try {
+				mTexts = textObjects;
+			} finally {
+				mTextLock.unlock();
+			}
+	
+			mTextChanged = true;
+		}
+	}
 }

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

@@ -303,8 +303,7 @@ public class SketchfabActivity extends Activity implements OnClickListener {
 							{
 								Log.i(RTABMapActivity.TAG, String.format("Zipped files = %d MB", fileSizeMB));
 								builder.setMessage(String.format("Total size to upload = %d MB. %sDo you want to continue?\n\n"
-										+ "Tip: To reduce the model size, try \"Optimized Mesh\" export. "
-										+ "You can also look at the Settings->Exporting options to reduce the output size.", fileSizeMB,
+										+ "Tip: To reduce the model size, you can also look at the Settings->Exporting options to reduce the output size.", fileSizeMB,
 										fileSizeMB>=50?"Note that for size over 50 MB, a Sketchfab PRO account is required, otherwise the upload may fail. ":""));
 							}
 
@@ -448,7 +447,7 @@ public class SketchfabActivity extends Activity implements OnClickListener {
 				multipart.addFormField("description", mDescription.getText().toString());
 				multipart.addFormField("tags", mTags.getText().toString());
 				multipart.addFormField("source", "RTAB-Map");
-				multipart.addFormField("isPublished", mDraft.isChecked()?"true":"false");  				
+				multipart.addFormField("isPublished", mDraft.isChecked()?"false":"true");	
 				multipart.addFilePart("modelFile", uploadFile);
 
 				Log.i(RTABMapActivity.TAG, "Starting multipart request");

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

@@ -0,0 +1,443 @@
+//package ri.blog.opengl008;
+package com.introlab.rtabmap;
+
+import java.nio.ByteBuffer;
+import java.nio.ByteOrder;
+import java.nio.FloatBuffer;
+import java.nio.ShortBuffer;
+import java.util.Iterator;
+import java.util.Vector;
+
+import android.content.Context;
+import android.graphics.Bitmap;
+import android.graphics.Canvas;
+import android.graphics.Color;
+import android.graphics.Rect;
+import android.graphics.Typeface;
+import android.opengl.GLES20;
+import android.opengl.GLUtils;
+import android.text.TextPaint;
+
+public class TextManager {
+	
+	/* SHADER Text
+	 * 
+	 * This shader is for rendering 2D text textures straight from a texture
+	 * Color and alpha blended.
+	 * 
+	 */
+	public static final String vs_Text =
+		"uniform mat4 uMVPMatrix;" +
+		"attribute vec4 vPosition;" +
+		"attribute vec4 a_Color;" +
+		"attribute vec2 a_texCoord;" +
+		"varying vec4 v_Color;" + 
+		"varying vec2 v_texCoord;" +
+	    "void main() {" +
+	    "  gl_Position = uMVPMatrix * vPosition;" +
+	    "  v_texCoord = a_texCoord;" +
+	    "  v_Color = a_Color;" + 
+	    "}";
+	public static final String fs_Text =
+	    "precision mediump float;" +
+	    "varying vec4 v_Color;" +
+	    "varying vec2 v_texCoord;" +
+        "uniform sampler2D s_texture;" +
+	    "void main() {" +
+	    "  gl_FragColor = texture2D( s_texture, v_texCoord ) * v_Color;" +
+	    "  gl_FragColor.rgb *= v_Color.a;" +
+	    "}";
+	
+	public static int sp_Text;
+	
+	public static final int RI_TEXT_TEXTURE_SIZE = 512; // 512
+	public static final float RI_TEXT_HEIGHT_BASE = 32.0f;
+	public static final char RI_TEXT_START = ' ';
+	public static final char RI_TEXT_STOP = '~'+1;
+	
+	public float getMaxTextHeight() {return mTextHeight;}
+	
+	private float mUVWidth;
+	private float mUVHeight;
+	private float mTextHeight;
+	
+	private FloatBuffer vertexBuffer;
+	private FloatBuffer textureBuffer;
+	private FloatBuffer colorBuffer;
+	private ShortBuffer drawListBuffer;
+	
+	private float[] vecs;
+	private float[] uvs;
+	private short[] indices;
+	private float[] colors;
+	
+	private int index_vecs;
+	private int index_indices;
+	private int index_uvs;
+	private int index_colors;
+
+	private int texturenr;
+	private int[] mTextures;
+	
+	private float uniformscale = 1.0f;
+	
+	float[] mCharacterWidth;
+	
+	public TextManager(Context context)
+	{
+		// Create the arrays
+		vecs = new float[3 * 10];
+		colors = new float[4 * 10];
+		uvs = new float[2 * 10];
+		indices = new short[10];
+		
+		// init as 0 as default
+		texturenr = 0;
+		
+		// Text shader
+		int vshadert = TextManager.loadShader(GLES20.GL_VERTEX_SHADER, TextManager.vs_Text);
+		int fshadert = TextManager.loadShader(GLES20.GL_FRAGMENT_SHADER, TextManager.fs_Text);
+
+		TextManager.sp_Text = GLES20.glCreateProgram();
+		GLES20.glAttachShader(TextManager.sp_Text, vshadert);
+		GLES20.glAttachShader(TextManager.sp_Text, fshadert); 		// add the fragment shader to program
+		GLES20.glLinkProgram(TextManager.sp_Text);                  // creates OpenGL ES program executables
+		
+		// Generate Textures, if more needed, alter these numbers.
+		mTextures = new int[1];
+		GLES20.glGenTextures(1, mTextures, 0);
+		
+		// Create an empty, mutable bitmap
+		Bitmap bitmap = Bitmap.createBitmap(RI_TEXT_TEXTURE_SIZE, RI_TEXT_TEXTURE_SIZE, Bitmap.Config.ARGB_4444);
+		// get a canvas to paint over the bitmap
+		Canvas canvas = new Canvas(bitmap);
+		bitmap.eraseColor(0);
+
+		// Draw the text
+		TextPaint textPaint = new TextPaint();
+		textPaint.setTextSize(RI_TEXT_HEIGHT_BASE);
+		textPaint.setColor(Color.WHITE);
+		textPaint.setAntiAlias(true);
+		textPaint.setTypeface(Typeface.create("Arial", Typeface.BOLD));
+		
+		// compute real maximum height
+		mTextHeight=0.0f;
+		for(char c=RI_TEXT_START; c<RI_TEXT_STOP; ++c)
+		{
+			Rect textBounds = new Rect();
+			textPaint.getTextBounds(String.valueOf(c), 0, 1, textBounds);
+			if(textBounds.height() + textBounds.bottom > mTextHeight)
+			{
+				mTextHeight = textBounds.height() + textBounds.bottom;
+			}
+		}
+		mUVWidth = (float)RI_TEXT_HEIGHT_BASE/(float)RI_TEXT_TEXTURE_SIZE;
+		mUVHeight = mTextHeight/(float)RI_TEXT_TEXTURE_SIZE;
+		
+		int colCount = RI_TEXT_TEXTURE_SIZE/(int)RI_TEXT_HEIGHT_BASE;
+		mCharacterWidth = new float[RI_TEXT_STOP-RI_TEXT_START];
+		int i=0;
+		for(char c=RI_TEXT_START; c<RI_TEXT_STOP; ++c)
+		{
+			Rect textBounds = new Rect();
+			textPaint.getTextBounds(String.valueOf(c), 0, 1, textBounds);
+			canvas.drawText(String.valueOf(c), (i%colCount)*RI_TEXT_HEIGHT_BASE, (i/colCount) * mTextHeight + RI_TEXT_HEIGHT_BASE, textPaint);
+			mCharacterWidth[i] = textPaint.measureText(String.valueOf(c));
+			++i;
+		}
+		
+		GLES20.glBindTexture(GLES20.GL_TEXTURE_2D, mTextures[0]);
+		GLES20.glTexParameteri(GLES20.GL_TEXTURE_2D, GLES20.GL_TEXTURE_MIN_FILTER, GLES20.GL_LINEAR);
+        GLES20.glTexParameteri(GLES20.GL_TEXTURE_2D, GLES20.GL_TEXTURE_MAG_FILTER, GLES20.GL_LINEAR);
+        GLUtils.texImage2D(GLES20.GL_TEXTURE_2D, 0, bitmap, 0);
+		
+		//Clean up
+		bitmap.recycle();
+	}
+	
+	public void setTextureID(int val)
+	{
+		texturenr = val;
+	}
+	
+	public static int loadShader(int type, String shaderCode){
+
+	    // create a vertex shader type (GLES20.GL_VERTEX_SHADER)
+	    // or a fragment shader type (GLES20.GL_FRAGMENT_SHADER)
+	    int shader = GLES20.glCreateShader(type);
+
+	    // add the source code to the shader and compile it
+	    GLES20.glShaderSource(shader, shaderCode);
+	    GLES20.glCompileShader(shader);
+	    
+	    // return the shader
+	    return shader;
+	}
+	
+	
+	public void AddCharRenderInformation(float[] vec, float[] cs, float[] uv, short[] indi)
+	{
+		// We need a base value because the object has indices related to 
+		// that object and not to this collection so basicly we need to 
+		// translate the indices to align with the vertexlocation in ou
+		// vecs array of vectors.
+		short base = (short) (index_vecs / 3);
+			
+		// We should add the vec, translating the indices to our saved vector
+		for(int i=0;i<vec.length;i++)
+		{
+			vecs[index_vecs] = vec[i];
+			index_vecs++;
+		}
+		
+		// We should add the colors, so we can use the same texture for multiple effects.
+		for(int i=0;i<cs.length;i++)
+		{
+			colors[index_colors] = cs[i];
+			index_colors++;
+		}
+		
+		// We should add the uvs
+		for(int i=0;i<uv.length;i++)
+		{
+			uvs[index_uvs] = uv[i];
+			index_uvs++;
+		}
+
+		// We handle the indices
+		for(int j=0;j<indi.length;j++)
+		{
+			indices[index_indices] = (short) (base + indi[j]);
+			index_indices++;
+		}
+	}
+
+	public void PrepareDrawInfo(Vector<TextObject> txtcollection)
+	{
+		// Reset the indices.
+		index_vecs = 0;
+		index_indices = 0;
+		index_uvs = 0;
+		index_colors = 0;
+		
+		// Get the total amount of characters
+		int charcount = 0;
+		for (TextObject txt : txtcollection) {
+			if(txt!=null)
+			{
+				if(!(txt.text==null))
+				{
+					charcount += txt.text.length(); 
+				}
+			}
+		}
+		
+		// Create the arrays we need with the correct size.
+		vecs = null;
+		colors = null;
+		uvs = null;
+		indices = null;
+		
+		vecs = new float[charcount * 12];
+		colors = new float[charcount * 16];
+		uvs = new float[charcount * 8];
+		indices = new short[charcount * 6];
+
+	}
+	
+	public void PrepareDraw(Vector<TextObject> txtcollection)
+	{
+		// Setup all the arrays
+		PrepareDrawInfo(txtcollection);
+		
+		// Using the iterator protects for problems with concurrency
+		for( Iterator< TextObject > it = txtcollection.iterator(); it.hasNext() ; )
+	    {
+	    	TextObject txt = it.next();
+	    	if(txt!=null)
+			{
+		    	if(!(txt.text==null))
+				{
+					convertTextToTriangleInfo(txt);
+				}
+			}
+	    }
+	}
+	
+	public void Draw(float[] m)
+	{
+		if(vecs.length > 0)
+		{
+			GLES20.glDisable(GLES20.GL_DEPTH_TEST);
+			
+			// Set the correct shader for our grid object.
+			GLES20.glUseProgram(sp_Text);
+			
+			// The vertex buffer.
+			ByteBuffer bb = ByteBuffer.allocateDirect(vecs.length * 4);
+			bb.order(ByteOrder.nativeOrder());
+			vertexBuffer = bb.asFloatBuffer();
+			vertexBuffer.put(vecs);
+			vertexBuffer.position(0);
+			
+			// The vertex buffer.
+			ByteBuffer bb3 = ByteBuffer.allocateDirect(colors.length * 4);
+			bb3.order(ByteOrder.nativeOrder());
+			colorBuffer = bb3.asFloatBuffer();
+			colorBuffer.put(colors);
+			colorBuffer.position(0);
+			
+			// The texture buffer
+			ByteBuffer bb2 = ByteBuffer.allocateDirect(uvs.length * 4);
+			bb2.order(ByteOrder.nativeOrder());
+			textureBuffer = bb2.asFloatBuffer();
+			textureBuffer.put(uvs);
+			textureBuffer.position(0);
+			
+			// initialize byte buffer for the draw list
+			ByteBuffer dlb = ByteBuffer.allocateDirect(indices.length * 2);
+			dlb.order(ByteOrder.nativeOrder());
+			drawListBuffer = dlb.asShortBuffer();
+			drawListBuffer.put(indices);
+			drawListBuffer.position(0);
+			
+			// get handle to vertex shader's vPosition member
+		    int mPositionHandle = GLES20.glGetAttribLocation(sp_Text, "vPosition");
+		    
+		    // Enable a handle to the triangle vertices
+		    GLES20.glEnableVertexAttribArray(mPositionHandle);
+		 
+		    // Prepare the background coordinate data
+		    GLES20.glVertexAttribPointer(mPositionHandle, 3,
+		                                 GLES20.GL_FLOAT, false,
+		                                 0, vertexBuffer);
+		    
+		    int mTexCoordLoc = GLES20.glGetAttribLocation(sp_Text, "a_texCoord" );
+		    
+		    // Prepare the texturecoordinates
+		    GLES20.glVertexAttribPointer ( mTexCoordLoc, 2, GLES20.GL_FLOAT,
+	                false, 
+	                0, textureBuffer);
+	
+		    GLES20.glEnableVertexAttribArray ( mPositionHandle );
+	        GLES20.glEnableVertexAttribArray ( mTexCoordLoc );
+	
+	        int mColorHandle = GLES20.glGetAttribLocation(sp_Text, "a_Color");
+	
+		    // Enable a handle to the triangle vertices
+		    GLES20.glEnableVertexAttribArray(mColorHandle);
+	
+		    // Prepare the background coordinate data
+		    GLES20.glVertexAttribPointer(mColorHandle, 4,
+		                                 GLES20.GL_FLOAT, false,
+		                                 0, colorBuffer);
+	
+		    // get handle to shape's transformation matrix
+	        int mtrxhandle = GLES20.glGetUniformLocation(sp_Text, "uMVPMatrix");
+	        
+	    	// Apply the projection and view transformation
+	        GLES20.glUniformMatrix4fv(mtrxhandle, 1, false, m, 0);
+	        
+	        int mSamplerLoc = GLES20.glGetUniformLocation (sp_Text, "s_texture" );
+	        
+	        // Texture activate unit 0
+	        GLES20.glActiveTexture(GLES20.GL_TEXTURE0);
+			// Bind the texture to this unit.
+	        GLES20.glBindTexture(GLES20.GL_TEXTURE_2D, mTextures[0]);
+	        // Set the sampler texture unit to our selected id
+		    GLES20.glUniform1i ( mSamplerLoc, texturenr);
+		    
+	        // Draw the triangle
+	        GLES20.glDrawElements(GLES20.GL_TRIANGLES, indices.length, GLES20.GL_UNSIGNED_SHORT, drawListBuffer);
+	
+	        // Disable vertex array
+	        GLES20.glDisableVertexAttribArray(mPositionHandle);
+	        GLES20.glDisableVertexAttribArray(mTexCoordLoc);
+	        GLES20.glDisableVertexAttribArray(mColorHandle);
+		}
+	}
+	
+	private void convertTextToTriangleInfo(TextObject val)
+	{		
+		// Get attributes from text object
+		float x = val.x;
+		float y = val.y;
+		String text = val.text;
+		
+		// Create 
+		for(int j=0; j<text.length(); j++)
+		{
+			// get ascii value
+			char c = text.charAt(j);
+			int c_val = (int)c;
+			
+			int indx = c_val-RI_TEXT_START;
+			
+			if(indx<0 || indx>=mCharacterWidth.length) {
+				// unknown character, we will add a space for it to be save.
+				indx = 0;
+			}
+			
+			int colCount = RI_TEXT_TEXTURE_SIZE/(int)RI_TEXT_HEIGHT_BASE;
+			
+			// Calculate the uv parts
+			int row = indx / colCount;
+			int col = indx % colCount;
+			
+			float v = row * mUVHeight;
+			float v2 = v + mUVHeight;
+			float u = col * mUVWidth;
+			float u2 = u + mCharacterWidth[indx]/(float)RI_TEXT_TEXTURE_SIZE;
+			
+			// Creating the triangle information
+			float[] vec = new float[12];
+			float[] uv = new float[8];
+			float[] colors = new float[16];
+			
+			vec[0] = x;
+			vec[1] = y + (mTextHeight * uniformscale);
+			vec[2] = 0.99f;
+			vec[3] = x;
+			vec[4] = y;
+			vec[5] = 0.99f;
+			vec[6] = x + (mCharacterWidth[indx] * uniformscale);
+			vec[7] = y;
+			vec[8] = 0.99f;
+			vec[9] = x + (mCharacterWidth[indx] * uniformscale);
+			vec[10] = y + (mTextHeight * uniformscale);
+			vec[11] = 0.99f;
+		
+			colors = new float[]
+				    	{val.color[0], val.color[1], val.color[2], val.color[3],
+						val.color[0], val.color[1], val.color[2], val.color[3],
+						val.color[0], val.color[1], val.color[2], val.color[3],
+						val.color[0], val.color[1], val.color[2], val.color[3]
+				    			  		           };
+			// 0.001f = texture bleeding hack/fix
+			uv[0] = u+0.001f;
+			uv[1] = v+0.001f;
+			uv[2] = u+0.001f;
+			uv[3] = v2-0.001f;
+			uv[4] = u2-0.001f;
+			uv[5] = v2-0.001f;
+			uv[6] = u2-0.001f;
+			uv[7] = v+0.001f;
+			
+			short[] inds = {0, 1, 2, 0, 2, 3};
+			
+			// Add our triangle information to our collection for 1 render call.
+			AddCharRenderInformation(vec, colors, uv, inds);
+			
+			// Calculate the new position
+			x += (mCharacterWidth[indx]  * uniformscale);
+		}
+	}
+
+	public float getUniformscale() {
+		return uniformscale;
+	}
+
+	public void setUniformscale(float uniformscale) {
+		this.uniformscale = uniformscale;
+	}
+}

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

@@ -0,0 +1,33 @@
+//package ri.blog.opengl008;
+package com.introlab.rtabmap;
+
+public class TextObject {
+	
+	public String text;
+	public float x;
+	public float y;
+	public float[] color;
+	
+	public TextObject()
+	{
+		text = "default";
+		x = 0f;
+		y = 0f;
+		color = new float[] {1f, 1f, 1f, 1.0f};
+	}
+	
+	public TextObject(String txt, float xcoord, float ycoord)
+	{
+		text = txt;
+		x = xcoord;
+		y = ycoord;
+		color = new float[] {1f, 1f, 1f, 1.0f};
+	}
+	
+	/*public boolean validate()
+	{
+		if(text.compareTo("")==0) return false;
+		
+		return true;
+	}*/
+}

corelib/src/DBDriverSqlite3.cpp

@@ -1992,7 +1992,7 @@ void DBDriverSqlite3::loadSignaturesQuery(const std::list<int> & ids, std::list<
 
 		// Prepare the query... Get the map from signature and visual words
 		std::stringstream query2;
-		if(uStrNumCmp(_version, "0.11.15") >= 0)
+		if(uStrNumCmp(_version, "0.12.0") >= 0)
 		{
 			query2 << "SELECT word_id, pos_x, pos_y, size, dir, response, octave, depth_x, depth_y, depth_z, descriptor_size, descriptor "
 					 "FROM Map_Node_Word "
@@ -2045,7 +2045,7 @@ void DBDriverSqlite3::loadSignaturesQuery(const std::list<int> & ids, std::list<
 				kpt.size = sqlite3_column_int(ppStmt, index++);
 				kpt.angle = sqlite3_column_double(ppStmt, index++);
 				kpt.response = sqlite3_column_double(ppStmt, index++);
-				if(uStrNumCmp(_version, "0.11.15") >= 0)
+				if(uStrNumCmp(_version, "0.12.0") >= 0)
 				{
 					kpt.octave = sqlite3_column_int(ppStmt, index++);
 				}
@@ -3867,7 +3867,7 @@ void DBDriverSqlite3::stepWordsChanged(sqlite3_stmt * ppStmt, int nodeId, int ol
 
 std::string DBDriverSqlite3::queryStepKeypoint() const
 {
-	if(uStrNumCmp(_version, "0.11.15") >= 0)
+	if(uStrNumCmp(_version, "0.12.0") >= 0)
 	{
 		return "INSERT INTO Map_Node_Word(node_id, word_id, pos_x, pos_y, size, dir, response, octave, depth_x, depth_y, depth_z, descriptor_size, descriptor) VALUES(?,?,?,?,?,?,?,?,?,?,?,?,?);";
 	}

corelib/src/Memory.cpp

@@ -2106,6 +2106,7 @@ void Memory::removeLink(int oldId, int newId)
 
 void Memory::removeRawData(int id, bool image, bool scan, bool userData)
 {
+	UDEBUG("id=%d image=%d scan=%d userData=%d", id, image?1:0, scan?1:0, userData?1:0);
 	Signature * s = this->_getSignature(id);
 	if(s)
 	{