bump 0.20.18

* add missing curl install to focal dockerfile

* add missing apt-get update

.github/workflows/docker.yml

@@ -11,7 +11,7 @@ jobs:
     
     strategy:
       matrix:
-        docker_tag: [xenial, bionic, focal, android23, android24, android26]
+        docker_tag: [xenial, bionic, focal, android23, android24, android26, android30]
         include:
         - docker_tag: xenial
           docker_tags: |
@@ -56,6 +56,12 @@ jobs:
           docker_platforms: |
             linux/amd64
           docker_path: 'bionic/android/rtabmap_api26'
+        - docker_tag: android30
+          docker_tags: |
+            introlab3it/rtabmap:android30
+          docker_platforms: |
+            linux/amd64
+          docker_path: 'bionic/android/rtabmap_api30'
     
     steps:
       -

CMakeLists.txt

@@ -20,7 +20,7 @@ SET(CMAKE_MODULE_PATH "${PROJECT_SOURCE_DIR}/cmake_modules")
 #######################
 SET(RTABMAP_MAJOR_VERSION 0)
 SET(RTABMAP_MINOR_VERSION 20)
-SET(RTABMAP_PATCH_VERSION 16)
+SET(RTABMAP_PATCH_VERSION 18)
 SET(RTABMAP_VERSION
   ${RTABMAP_MAJOR_VERSION}.${RTABMAP_MINOR_VERSION}.${RTABMAP_PATCH_VERSION})
   
@@ -433,16 +433,29 @@ IF(WITH_DC1394)
 ENDIF(WITH_DC1394)
 
 IF(WITH_G2O)
-	FIND_PACKAGE(g2o QUIET NO_MODULE)
+	  FIND_PACKAGE(g2o QUIET NO_MODULE)
     IF(g2o_FOUND)
        MESSAGE(STATUS "Found g2o (targets)")
-	   SET(G2O_FOUND ${g2o_FOUND})
+	     SET(G2O_FOUND ${g2o_FOUND})
-	   SET(G2O_CPP11 1)
+	     get_target_property(G2O_INCLUDES g2o::core INTERFACE_INCLUDE_DIRECTORIES)
+	     MESSAGE(STATUS "g2o include dir: ${G2O_INCLUDES}")
+	     FIND_FILE(G2O_FACTORY_FILE g2o/core/factory.h 
+         PATHS ${G2O_INCLUDES}
+         NO_DEFAULT_PATH)
+       FILE(READ ${G2O_FACTORY_FILE} TMPTXT)
+       STRING(FIND "${TMPTXT}" "shared_ptr" matchres)
+       IF(${matchres} EQUAL -1)
+           MESSAGE(STATUS "Old g2o factory version detected without shared ptr (factory file: ${G2O_FACTORY_FILE}).")
+           SET(G2O_CPP11 2)
+       ELSE()
+           MESSAGE(STATUS "Latest g2o factory version detected with shared ptr (factory file: ${G2O_FACTORY_FILE}).")
+           SET(G2O_CPP11 1)
+       ENDIF()
     ELSE()
-       FIND_PACKAGE(G2O QUIET)
+        FIND_PACKAGE(G2O QUIET)
-       IF(G2O_FOUND)
+        IF(G2O_FOUND)
-		 MESSAGE(STATUS "Found g2o: ${G2O_INCLUDE_DIRS}")
+            MESSAGE(STATUS "Found g2o: ${G2O_INCLUDE_DIRS}")
-       ENDIF(G2O_FOUND)
+        ENDIF(G2O_FOUND)
     ENDIF()
 ENDIF(WITH_G2O)
 
@@ -475,7 +488,7 @@ ENDIF(WITH_POINTMATCHER)
 IF(libpointmatcher_FOUND OR GTSAM_FOUND)
     find_package(Boost COMPONENTS thread filesystem system program_options date_time REQUIRED)
     IF(Boost_MINOR_VERSION GREATER 47)
-        find_package(Boost COMPONENTS thread filesystem system program_options date_time chrono timer REQUIRED)
+        find_package(Boost COMPONENTS thread filesystem system program_options date_time chrono timer serialization REQUIRED)
     ENDIF(Boost_MINOR_VERSION GREATER 47)
     IF(WIN32)
 	    MESSAGE(STATUS "Boost_LIBRARY_DIRS=${Boost_LIBRARY_DIRS}")
@@ -804,6 +817,7 @@ ENDIF()
 
 IF(NOT G2O_FOUND)
    SET(G2O "//")
+   SET(G2O_CPP_CONF "//")
 ELSE()
    SET(CONF_DEPENDENCIES ${CONF_DEPENDENCIES} ${G2O_LIBRARIES})
    IF(NOT G2O_CPP11)

README.md

@@ -7,7 +7,7 @@ rtabmap
 [![License][license-image]][license]
 Linux: [![Build Status](https://github.com/introlab/rtabmap/actions/workflows/cmake.yml/badge.svg)](https://github.com/introlab/rtabmap/actions/workflows/cmake.yml) [![docker](https://github.com/introlab/rtabmap/actions/workflows/docker.yml/badge.svg)](https://github.com/introlab/rtabmap/actions/workflows/docker.yml) Windows: [![Build status](https://ci.appveyor.com/api/projects/status/hr73xspix9oqa26h/branch/master?svg=true)](https://ci.appveyor.com/project/matlabbe/rtabmap/branch/master)
 
-[release-image]: https://img.shields.io/badge/release-0.20.8-green.svg?style=flat
+[release-image]: https://img.shields.io/badge/release-0.20.16-green.svg?style=flat
 [releases]: https://github.com/introlab/rtabmap/releases
 
 [license-image]: https://img.shields.io/badge/license-BSD-green.svg?style=flat

Version.h.in

@@ -40,7 +40,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 @NONFREE@#define RTABMAP_NONFREE
 @TORO@#define RTABMAP_TORO
 @G2O@#define RTABMAP_G2O
-@G2O_CPP_CONF@#define RTABMAP_G2O_CPP11
+@G2O_CPP_CONF@#define RTABMAP_G2O_CPP11 @G2O_CPP11@
 @GTSAM@#define RTABMAP_GTSAM
 @CERES@#define RTABMAP_CERES
 @VERTIGO@#define RTABMAP_VERTIGO

app/android/AndroidManifest.xml.in

@@ -8,6 +8,7 @@
     <uses-permission android:name="android.permission.CAMERA" />
     <uses-permission android:name="android.permission.READ_EXTERNAL_STORAGE" />
     <uses-permission android:name="android.permission.WRITE_EXTERNAL_STORAGE" />
+    <uses-permission android:name="android.permission.ACCESS_MEDIA_LOCATION" />
     <uses-permission android:name="android.permission.INTERNET" />
     <uses-permission android:name="android.permission.ACCESS_NETWORK_STATE" />
     <uses-permission android:name="android.permission.ACCESS_FINE_LOCATION" />
@@ -17,6 +18,11 @@
 
     <!-- This is the platform API where depth16 support in android was introduced. -->
     <uses-sdk android:minSdkVersion="@ANDROID_NATIVE_API_LEVEL@" />
+    
+    <queries>
+        <package android:name="com.google.ar.core" />
+        <package android:name="com.huawei.ar.engine" /> 
+    </queries>
 
     <!-- This .apk has no Java code itself, so set hasCode to false. -->
     <application 
@@ -42,7 +48,16 @@
             <intent-filter>
                 <action android:name="android.intent.action.MAIN" />
                 <category android:name="android.intent.category.LAUNCHER" />
-            </intent-filter>
+		    </intent-filter>
+		    <intent-filter>
+		        <action android:name="android.intent.action.SEND" />
+		        <action android:name="android.intent.action.SEND_MULTIPLE" />
+		        <action android:name="android.intent.action.OPEN_DOCUMENT" />
+		        <category android:name="android.intent.category.DEFAULT" />
+		        <data android:mimeType="application/octet-stream" />
+		        <data android:pathPattern=".*\.db" />
+		    </intent-filter>
+		    
         </activity>
 
         <activity android:name="SettingsActivity" android:label="@string/settings" android:screenOrientation="fullSensor"/>

app/android/jni/CameraARCore.cpp

@@ -520,11 +520,11 @@ SensorData CameraARCore::captureImage(CameraInfo * info)
 							cv::Mat yuv(height+height/2, width, CV_8UC1);
 							memcpy(yuv.data, plane_data, data_length);
 							memcpy(yuv.data+data_length, plane_uv_data, height/2*width);
-							cv::cvtColor(yuv, rgb, CV_YUV2BGR_NV21);
+							cv::cvtColor(yuv, rgb, cv::COLOR_YUV2BGR_NV21);
 						}
 						else
 						{
-							cv::cvtColor(cv::Mat(height+height/2, width, CV_8UC1, (void*)plane_data), rgb, CV_YUV2BGR_NV21);
+							cv::cvtColor(cv::Mat(height+height/2, width, CV_8UC1, (void*)plane_data), rgb, cv::COLOR_YUV2BGR_NV21);
 						}
 
 						std::vector<cv::KeyPoint> kpts;

app/android/jni/CameraAREngine.cpp

@@ -244,7 +244,7 @@ SensorData CameraAREngine::captureImage(CameraInfo * info)
 				cv::Mat outputRGB;
 				if(imageData != nullptr && len>0)
 				{
-					cv::cvtColor(cv::Mat(height+height/2, width, CV_8UC1, (void*)imageData), outputRGB, CV_YUV2BGR_NV21);
+					cv::cvtColor(cv::Mat(height+height/2, width, CV_8UC1, (void*)imageData), outputRGB, cv::COLOR_YUV2BGR_NV21);
 				}
 
 				//Depth

app/android/jni/CameraMobile.cpp

@@ -171,7 +171,7 @@ void CameraMobile::setData(const SensorData & data, const Transform & pose, cons
     if(textureId_ != 0 && texCoord != 0)
     {
         cv::Mat rgbImage;
-        cv::cvtColor(data.imageRaw(), rgbImage, CV_BGR2RGBA);
+        cv::cvtColor(data.imageRaw(), rgbImage, cv::COLOR_BGR2RGBA);
         
         glBindTexture(GL_TEXTURE_2D, textureId_);
         glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_MIN_FILTER, GL_LINEAR);

app/android/jni/CameraTango.cpp

@@ -478,15 +478,15 @@ void CameraTango::cloudReceived(const cv::Mat & cloud, double timestamp)
 				LOGD("tangoColorType=%d", tangoColorType);
 				if(tangoColorType == TANGO_HAL_PIXEL_FORMAT_RGBA_8888)
 				{
-					cv::cvtColor(tangoImage, rgb, CV_RGBA2BGR);
+					cv::cvtColor(tangoImage, rgb, cv::COLOR_RGBA2BGR);
 				}
 				else if(tangoColorType == TANGO_HAL_PIXEL_FORMAT_YV12)
 				{
-					cv::cvtColor(tangoImage, rgb, CV_YUV2BGR_YV12);
+					cv::cvtColor(tangoImage, rgb, cv::COLOR_YUV2BGR_YV12);
 				}
 				else if(tangoColorType == TANGO_HAL_PIXEL_FORMAT_YCrCb_420_SP)
 				{
-					cv::cvtColor(tangoImage, rgb, CV_YUV2BGR_NV21);
+					cv::cvtColor(tangoImage, rgb, cv::COLOR_YUV2BGR_NV21);
 				}
 				else if(tangoColorType == 35)
 				{

app/android/jni/RTABMapApp.cpp

@@ -64,6 +64,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #include <rtabmap/core/Memory.h>
 #include <rtabmap/core/GainCompensator.h>
 #include <rtabmap/core/DBDriver.h>
+#include <rtabmap/core/Recovery.h>
 #include <pcl/common/common.h>
 #include <pcl/filters/extract_indices.h>
 #include <pcl/io/ply_io.h>
@@ -82,6 +83,43 @@ static JavaVM *jvm;
 static jobject RTABMapActivity = 0;
 #endif
 
+#ifdef __ANDROID__
+#ifndef DISABLE_LOG
+//ref: https://codelab.wordpress.com/2014/11/03/how-to-use-standard-output-streams-for-logging-in-android-apps/
+static int pfd[2];
+static pthread_t thr;
+static void *thread_func(void*)
+{
+    ssize_t rdsz;
+    char buf[128];
+    while((rdsz = read(pfd[0], buf, sizeof buf - 1)) > 0) {
+        if(buf[rdsz - 1] == '\n') --rdsz;
+        buf[rdsz] = 0;  /* add null-terminator */
+        __android_log_write(ANDROID_LOG_DEBUG, LOG_TAG, buf);
+    }
+    return 0;
+}
+
+int start_logger()
+{
+    /* make stdout line-buffered and stderr unbuffered */
+    setvbuf(stdout, 0, _IOLBF, 0);
+    setvbuf(stderr, 0, _IONBF, 0);
+
+    /* create the pipe and redirect stdout and stderr */
+    pipe(pfd);
+    dup2(pfd[1], 1);
+    dup2(pfd[1], 2);
+
+    /* spawn the logging thread */
+    if(pthread_create(&thr, 0, thread_func, 0) == -1)
+        return -1;
+    pthread_detach(thr);
+    return 0;
+}
+#endif
+#endif
+
 rtabmap::ParametersMap RTABMapApp::getRtabmapParameters()
 {
 	rtabmap::ParametersMap parameters;
@@ -243,6 +281,12 @@ RTABMapApp::RTABMapApp() :
 
 	this->registerToEventsManager();
 	LOGI("RTABMapApp::RTABMapApp() end");
+
+#ifdef __ANDROID__
+#ifndef DISABLE_LOG
+	start_logger();
+#endif
+#endif
 }
 
 #ifndef __ANDROID__ // __APPLE__
@@ -305,9 +349,9 @@ void RTABMapApp::setScreenRotation(int displayRotation, int cameraRotation)
 	}
 }
 
-int RTABMapApp::openDatabase(const std::string & databasePath, bool databaseInMemory, bool optimize, const std::string & databaseSource)
+int RTABMapApp::openDatabase(const std::string & databasePath, bool databaseInMemory, bool optimize, bool clearDatabase)
 {
-	LOGW("Opening database %s (inMemory=%d, optimize=%d)", databasePath.c_str(), databaseInMemory?1:0, optimize?1:0);
+	LOGW("Opening database %s (inMemory=%d, optimize=%d, clearDatabase=%d)", databasePath.c_str(), databaseInMemory?1:0, optimize?1:0, clearDatabase?1:0);
 	this->unregisterFromEventsManager(); // to ignore published init events when closing rtabmap
 	status_.first = rtabmap::RtabmapEventInit::kInitializing;
 	rtabmapMutex_.lock();
@@ -360,11 +404,11 @@ int RTABMapApp::openDatabase(const std::string & databasePath, bool databaseInMe
 	std::vector<std::vector<Eigen::Vector2f> > texCoords;
 #endif
 	cv::Mat textures;
-	if(!databaseSource.empty())
+	if(!databasePath.empty() && UFile::exists(databasePath) && !clearDatabase)
 	{
 		UEventsManager::post(new rtabmap::RtabmapEventInit(rtabmap::RtabmapEventInit::kInfo, "Loading optimized cloud/mesh..."));
 		rtabmap::DBDriver * driver = rtabmap::DBDriver::create();
-		if(driver->openConnection(databaseSource))
+		if(driver->openConnection(databasePath))
 		{
 			cloudMat = driver->loadOptimizedMesh(&polygons, &texCoords, &textures);
 			if(!cloudMat.empty())
@@ -416,13 +460,11 @@ int RTABMapApp::openDatabase(const std::string & databasePath, bool databaseInMe
 	}
 
 	UEventsManager::post(new rtabmap::RtabmapEventInit(rtabmap::RtabmapEventInit::kInfo, "Loading database..."));
-	LOGI("Erasing database \"%s\"...", databasePath.c_str());
+	if(clearDatabase)
-	UFile::erase(databasePath);
+    {
-	if(!databaseSource.empty())
+        LOGI("Erasing database \"%s\"...", databasePath.c_str());
-	{
+        UFile::erase(databasePath);
-		LOGI("Copying database source \"%s\" to \"%s\"...", databaseSource.c_str(), databasePath.c_str());
+    }
-		UFile::copy(databaseSource, databasePath);
-	}
 
 	//Rtabmap
 	mapToOdom_.setIdentity();
@@ -593,7 +635,7 @@ int RTABMapApp::openDatabase(const std::string & databasePath, bool databaseInMe
                                     if(renderingTextureDecimation_>1)
                                     {
                                         cv::Size reducedSize(data.imageRaw().cols/renderingTextureDecimation_, data.imageRaw().rows/renderingTextureDecimation_);
-                                        cv::resize(data.imageRaw(), inserted.first->second.texture, reducedSize, 0, 0, CV_INTER_LINEAR);
+                                        cv::resize(data.imageRaw(), inserted.first->second.texture, reducedSize, 0, 0, cv::INTER_LINEAR);
                                     }
                                     else
                                     {
@@ -1199,7 +1241,12 @@ int RTABMapApp::Render()
 	std::list<rtabmap::RtabmapEvent*> rtabmapEvents;
 	try
 	{
-		UTimer fpsTime;
+        if(camera_ == 0)
+        {
+            // We are not doing continous drawing, just measure single draw
+            fpsTime_.restart();
+        }
+        
 #ifdef DEBUG_RENDERING_PERFORMANCE
 		UTimer time;
 #endif
@@ -1446,12 +1493,12 @@ int RTABMapApp::Render()
 
 			main_scene_.setMeshRendering(main_scene_.hasMesh(g_optMeshId), main_scene_.hasTexture(g_optMeshId));
 
-			fpsTime.restart();
 			main_scene_.setFrustumVisible(camera_!=0);
 			lastDrawnCloudsCount_ = main_scene_.Render(uvsTransformed, arViewMatrix, arProjectionMatrix);
-			if(renderingTime_ < fpsTime.elapsed())
+            double fpsTime = fpsTime_.ticks();
+			if(renderingTime_ < fpsTime)
 			{
-				renderingTime_ = fpsTime.elapsed();
+				renderingTime_ = fpsTime;
 			}
 
 			// revert state
@@ -1578,7 +1625,7 @@ int RTABMapApp::Render()
 									{
 										cv::Size reducedSize(textureRaw.cols/renderingTextureDecimation_, textureRaw.rows/renderingTextureDecimation_);
 										LOGD("resize image from %dx%d to %dx%d", textureRaw.cols, textureRaw.rows, reducedSize.width, reducedSize.height);
-										cv::resize(textureRaw, iter->second.texture, reducedSize, 0, 0, CV_INTER_LINEAR);
+										cv::resize(textureRaw, iter->second.texture, reducedSize, 0, 0, cv::INTER_LINEAR);
 									}
 									else
 									{
@@ -1863,7 +1910,7 @@ int RTABMapApp::Render()
                                                 if(renderingTextureDecimation_ > 1)
                                                 {
                                                     cv::Size reducedSize(data.imageRaw().cols/renderingTextureDecimation_, data.imageRaw().rows/renderingTextureDecimation_);
-                                                    cv::resize(data.imageRaw(), inserted.first->second.texture, reducedSize, 0, 0, CV_INTER_LINEAR);
+                                                    cv::resize(data.imageRaw(), inserted.first->second.texture, reducedSize, 0, 0, cv::INTER_LINEAR);
 #ifdef DEBUG_RENDERING_PERFORMANCE
                                                     LOGW("resize image from %dx%d to %dx%d (%fs)", data.imageRaw().cols, data.imageRaw().rows, reducedSize.width, reducedSize.height, time.ticks());
 #endif
@@ -2053,13 +2100,13 @@ int RTABMapApp::Render()
 				notifyDataLoaded = true;
 			}
 
-			fpsTime.restart();
+            main_scene_.setFrustumVisible(camera_!=0);
-			main_scene_.setFrustumVisible(camera_!=0);
 			lastDrawnCloudsCount_ = main_scene_.Render(uvsTransformed, arViewMatrix, arProjectionMatrix, occlusionMesh, true);
-			if(renderingTime_ < fpsTime.elapsed())
+            double fpsTime = fpsTime_.ticks();
+            if(renderingTime_ < fpsTime)
 			{
-				renderingTime_ = fpsTime.elapsed();
+				renderingTime_ = fpsTime;
-			}
+            }
 
 			if(rtabmapEvents.size())
 			{
@@ -2096,7 +2143,7 @@ int RTABMapApp::Render()
 			cv::Mat image(h, w, CV_8UC4);
 			glReadPixels(0, 0, w, h, GL_RGBA, GL_UNSIGNED_BYTE, image.data);
 			cv::flip(image, image, 0);
-			cv::cvtColor(image, image, CV_RGBA2BGRA);
+			cv::cvtColor(image, image, cv::COLOR_RGBA2BGRA);
 			cv::Mat roi;
 			if(w>h)
 			{
@@ -2115,16 +2162,6 @@ int RTABMapApp::Render()
 			screenshotReady_.release();
 		}
 
-		if((openingDatabase_ && !visualizingMesh_) || exporting_ || postProcessing_)
-		{
-			// throttle rendering max 5Hz if we are doing some processing
-			double renderTime = fpsTime.elapsed();
-			if(0.2 - renderTime > 0.0)
-			{
-				uSleep((0.2 - renderTime)*1000);
-			}
-		}
-
 		if((rtabmapThread_==0 || !rtabmapThread_->isRunning()) && lastPostRenderEventTime_ > 0.0)
 		{
 			double interval = UTimer::now() - lastPostRenderEventTime_;
@@ -2557,6 +2594,26 @@ void RTABMapApp::save(const std::string & databasePath)
 	}
 }
 
+bool RTABMapApp::recover(const std::string & from, const std::string & to)
+{
+    std::string errorMsg;
+    if(!databaseRecovery(from, false, &errorMsg, &progressionStatus_))
+    {
+        LOGE("Recovery Error: %s", errorMsg.c_str());
+        return false;
+    }
+    else
+    {
+        LOGI("Renaming %s to %s", from.c_str(), to.c_str());
+        if(UFile::rename(from, to) != 0)
+        {
+            LOGE("Failed renaming %s to %s", from.c_str(), to.c_str());
+            return false;
+        }
+        return true;
+    }
+}
+
 void RTABMapApp::cancelProcessing()
 {
 	UWARN("Processing canceled!");
@@ -3711,14 +3768,14 @@ void RTABMapApp::postOdometryEvent(
 					cv::Mat yuv(rgbHeight+rgbHeight/2, rgbWidth, CV_8UC1);
 					memcpy(yuv.data, yPlane, yPlaneLen);
 					memcpy(yuv.data+yPlaneLen, vPlane, rgbHeight/2*rgbWidth);
-					cv::cvtColor(yuv, outputRGB, CV_YUV2BGR_NV21);
+					cv::cvtColor(yuv, outputRGB, cv::COLOR_YUV2BGR_NV21);
 				}
 				else
 				{
 #ifdef __ANDROID__
-					cv::cvtColor(cv::Mat(rgbHeight+rgbHeight/2, rgbWidth, CV_8UC1, (void*)yPlane), outputRGB, CV_YUV2BGR_NV21);
+					cv::cvtColor(cv::Mat(rgbHeight+rgbHeight/2, rgbWidth, CV_8UC1, (void*)yPlane), outputRGB, cv::COLOR_YUV2BGR_NV21);
 #else // __APPLE__
-                    cv::cvtColor(cv::Mat(rgbHeight+rgbHeight/2, rgbWidth, CV_8UC1, (void*)yPlane), outputRGB, CV_YUV2RGB_NV21);
+					cv::cvtColor(cv::Mat(rgbHeight+rgbHeight/2, rgbWidth, CV_8UC1, (void*)yPlane), outputRGB, cv::COLOR_YUV2RGB_NV21);
 #endif
 				}
 

app/android/jni/RTABMapApp.h

@@ -75,7 +75,7 @@ class RTABMapApp : public UEventsHandler {
 
   void setScreenRotation(int displayRotation, int cameraRotation);
 
-  int openDatabase(const std::string & databasePath, bool databaseInMemory, bool optimize, const std::string & databaseSource=std::string());
+  int openDatabase(const std::string & databasePath, bool databaseInMemory, bool optimize, bool clearDatabase);
 
   bool isBuiltWith(int cameraDriver) const;
 #ifdef __ANDROID__
@@ -153,6 +153,7 @@ class RTABMapApp : public UEventsHandler {
   void addEnvSensor(int type, float value);
 
   void save(const std::string & databasePath);
+  bool recover(const std::string & from, const std::string & to);
   void cancelProcessing();
   bool exportMesh(
 		  float cloudVoxelSize,
@@ -265,6 +266,8 @@ class RTABMapApp : public UEventsHandler {
   // main_scene_ includes all drawable object for visualizing Tango device's
   // movement and point cloud.
   Scene main_scene_;
+    
+    UTimer fpsTime_;
 
 	std::list<rtabmap::RtabmapEvent*> rtabmapEvents_;
 	std::list<rtabmap::OdometryEvent> odomEvents_;

app/android/jni/jni_interface.cpp

@@ -89,13 +89,13 @@ Java_com_introlab_rtabmap_RTABMapLib_setScreenRotation(
 
 JNIEXPORT int JNICALL
 Java_com_introlab_rtabmap_RTABMapLib_openDatabase(
-    JNIEnv* env, jclass, jlong native_application, jstring databasePath, bool databaseInMemory, bool optimize)
+    JNIEnv* env, jclass, jlong native_application, jstring databasePath, bool databaseInMemory, bool optimize, bool clearDatabase)
 {
 	std::string databasePathC;
 	GetJStringContent(env,databasePath,databasePathC);
 	if(native_application)
 	{
-		return native(native_application)->openDatabase(databasePathC, databaseInMemory, optimize);
+		return native(native_application)->openDatabase(databasePathC, databaseInMemory, optimize, clearDatabase);
 	}
 	else
 	{
@@ -104,17 +104,17 @@ Java_com_introlab_rtabmap_RTABMapLib_openDatabase(
 	}
 }
 
-JNIEXPORT int JNICALL
+JNIEXPORT bool JNICALL
-Java_com_introlab_rtabmap_RTABMapLib_openDatabase2(
+Java_com_introlab_rtabmap_RTABMapLib_recover(
-    JNIEnv* env, jclass, jlong native_application, jstring databaseSource, jstring databasePath, bool databaseInMemory, bool optimize)
+    JNIEnv* env, jclass, jlong native_application, jstring from, jstring to)
 {
 	if(native_application)
 	{
-		std::string databasePathC;
+		std::string toC;
-		GetJStringContent(env,databasePath,databasePathC);
+		GetJStringContent(env,to,toC);
-		std::string databaseSourceC;
+		std::string fromC;
-		GetJStringContent(env,databaseSource,databaseSourceC);
+		GetJStringContent(env,from,fromC);
-		return native(native_application)->openDatabase(databasePathC, databaseInMemory, optimize, databaseSourceC);
+		return native(native_application)->recover(fromC, toC);
 	}
 	else
 	{

app/android/jni/point_cloud_drawable.cpp

@@ -309,8 +309,8 @@ PointCloudDrawable::PointCloudDrawable(
 		float gainR,
 		float gainG,
 		float gainB) :
-				vertex_buffers_(0),
+				vertex_buffer_(0),
-				textures_(0),
+				texture_(0),
 				nPoints_(0),
 				pose_(rtabmap::Transform::getIdentity()),
 				poseGl_(1.0f),
@@ -320,14 +320,16 @@ PointCloudDrawable::PointCloudDrawable(
 				gainG_(gainG),
 				gainB_(gainB)
 {
+    index_buffers_.resize(6, 0);
+    index_buffers_count_.resize(6, 0);
 	updateCloud(cloud, indices);
 }
 
 PointCloudDrawable::PointCloudDrawable(
 		const rtabmap::Mesh & mesh,
 		bool createWireframe) :
-				vertex_buffers_(0),
+				vertex_buffer_(0),
-				textures_(0),
+				texture_(0),
 				nPoints_(0),
 				pose_(rtabmap::Transform::getIdentity()),
 				poseGl_(1.0f),
@@ -337,64 +339,83 @@ PointCloudDrawable::PointCloudDrawable(
 				gainG_(1.0f),
 				gainB_(1.0f)
 {
+    index_buffers_.resize(6, 0);
+    index_buffers_count_.resize(6, 0);
 	updateMesh(mesh, createWireframe);
 }
 
 PointCloudDrawable::~PointCloudDrawable()
 {
-	LOGI("Freeing cloud buffer %d", vertex_buffers_);
+	LOGI("Freeing cloud buffer %d", vertex_buffer_);
-	if (vertex_buffers_)
+	if (vertex_buffer_)
 	{
-		glDeleteBuffers(1, &vertex_buffers_);
+		glDeleteBuffers(1, &vertex_buffer_);
 		tango_gl::util::CheckGlError("PointCloudDrawable::~PointCloudDrawable()");
-		vertex_buffers_ = 0;
+		vertex_buffer_ = 0;
 	}
 
-	if (textures_)
+	if (texture_)
 	{
-		glDeleteTextures(1, &textures_);
+		glDeleteTextures(1, &texture_);
 		tango_gl::util::CheckGlError("PointCloudDrawable::~PointCloudDrawable()");
-		textures_ = 0;
+		texture_ = 0;
 	}
+    
+    for(size_t i=0; i<index_buffers_.size(); ++i)
+    {
+        if(index_buffers_[i])
+        {
+            glDeleteBuffers(1, &index_buffers_[i]);
+            index_buffers_[i] = 0;
+            tango_gl::util::CheckGlError("PointCloudDrawable::~PointCloudDrawable()");
+        }
+    }
 }
 
 void PointCloudDrawable::updatePolygons(const std::vector<pcl::Vertices> & polygons, const std::vector<pcl::Vertices> & polygonsLowRes, bool createWireframe)
 {
+    for(int i=0; i<4; ++i)
+    {
+        if(index_buffers_[i])
+        {
+            glDeleteBuffers(1, &index_buffers_[i]);
+            index_buffers_[i] = 0;
+            tango_gl::util::CheckGlError("PointCloudDrawable::updatePolygons() clearing polygon buffers");
+        }
+    }
+    
 	//LOGD("Update polygons");
-	polygons_.clear();
-	polygonLines_.clear();
-	polygonsLowRes_.clear();
-	polygonLinesLowRes_.clear();
 	if(polygons.size() && organizedToDenseIndices_.size())
 	{
-		unsigned int polygonSize = polygons[0].vertices.size();
+		size_t polygonSize = polygons[0].vertices.size();
 		UASSERT(polygonSize == 3);
-		polygons_.resize(polygons.size() * polygonSize);
+        std::vector<std::vector<GLuint> > indexes(4);
+        indexes[0].resize(polygons.size() * polygonSize);
 		if(createWireframe)
-			polygonLines_.resize(polygons_.size()*2);
+            indexes[2].resize(indexes[0].size()*2);
 		int oi = 0;
 		int li = 0;
-		for(unsigned int i=0; i<polygons.size(); ++i)
+        for(size_t i=0; i<polygons.size(); ++i)
 		{
 			UASSERT(polygons[i].vertices.size() == polygonSize);
 			for(unsigned int j=0; j<polygonSize; ++j)
 			{
-				polygons_[oi++] = organizedToDenseIndices_.at(polygons[i].vertices[j]);
+                indexes[0][oi++] = organizedToDenseIndices_.at(polygons[i].vertices[j]);
 				if(createWireframe)
 				{
-					polygonLines_[li++] = organizedToDenseIndices_.at(polygons[i].vertices[j]);
+                    indexes[2][li++] = organizedToDenseIndices_.at(polygons[i].vertices[j]);
-					polygonLines_[li++] = organizedToDenseIndices_.at(polygons[i].vertices[(j+1) % polygonSize]);
+                    indexes[2][li++] = organizedToDenseIndices_.at(polygons[i].vertices[(j+1) % polygonSize]);
 				}
 			}
 		}
 
 		if(polygonsLowRes.size())
 		{
-			unsigned int polygonSize = polygonsLowRes[0].vertices.size();
+			size_t polygonSize = polygonsLowRes[0].vertices.size();
 			UASSERT(polygonSize == 3);
-			polygonsLowRes_.resize(polygonsLowRes.size() * polygonSize);
+            indexes[1].resize(polygonsLowRes.size() * polygonSize);
 			if(createWireframe)
-				polygonLinesLowRes_.resize(polygonsLowRes_.size()*2);
+                indexes[3].resize(indexes[1].size()*2);
 			int oi = 0;
 			int li = 0;
 			for(unsigned int i=0; i<polygonsLowRes.size(); ++i)
@@ -402,15 +423,44 @@ void PointCloudDrawable::updatePolygons(const std::vector<pcl::Vertices> & polyg
 				UASSERT(polygonsLowRes[i].vertices.size() == polygonSize);
 				for(unsigned int j=0; j<polygonSize; ++j)
 				{
-					polygonsLowRes_[oi++] = organizedToDenseIndices_.at(polygonsLowRes[i].vertices[j]);
+                    indexes[1][oi++] = organizedToDenseIndices_.at(polygonsLowRes[i].vertices[j]);
 					if(createWireframe)
 					{
-						polygonLinesLowRes_[li++] = organizedToDenseIndices_.at(polygonsLowRes[i].vertices[j]);
+                        indexes[3][li++] = organizedToDenseIndices_.at(polygonsLowRes[i].vertices[j]);
-						polygonLinesLowRes_[li++] = organizedToDenseIndices_.at(polygonsLowRes[i].vertices[(j+1)%polygonSize]);
+                        indexes[3][li++] = organizedToDenseIndices_.at(polygonsLowRes[i].vertices[(j+1)%polygonSize]);
 					}
 				}
 			}
 		}
+        
+        // Generate index buffers
+        for(size_t i=0; i<indexes.size(); ++i)
+        {
+            if(!indexes[i].empty())
+            {
+                glGenBuffers(1, &index_buffers_[i]);
+                if(!index_buffers_[i])
+                {
+                    LOGE("OpenGL: could not generate index buffer %ld\n", i);
+                    return;
+                }
+                
+                LOGD("Adding polygon index %ld size=%ld", i, indexes[i].size());
+                
+                glBindBuffer(GL_ELEMENT_ARRAY_BUFFER, index_buffers_[i]);
+                glBufferData(GL_ELEMENT_ARRAY_BUFFER, sizeof(uint32_t) * indexes[i].size(), indexes[i].data(), GL_STATIC_DRAW);
+                glBindBuffer(GL_ELEMENT_ARRAY_BUFFER, 0);
+                index_buffers_count_[i] = (int)indexes[i].size();
+
+                GLint error = glGetError();
+                if(error != GL_NO_ERROR)
+                {
+                    LOGE("OpenGL: Could not allocate indexes (0x%x)\n", error);
+                    index_buffers_[i] = 0;
+                    return;
+                }
+            }
+        }
 	}
 }
 
@@ -418,43 +468,51 @@ void PointCloudDrawable::updateCloud(const pcl::PointCloud<pcl::PointXYZRGB>::Pt
 {
 	UASSERT(cloud.get() && !cloud->empty());
 	nPoints_ = 0;
-	polygons_.clear();
-	polygonsLowRes_.clear();
-	verticesLowRes_.clear();
-	verticesLowLowRes_.clear();
 	aabbMinModel_ = aabbMinWorld_ = pcl::PointXYZ(1000,1000,1000);
 	aabbMaxModel_ = aabbMaxWorld_ = pcl::PointXYZ(-1000,-1000,-1000);
 
-	if (vertex_buffers_)
+	if (vertex_buffer_)
 	{
-		glDeleteBuffers(1, &vertex_buffers_);
+		glDeleteBuffers(1, &vertex_buffer_);
-		tango_gl::util::CheckGlError("PointCloudDrawable::~PointCloudDrawable()");
+		tango_gl::util::CheckGlError("PointCloudDrawable::updateCloud() clear vertex buffer");
-		vertex_buffers_ = 0;
+		vertex_buffer_ = 0;
 	}
 
-	if (textures_)
+	if (texture_)
 	{
-		glDeleteTextures(1, &textures_);
+		glDeleteTextures(1, &texture_);
-		tango_gl::util::CheckGlError("PointCloudDrawable::~PointCloudDrawable()");
+		tango_gl::util::CheckGlError("PointCloudDrawable::updateCloud() clear texture buffer");
-		textures_ = 0;
+		texture_ = 0;
 	}
+    
+    for(size_t i=0; i<index_buffers_.size(); ++i)
+    {
+        if(index_buffers_[i])
+        {
+            glDeleteBuffers(1, &index_buffers_[i]);
+            index_buffers_[i] = 0;
+            tango_gl::util::CheckGlError("PointCloudDrawable::updateCloud() clear index buffer");
+        }
+    }
 
-	glGenBuffers(1, &vertex_buffers_);
+	glGenBuffers(1, &vertex_buffer_);
-	if(!vertex_buffers_)
+	if(!vertex_buffer_)
 	{
 		LOGE("OpenGL: could not generate vertex buffers\n");
 		return;
 	}
 
-	LOGI("Creating cloud buffer %d", vertex_buffers_);
+	LOGI("Creating cloud buffer %d", vertex_buffer_);
 	std::vector<float> vertices;
-	int totalPoints = 0;
+	size_t totalPoints = 0;
+    std::vector<GLuint> verticesLowRes;
+    std::vector<GLuint> verticesLowLowRes;
 	if(indices.get() && indices->size())
 	{
 		totalPoints = indices->size();
 		vertices.resize(indices->size()*4);
-		verticesLowRes_.resize(cloud->isOrganized()?totalPoints:0);
+		verticesLowRes.resize(cloud->isOrganized()?totalPoints:0);
-		verticesLowLowRes_.resize(cloud->isOrganized()?totalPoints:0);
+		verticesLowLowRes.resize(cloud->isOrganized()?totalPoints:0);
 		int oi_low = 0;
 		int oi_lowlow = 0;
 		for(unsigned int i=0; i<indices->size(); ++i)
@@ -471,23 +529,23 @@ void PointCloudDrawable::updateCloud(const pcl::PointCloud<pcl::PointXYZRGB>::Pt
 			{
 				if(indices->at(i)%LOW_DEC == 0 && (indices->at(i)/cloud->width) % LOW_DEC == 0)
 				{
-					verticesLowRes_[oi_low++] = i;
+					verticesLowRes[oi_low++] = i;
 				}
 				if(indices->at(i)%LOWLOW_DEC == 0 && (indices->at(i)/cloud->width) % LOWLOW_DEC == 0)
 				{
-					verticesLowLowRes_[oi_lowlow++] = i;
+					verticesLowLowRes[oi_lowlow++] = i;
 				}
 			}
 		}
-		verticesLowRes_.resize(oi_low);
+		verticesLowRes.resize(oi_low);
-		verticesLowLowRes_.resize(oi_lowlow);
+		verticesLowLowRes.resize(oi_lowlow);
 	}
 	else
 	{
 		totalPoints = cloud->size();
 		vertices.resize(cloud->size()*4);
-		verticesLowRes_.resize(cloud->isOrganized()?totalPoints:0);
+		verticesLowRes.resize(cloud->isOrganized()?totalPoints:0);
-		verticesLowLowRes_.resize(cloud->isOrganized()?totalPoints:0);
+		verticesLowLowRes.resize(cloud->isOrganized()?totalPoints:0);
 		int oi_low = 0;
 		int oi_lowlow = 0;
 		for(unsigned int i=0; i<cloud->size(); ++i)
@@ -504,19 +562,19 @@ void PointCloudDrawable::updateCloud(const pcl::PointCloud<pcl::PointXYZRGB>::Pt
 			{
 				if(i%LOW_DEC == 0 && (i/cloud->width) % LOW_DEC == 0)
 				{
-					verticesLowRes_[oi_low++] = i;
+					verticesLowRes[oi_low++] = i;
 				}
 				if(i%LOWLOW_DEC == 0 && (i/cloud->width) % LOWLOW_DEC == 0)
 				{
-					verticesLowLowRes_[oi_lowlow++] = i;
+					verticesLowLowRes[oi_lowlow++] = i;
 				}
 			}
 		}
-		verticesLowRes_.resize(oi_low);
+		verticesLowRes.resize(oi_low);
-		verticesLowLowRes_.resize(oi_lowlow);
+		verticesLowLowRes.resize(oi_lowlow);
 	}
 
-	glBindBuffer(GL_ARRAY_BUFFER, vertex_buffers_);
+	glBindBuffer(GL_ARRAY_BUFFER, vertex_buffer_);
 	glBufferData(GL_ARRAY_BUFFER, sizeof(GLfloat) * (int)vertices.size(), (const void *)vertices.data(), GL_STATIC_DRAW);
 	glBindBuffer(GL_ARRAY_BUFFER, 0);
 
@@ -524,11 +582,40 @@ void PointCloudDrawable::updateCloud(const pcl::PointCloud<pcl::PointXYZRGB>::Pt
 	if(error != GL_NO_ERROR)
 	{
 		LOGE("OpenGL: Could not allocate point cloud (0x%x)\n", error);
-		vertex_buffers_ = 0;
+		vertex_buffer_ = 0;
 		return;
 	}
+    
+    // vertex index buffers
+    for(size_t i=4; i<5; ++i)
+    {
+        if((i==4 && !verticesLowRes.empty()) ||
+           (i==5 && !verticesLowLowRes.empty()))
+        {
+            glGenBuffers(1, &index_buffers_[i]);
+            if(!index_buffers_[i])
+            {
+                LOGE("OpenGL: could not generate index buffer %ld\n", i);
+                return;
+            }
+            
+            index_buffers_count_[i] = i==4?(int)verticesLowRes.size():(int)verticesLowLowRes.size();
+            glBindBuffer(GL_ELEMENT_ARRAY_BUFFER, index_buffers_[i]);
+            glBufferData(GL_ELEMENT_ARRAY_BUFFER, sizeof(uint32_t) * index_buffers_count_[i], i==4?verticesLowRes.data():verticesLowLowRes.data(), GL_STATIC_DRAW);
+            glBindBuffer(GL_ELEMENT_ARRAY_BUFFER, 0);
+            
 
-	nPoints_ = totalPoints;
+            GLint error = glGetError();
+            if(error != GL_NO_ERROR)
+            {
+                LOGE("OpenGL: Could not allocate indexes (0x%x)\n", error);
+                index_buffers_[i] = 0;
+                return;
+            }
+        }
+    }
+
+	nPoints_ = (int)totalPoints;
 }
 
 void PointCloudDrawable::updateMesh(const rtabmap::Mesh & mesh, bool createWireframe)
@@ -538,12 +625,22 @@ void PointCloudDrawable::updateMesh(const rtabmap::Mesh & mesh, bool createWiref
 	aabbMinModel_ = aabbMinWorld_ = pcl::PointXYZ(1000,1000,1000);
 	aabbMaxModel_ = aabbMaxWorld_ = pcl::PointXYZ(-1000,-1000,-1000);
 
-	if (vertex_buffers_)
+	if (vertex_buffer_)
 	{
-		glDeleteBuffers(1, &vertex_buffers_);
+		glDeleteBuffers(1, &vertex_buffer_);
-		tango_gl::util::CheckGlError("PointCloudDrawable::~PointCloudDrawable()");
+		tango_gl::util::CheckGlError("PointCloudDrawable::updateMesh() clear vertex buffer");
-		vertex_buffers_ = 0;
+		vertex_buffer_ = 0;
 	}
+    
+    for(size_t i=0; i<index_buffers_.size(); ++i)
+    {
+        if(index_buffers_[i])
+        {
+            glDeleteBuffers(1, &index_buffers_[i]);
+            index_buffers_[i] = 0;
+            tango_gl::util::CheckGlError("PointCloudDrawable::updateMesh() clear index buffer");
+        }
+    }
 
 	gainR_ = mesh.gains[0];
 	gainG_ = mesh.gains[1];
@@ -552,17 +649,17 @@ void PointCloudDrawable::updateMesh(const rtabmap::Mesh & mesh, bool createWiref
 	bool textureUpdate = false;
 	if(!mesh.texture.empty() && mesh.texture.type() == CV_8UC3)
 	{
-		if (textures_)
+		if (texture_)
 		{
-			glDeleteTextures(1, &textures_);
+			glDeleteTextures(1, &texture_);
-			tango_gl::util::CheckGlError("PointCloudDrawable::~PointCloudDrawable()");
+			tango_gl::util::CheckGlError("PointCloudDrawable::updateMesh() clear texture buffer");
-			textures_ = 0;
+			texture_ = 0;
 		}
 		textureUpdate = true;
 	}
 
-	glGenBuffers(1, &vertex_buffers_);
+	glGenBuffers(1, &vertex_buffer_);
-	if(!vertex_buffers_)
+	if(!vertex_buffer_)
 	{
 		LOGE("OpenGL: could not generate vertex buffers\n");
 		return;
@@ -570,10 +667,10 @@ void PointCloudDrawable::updateMesh(const rtabmap::Mesh & mesh, bool createWiref
 
 	if(textureUpdate)
 	{
-		glGenTextures(1, &textures_);
+		glGenTextures(1, &texture_);
-		if(!textures_)
+		if(!texture_)
 		{
-			vertex_buffers_ = 0;
+			vertex_buffer_ = 0;
 			LOGE("OpenGL: could not generate texture buffers\n");
 			return;
 		}
@@ -591,11 +688,13 @@ void PointCloudDrawable::updateMesh(const rtabmap::Mesh & mesh, bool createWiref
 		polygonsLowRes = mesh.polygonsLowRes; // only in organized we keep the low res
 		organizedToDenseIndices_ = std::vector<unsigned int>(mesh.cloud->width*mesh.cloud->height, -1);
 		totalPoints = (int)mesh.indices->size();
-		verticesLowRes_.resize(totalPoints);
+        std::vector<GLuint> verticesLowRes;
-		verticesLowLowRes_.resize(totalPoints);
+        std::vector<GLuint> verticesLowLowRes;
+        verticesLowRes.resize(totalPoints);
+        verticesLowLowRes.resize(totalPoints);
 		int oi_low = 0;
 		int oi_lowlow = 0;
-		if(textures_ && polygons.size())
+		if(texture_ && polygons.size())
 		{
 			int items = hasNormals_?9:6;
 			vertices = std::vector<float>(mesh.indices->size()*items);
@@ -628,11 +727,11 @@ void PointCloudDrawable::updateMesh(const rtabmap::Mesh & mesh, bool createWiref
 
 				if(mesh.indices->at(i)%LOW_DEC == 0 && (mesh.indices->at(i)/mesh.cloud->width) % LOW_DEC == 0)
 				{
-					verticesLowRes_[oi_low++] = i;
+                    verticesLowRes[oi_low++] = i;
 				}
 				if(mesh.indices->at(i)%LOWLOW_DEC == 0 && (mesh.indices->at(i)/mesh.cloud->width) % LOWLOW_DEC == 0)
 				{
-					verticesLowLowRes_[oi_lowlow++] = i;
+                    verticesLowLowRes[oi_lowlow++] = i;
 				}
 			}
 		}
@@ -663,20 +762,48 @@ void PointCloudDrawable::updateMesh(const rtabmap::Mesh & mesh, bool createWiref
 
 				if(mesh.indices->at(i)%LOW_DEC == 0 && (mesh.indices->at(i)/mesh.cloud->width) % LOW_DEC == 0)
 				{
-					verticesLowRes_[oi_low++] = i;
+                    verticesLowRes[oi_low++] = i;
 				}
 				if(mesh.indices->at(i)%LOWLOW_DEC == 0 && (mesh.indices->at(i)/mesh.cloud->width) % LOWLOW_DEC == 0)
 				{
-					verticesLowLowRes_[oi_lowlow++] = i;
+                    verticesLowLowRes[oi_lowlow++] = i;
 				}
 			}
 		}
-		verticesLowRes_.resize(oi_low);
+        verticesLowRes.resize(oi_low);
-		verticesLowLowRes_.resize(oi_lowlow);
+        verticesLowLowRes.resize(oi_lowlow);
+        
+        // vertex index buffers
+        for(size_t i=4; i<5; ++i)
+        {
+            if((i==4 && !verticesLowRes.empty()) ||
+               (i==5 && !verticesLowLowRes.empty()))
+            {
+                glGenBuffers(1, &index_buffers_[i]);
+                if(!index_buffers_[i])
+                {
+                    LOGE("OpenGL: could not generate index buffer %ld\n", i);
+                    return;
+                }
+                
+                index_buffers_count_[i] = i==4?(int)verticesLowRes.size():(int)verticesLowLowRes.size();
+                glBindBuffer(GL_ELEMENT_ARRAY_BUFFER, index_buffers_[i]);
+                glBufferData(GL_ELEMENT_ARRAY_BUFFER, sizeof(uint32_t) * index_buffers_count_[i], i==4?verticesLowRes.data():verticesLowLowRes.data(), GL_STATIC_DRAW);
+                glBindBuffer(GL_ELEMENT_ARRAY_BUFFER, 0);
+
+                GLint error = glGetError();
+                if(error != GL_NO_ERROR)
+                {
+                    LOGE("OpenGL: Could not allocate indexes (0x%x)\n", error);
+                    index_buffers_[i] = 0;
+                    return;
+                }
+            }
+        }
 	}
 	else // assume dense mesh with texCoords set to polygons
 	{
-		if(textures_ && polygons.size())
+		if(texture_ && polygons.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);
@@ -743,7 +870,7 @@ void PointCloudDrawable::updateMesh(const rtabmap::Mesh & mesh, bool createWiref
 		}
 		else
 		{
-			totalPoints = mesh.cloud->size();
+			totalPoints = (int)mesh.cloud->size();
 			//LOGD("Dense mesh");
 			int items = hasNormals_?7:4;
 			organizedToDenseIndices_ = std::vector<unsigned int>(totalPoints, -1);
@@ -771,7 +898,7 @@ void PointCloudDrawable::updateMesh(const rtabmap::Mesh & mesh, bool createWiref
 		}
 	}
 
-	glBindBuffer(GL_ARRAY_BUFFER, vertex_buffers_);
+	glBindBuffer(GL_ARRAY_BUFFER, vertex_buffer_);
 	glBufferData(GL_ARRAY_BUFFER, sizeof(GLfloat) * (int)vertices.size(), (const void *)vertices.data(), GL_STATIC_DRAW);
 	glBindBuffer(GL_ARRAY_BUFFER, 0);
 
@@ -779,11 +906,11 @@ void PointCloudDrawable::updateMesh(const rtabmap::Mesh & mesh, bool createWiref
 	if(error != GL_NO_ERROR)
 	{
 		LOGE("OpenGL: Could not allocate point cloud (0x%x)\n", error);
-		vertex_buffers_ = 0;
+		vertex_buffer_ = 0;
 		return;
 	}
 
-	if(textures_ && textureUpdate)
+	if(texture_ && textureUpdate)
 	{
 		//GLint maxTextureSize = 0;
 		//glGetIntegerv(GL_MAX_TEXTURE_SIZE, &maxTextureSize);
@@ -793,13 +920,13 @@ void PointCloudDrawable::updateMesh(const rtabmap::Mesh & mesh, bool createWiref
 		//LOGW("maxTextureUnits=%d", maxTextureUnits);
 
 		// gen texture from image
-		glBindTexture(GL_TEXTURE_2D, textures_);
+		glBindTexture(GL_TEXTURE_2D, texture_);
 		glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_MIN_FILTER, GL_LINEAR);
 		glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_MAG_FILTER, GL_LINEAR);
 		glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_WRAP_S, GL_CLAMP_TO_EDGE);
 		glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_WRAP_T, GL_CLAMP_TO_EDGE);
 		cv::Mat rgbImage;
-		cv::cvtColor(mesh.texture, rgbImage, CV_BGR2RGBA);
+		cv::cvtColor(mesh.texture, rgbImage, cv::COLOR_BGR2RGBA);
 
 		glPixelStorei(GL_UNPACK_ALIGNMENT, 4);
 		//glPixelStorei(GL_UNPACK_ROW_LENGTH, 0);
@@ -811,20 +938,17 @@ void PointCloudDrawable::updateMesh(const rtabmap::Mesh & mesh, bool createWiref
 		if(error != GL_NO_ERROR)
 		{
 			LOGE("OpenGL: Could not allocate texture (0x%x)\n", error);
-			textures_ = 0;
+			texture_ = 0;
 
-			glDeleteBuffers(1, &vertex_buffers_);
+			glDeleteBuffers(1, &vertex_buffer_);
-			vertex_buffers_ = 0;
+			vertex_buffer_ = 0;
 			return;
 		}
 	}
 
 	nPoints_ = totalPoints;
 
-	if(polygons_.size() != polygons.size())
+    updatePolygons(polygons, polygonsLowRes, createWireframe);
-	{
-		updatePolygons(polygons, polygonsLowRes, createWireframe);
-	}
 
 	if(!pose_.isNull())
 	{
@@ -886,14 +1010,14 @@ void PointCloudDrawable::Render(
 		bool packDepthToColorChannel,
 		bool wireFrame) const
 {
-	if(vertex_buffers_ && nPoints_ && visible_ && !shaderPrograms_.empty())
+	if(vertex_buffer_ && nPoints_ && visible_ && !shaderPrograms_.empty())
 	{
 		if(packDepthToColorChannel || !hasNormals_)
 		{
 			lighting = false;
 		}
 
-		if(packDepthToColorChannel || !(meshRendering && textureRendering && textures_))
+		if(packDepthToColorChannel || !(meshRendering && textureRendering && texture_))
 		{
 			textureRendering = false;
 		}
@@ -996,7 +1120,7 @@ void PointCloudDrawable::Render(
 				// Texture activate unit 0
 				glActiveTexture(GL_TEXTURE0);
 				// Bind the texture to this unit.
-				glBindTexture(GL_TEXTURE_2D, textures_);
+				glBindTexture(GL_TEXTURE_2D, texture_);
 				// Tell the texture uniform sampler to use this texture in the shader by binding to texture unit 0.
 				GLuint texture_handle = glGetUniformLocation(program, "uTexture");
 				glUniform1i(texture_handle, 0);
@@ -1012,8 +1136,8 @@ void PointCloudDrawable::Render(
 		}
 		tango_gl::util::CheckGlError("Pointcloud::Render() common");
 
-		glBindBuffer(GL_ARRAY_BUFFER, vertex_buffers_);
+		glBindBuffer(GL_ARRAY_BUFFER, vertex_buffer_);
-		if(textures_)
+		if(texture_)
 		{
 			glVertexAttribPointer(attribute_vertex, 3, GL_FLOAT, GL_FALSE, (hasNormals_?9:6)*sizeof(GLfloat), 0);
 			if(textureRendering)
@@ -1044,53 +1168,49 @@ void PointCloudDrawable::Render(
 		tango_gl::util::CheckGlError("Pointcloud::Render() set attribute pointer");
 
 		UTimer drawTime;
-		if(textureRendering)
+		if((textureRendering || meshRendering) && index_buffers_[0])
 		{
-			if(distanceToCameraSqr<16.0f || polygonsLowRes_.empty())
+            float dist = meshRendering?50.0f:16.0f;
+			if(distanceToCameraSqr<dist || index_buffers_[1]==0)
 			{
-				wireFrame = wireFrame && polygonLines_.size();
+				wireFrame = wireFrame && index_buffers_[2];
 				if(wireFrame)
-					glDrawElements(GL_LINES, polygonLines_.size(), GL_UNSIGNED_INT, polygonLines_.data());
+                {
+                    glBindBuffer(GL_ELEMENT_ARRAY_BUFFER, index_buffers_[2]);
+                    glDrawElements(GL_LINES, index_buffers_count_[2], GL_UNSIGNED_INT, 0);
+                }
 				else
-					glDrawElements(GL_TRIANGLES, polygons_.size(), GL_UNSIGNED_INT, polygons_.data());
+                {
+                    glBindBuffer(GL_ELEMENT_ARRAY_BUFFER, index_buffers_[0]);
+                    glDrawElements(GL_TRIANGLES, index_buffers_count_[0], GL_UNSIGNED_INT, 0);
+                }
 			}
 			else
 			{
-				wireFrame = wireFrame && polygonLinesLowRes_.size();
+				wireFrame = wireFrame && index_buffers_[3];
 				if(wireFrame)
-					glDrawElements(GL_LINES, polygonLinesLowRes_.size(), GL_UNSIGNED_INT, polygonLinesLowRes_.data());
+                {
+                    glBindBuffer(GL_ELEMENT_ARRAY_BUFFER, index_buffers_[3]);
+                    glDrawElements(GL_LINES, index_buffers_count_[3], GL_UNSIGNED_INT, 0);
+                }
 				else
-					glDrawElements(GL_TRIANGLES, polygonsLowRes_.size(), GL_UNSIGNED_INT, polygonsLowRes_.data());
+                {
+                    glBindBuffer(GL_ELEMENT_ARRAY_BUFFER, index_buffers_[1]);
+                    glDrawElements(GL_TRIANGLES, index_buffers_count_[1], GL_UNSIGNED_INT, 0);
+                }
 			}
 		}
-		else if(meshRendering && polygons_.size())
+		else if(index_buffers_[4])
 		{
-			if(distanceToCameraSqr<50.0f || polygonsLowRes_.empty())
+			if(distanceToCameraSqr>600.0f && index_buffers_[5])
 			{
-				wireFrame = wireFrame && polygonLines_.size();
+                glBindBuffer(GL_ELEMENT_ARRAY_BUFFER, index_buffers_[5]);
-				if(wireFrame)
+                glDrawElements(GL_POINTS, index_buffers_count_[5], GL_UNSIGNED_INT, 0);
-					glDrawElements(GL_LINES, polygonLines_.size(), GL_UNSIGNED_INT, polygonLines_.data());
-				else
-					glDrawElements(GL_TRIANGLES, polygons_.size(), GL_UNSIGNED_INT, polygons_.data());
-			}
-			else
-			{
-				wireFrame = wireFrame && polygonLinesLowRes_.size();
-				if(wireFrame)
-					glDrawElements(GL_LINES, polygonLinesLowRes_.size(), GL_UNSIGNED_INT, polygonLinesLowRes_.data());
-				else
-					glDrawElements(GL_TRIANGLES, polygonsLowRes_.size(), GL_UNSIGNED_INT, polygonsLowRes_.data());
-			}
-		}
-		else if(!verticesLowRes_.empty())
-		{
-			if(distanceToCameraSqr>600.0f)
-			{
-				glDrawElements(GL_POINTS, verticesLowLowRes_.size(), GL_UNSIGNED_INT, verticesLowLowRes_.data());
 			}
 			else if(distanceToCameraSqr>150.0f)
 			{
-				glDrawElements(GL_POINTS, verticesLowRes_.size(), GL_UNSIGNED_INT, verticesLowRes_.data());
+                glBindBuffer(GL_ELEMENT_ARRAY_BUFFER, index_buffers_[4]);
+                glDrawElements(GL_POINTS, index_buffers_count_[4], GL_UNSIGNED_INT, 0);
 			}
 			else
 			{
@@ -1106,6 +1226,7 @@ void PointCloudDrawable::Render(
 
 		glDisableVertexAttribArray(0);
 		glBindBuffer(GL_ARRAY_BUFFER, 0);
+        glBindBuffer(GL_ELEMENT_ARRAY_BUFFER, 0);
 
 		glUseProgram(0);
 		tango_gl::util::CheckGlError("Pointcloud::Render() cleaning");

app/android/jni/point_cloud_drawable.h

@@ -70,8 +70,8 @@ private:
   rtabmap::Transform getPose() const {return pose_;}
   const glm::mat4 & getPoseGl() const {return poseGl_;}
   bool isVisible() const {return visible_;}
-  bool hasMesh() const {return polygons_.size()!=0;}
+  bool hasMesh() const {return index_buffers_[0] != 0;}
-  bool hasTexture() const {return textures_ != 0;}
+  bool hasTexture() const {return texture_ != 0;}
   float getMinHeight() const {return minHeight_;}
   const pcl::PointXYZ & aabbMinModel() const {return aabbMinModel_;}
   const pcl::PointXYZ & aabbMaxModel() const {return aabbMaxModel_;}
@@ -115,14 +115,10 @@ private:
 
  private:
   // Vertex buffer of the point cloud geometry.
-  GLuint vertex_buffers_;
+  GLuint vertex_buffer_;
-  GLuint textures_;
+  GLuint texture_;
-  std::vector<GLuint> polygons_;
+  std::vector<GLuint> index_buffers_;
-  std::vector<GLuint> polygonsLowRes_;
+  std::vector<int> index_buffers_count_;
-  std::vector<GLuint> polygonLines_;
-  std::vector<GLuint> polygonLinesLowRes_;
-  std::vector<GLuint> verticesLowRes_;
-  std::vector<GLuint> verticesLowLowRes_;
   int nPoints_;
   rtabmap::Transform pose_;
   glm::mat4 poseGl_;

app/android/jni/scene.cpp

@@ -102,7 +102,7 @@ Scene::Scene() :
 		screenHeight_(0),
 		doubleTapOn_(false)
 {
-    depthTextures_[0] = depthTextures_[1] = 0;
+    depthTexture_ = 0;
 	gesture_camera_ = new tango_gl::GestureCamera();
 	gesture_camera_->SetCameraType(
 	      tango_gl::GestureCamera::kThirdPersonFollow);
@@ -179,8 +179,8 @@ void Scene::DeleteResources() {
 		fboId_ = 0;
 		glDeleteRenderbuffers(1, &rboId_);
 		rboId_ = 0;
-        glDeleteTextures(2, depthTextures_);
+        glDeleteTextures(1, &depthTexture_);
-        depthTextures_[0] = depthTextures_[1] = 0;
+        depthTexture_ = 0;
 	}
 
 	clear();
@@ -234,8 +234,8 @@ void Scene::SetupViewPort(int w, int h) {
 			fboId_ = 0;
 			glDeleteRenderbuffers(1, &rboId_);
 			rboId_ = 0;
-			glDeleteTextures(2, depthTextures_);
+			glDeleteTextures(1, &depthTexture_);
-            depthTextures_[0] = depthTextures_[1] = 0;
+            depthTexture_ = 0;
 		}
 
         GLint originid = 0;
@@ -247,8 +247,8 @@ void Scene::SetupViewPort(int w, int h) {
 		glBindFramebuffer(GL_FRAMEBUFFER, fboId_);
 
 		// Create depth texture
-		glGenTextures(2, depthTextures_);
+		glGenTextures(1, &depthTexture_);
-		glBindTexture(GL_TEXTURE_2D, depthTextures_[0]);
+		glBindTexture(GL_TEXTURE_2D, depthTexture_);
 		glTexParameterf(GL_TEXTURE_2D, GL_TEXTURE_WRAP_S, GL_CLAMP_TO_EDGE);
 		glTexParameterf(GL_TEXTURE_2D, GL_TEXTURE_WRAP_T, GL_CLAMP_TO_EDGE);
 		glTexParameterf(GL_TEXTURE_2D, GL_TEXTURE_MAG_FILTER, GL_NEAREST);
@@ -256,21 +256,13 @@ void Scene::SetupViewPort(int w, int h) {
 		glTexImage2D(GL_TEXTURE_2D, 0, GL_RGBA, w, h, 0, GL_RGBA, GL_UNSIGNED_BYTE, NULL);
 		glBindTexture(GL_TEXTURE_2D, 0);
         
-        glBindTexture(GL_TEXTURE_2D, depthTextures_[1]);
-        glTexParameterf(GL_TEXTURE_2D, GL_TEXTURE_WRAP_S, GL_CLAMP_TO_EDGE);
-        glTexParameterf(GL_TEXTURE_2D, GL_TEXTURE_WRAP_T, GL_CLAMP_TO_EDGE);
-        glTexParameterf(GL_TEXTURE_2D, GL_TEXTURE_MAG_FILTER, GL_NEAREST);
-        glTexParameterf(GL_TEXTURE_2D, GL_TEXTURE_MIN_FILTER, GL_NEAREST);
-        glTexImage2D(GL_TEXTURE_2D, 0, GL_RGBA, w, h, 0, GL_RGBA, GL_UNSIGNED_BYTE, NULL);
-        glBindTexture(GL_TEXTURE_2D, 0);
-
 		glGenRenderbuffers(1, &rboId_);
 		glBindRenderbuffer(GL_RENDERBUFFER, rboId_);
 		glRenderbufferStorage(GL_RENDERBUFFER, GL_DEPTH_COMPONENT16, w, h);
 		glBindRenderbuffer(GL_RENDERBUFFER, 0);
 
 		// Set the texture to be at the color attachment point of the FBO (we pack depth 32 bits in color)
-		glFramebufferTexture2D(GL_FRAMEBUFFER, GL_COLOR_ATTACHMENT0, GL_TEXTURE_2D, depthTextures_[0], 0);
+		glFramebufferTexture2D(GL_FRAMEBUFFER, GL_COLOR_ATTACHMENT0, GL_TEXTURE_2D, depthTexture_, 0);
 		glFramebufferRenderbuffer(GL_FRAMEBUFFER, GL_DEPTH_ATTACHMENT, GL_RENDERBUFFER, rboId_);
 
 		GLuint status = glCheckFramebufferStatus(GL_FRAMEBUFFER);
@@ -349,8 +341,9 @@ std::vector<glm::vec4> computeFrustumPlanes(const glm::mat4 & mat, bool normaliz
 /**
  * Tells whether or not b is intersecting f.
  * http://www.txutxi.com/?p=584
- * @param f Viewing frustum.
+ * @param planes Viewing frustum.
- * @param b An axis aligned bounding box.
+ * @param boxMin The axis aligned bounding box min.
+ * @param boxMax The axis aligned bounding box max.
  * @return True if b intersects f, false otherwise.
  */
 bool intersectFrustumAABB(
@@ -389,7 +382,8 @@ bool intersectFrustumAABB(
 }
 
 //Should only be called in OpenGL thread!
-int Scene::Render(const float * uvsTransformed, glm::mat4 arViewMatrix, glm::mat4 arProjectionMatrix, const rtabmap::Mesh & occlusionMesh, bool mapping) {
+int Scene::Render(const float * uvsTransformed, glm::mat4 arViewMatrix, glm::mat4 arProjectionMatrix, const rtabmap::Mesh & occlusionMesh, bool mapping)
+{
 	UASSERT(gesture_camera_ != 0);
 
 	if(currentPose_ == 0)
@@ -428,7 +422,7 @@ int Scene::Render(const float * uvsTransformed, glm::mat4 arViewMatrix, glm::mat
 
 	if(renderBackgroundCamera)
 	{
-        if(projectionMatrix[0][0] > arProjectionMatrix[0][0]-0.2)
+        if(projectionMatrix[0][0] > arProjectionMatrix[0][0]-0.3)
         {
             projectionMatrix = arProjectionMatrix;
             viewMatrix = arViewMatrix;
@@ -486,9 +480,15 @@ int Scene::Render(const float * uvsTransformed, glm::mat4 arViewMatrix, glm::mat
 		glDisable(GL_CULL_FACE);
 	}
 
-	UTimer timer;
+	bool onlineBlending =
+        (!meshRendering_ &&
+         occlusionMesh.cloud.get() &&
+         occlusionMesh.cloud->size()) ||
+        (blending_ &&
+         gesture_camera_->GetCameraType()!=tango_gl::GestureCamera::kTopOrtho &&
+         mapRendering_ && meshRendering_ &&
+         (cloudsToDraw.size() > 1 || (renderBackgroundCamera && wireFrame_)));
 
-	bool onlineBlending = (!meshRendering_ && occlusionMesh.cloud.get() && occlusionMesh.cloud->size()) || (blending_ && gesture_camera_->GetCameraType()!=tango_gl::GestureCamera::kTopOrtho && mapRendering_ && meshRendering_);
 	if(onlineBlending && fboId_)
 	{
         GLint originid = 0;
@@ -499,7 +499,7 @@ int Scene::Render(const float * uvsTransformed, glm::mat4 arViewMatrix, glm::mat
 
 		glClearColor(0, 0, 0, 0);
 		glClear(GL_DEPTH_BUFFER_BIT | GL_COLOR_BUFFER_BIT);
-
+        
         // Draw scene
         for(std::vector<PointCloudDrawable*>::const_iterator iter=cloudsToDraw.begin(); iter!=cloudsToDraw.end(); ++iter)
         {
@@ -511,16 +511,12 @@ int Scene::Render(const float * uvsTransformed, glm::mat4 arViewMatrix, glm::mat
             (*iter)->Render(projectionMatrix, viewMatrix, meshRendering_, pointSize_, false, false, distanceToCameraSqr, 0, 0, 0, 0, 0, true);
         }
         
-        glBindTexture(GL_TEXTURE_2D, depthTextures_[1]);
-        glCopyTexSubImage2D(GL_TEXTURE_2D, 0, 0, 0, 0, 0, screenWidth_, screenHeight_);
-        glBindTexture(GL_TEXTURE_2D, 0);
-        
         if(!meshRendering_ && occlusionMesh.cloud.get() && occlusionMesh.cloud->size())
         {
             PointCloudDrawable drawable(occlusionMesh);
             drawable.Render(projectionMatrix, viewMatrix, true, pointSize_, false, false, 0, 0, 0, 0, 0, 0, true);
         }
-
+        
 		// back to normal window-system-provided framebuffer
 		glBindFramebuffer(GL_FRAMEBUFFER, originid); // unbind
 	}
@@ -559,7 +555,7 @@ int Scene::Render(const float * uvsTransformed, glm::mat4 arViewMatrix, glm::mat
 	glClearColor(r_, g_, b_, 1.0f);
 	glClear(GL_DEPTH_BUFFER_BIT | GL_COLOR_BUFFER_BIT);
     
-    if(renderBackgroundCamera && !onlineBlending)
+    if(renderBackgroundCamera && (!onlineBlending || !meshRendering_))
     {
         background_renderer_->Draw(uvsTransformed, 0, screenWidth_, screenHeight_, false);
 
@@ -633,14 +629,14 @@ int Scene::Render(const float * uvsTransformed, glm::mat4 arViewMatrix, glm::mat
 				cloud->getPose().z() - openglCamera.z());
 		float distanceToCameraSqr = cloudToCamera[0]*cloudToCamera[0] + cloudToCamera[1]*cloudToCamera[1] + cloudToCamera[2]*cloudToCamera[2];
 
-		cloud->Render(projectionMatrix, viewMatrix, meshRendering_, pointSize_, meshRenderingTexture_, lighting_, distanceToCameraSqr, onlineBlending?depthTextures_[0]:0, screenWidth_, screenHeight_, gesture_camera_->getNearClipPlane(), gesture_camera_->getFarClipPlane(), false, wireFrame_);
+		cloud->Render(projectionMatrix, viewMatrix, meshRendering_, pointSize_, meshRenderingTexture_, lighting_, distanceToCameraSqr, onlineBlending?depthTexture_:0, screenWidth_, screenHeight_, gesture_camera_->getNearClipPlane(), gesture_camera_->getFarClipPlane(), false, wireFrame_);
 	}
 
 	if(onlineBlending)
 	{
-        if(renderBackgroundCamera)
+        if(renderBackgroundCamera && meshRendering_)
         {
-            background_renderer_->Draw(uvsTransformed, depthTextures_[1], screenWidth_, screenHeight_, meshRendering_?mapping:false);
+            background_renderer_->Draw(uvsTransformed, depthTexture_, screenWidth_, screenHeight_, mapping);
         }
         
 		glDisable (GL_BLEND);

app/android/jni/scene.h

@@ -208,7 +208,7 @@ class Scene {
   float b_;
   GLuint fboId_;
   GLuint rboId_;
-  GLuint depthTextures_[2]; // 0=objects+occlusion 1=objects only
+  GLuint depthTexture_; // 0=objects+occlusion
   GLsizei screenWidth_;
   GLsizei screenHeight_;
   bool doubleTapOn_;

app/android/jni/tango-gl/gesture_camera.cpp

@@ -183,7 +183,7 @@ void GestureCamera::SetCameraType(CameraType camera_index) {
     case kFirstPerson:
       SetOrthoMode(false);
       SetFieldOfView(kLowestFov);
-      SetNearFarClipPlanes(0.1, 50);
+      SetNearFarClipPlanes(0.25, 25);
       SetPosition(glm::vec3(0.0f, 0.0f, 0.0f));
       SetRotation(glm::quat(1.0f, 0.0f, 0.0f, 0.0f));
       cam_cur_dist_ = 0.0f;

app/android/res/values/strings.xml

@@ -19,7 +19,7 @@
     <string name="light_off">Lighting</string>
     <string name="wireframe">Wireframe</string>
     <string name="close_visualization">Close Visualization</string>
-    <string name="save_to_file">Export to File&#8230;</string>
+    <string name="save_to_file">Export OBJ/PLY&#8230;</string>
     <string name="share_to_sketchfab">Share to Sketchfab&#8230;</string>
     <string name="start">Start</string>
     <string name="nodes">"Nodes (WM): "</string>
@@ -80,7 +80,7 @@
     <string name="pref_default_fisheye">false</string>
     
     <string name="pref_key_camera_driver">pref_key_camera_driver</string>
-    <string name="pref_default_camera_driver">0</string>
+    <string name="pref_default_camera_driver">-1</string>
     <string name="pref_key_depth_from_motion">pref_key_depth_from_motion</string>
     <string name="pref_default_depth_from_motion">false</string>
     <string name="pref_key_arcore_localization_filtering_speed">pref_key_arcore_localization_filtering_speed</string>
@@ -108,7 +108,7 @@
     <string name="pref_key_features_type">pref_key_features_type</string>
     <string name="pref_default_features_type">6</string>
     <string name="pref_key_optimizer">pref_key_optimizer</string>
-    <string name="pref_default_optimizer">1</string>
+    <string name="pref_default_optimizer">2</string>
     <string name="pref_key_optimize_end">pref_key_optimize_end</string>
     <string name="pref_default_optimize_end">true</string>
     <string name="pref_key_marker_detection">pref_key_marker_detection</string>
@@ -381,12 +381,14 @@
     <string name="pref_summary_db_in_memory">The database is kept in RAM for fast access. Set to false to reduce RAM used at the cost of slower access. This parameter is applied on reset or when a database is opened.</string>
 
     <string-array name="pref_camera_driver_keys">
+        <item>"Auto"</item>
         <item>"Google Tango NDK"</item>
         <item>"ARCore NDK"</item>
         <item>"AREngine NDK"</item>
         <item>"ARCore Java"</item>
     </string-array>
     <string-array name="pref_camera_driver_values">
+        <item>"-1"</item>
         <item>"0"</item>
         <item>"1"</item>
         <item>"2"</item>

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

@@ -688,7 +688,7 @@ public class ARCoreSharedCamera {
 					if(mToast!=null && previousAnchorPose != null)
 					{
 						String msg = "Tracking lost! If you are mapping, you will need to relocalize before continuing.";
-						if(!mToast.getView().isShown())
+						if(mToast.getView() == null || !mToast.getView().isShown())
 						{
 							mToast.makeText(mActivity.getApplicationContext(), 
 									msg, Toast.LENGTH_LONG).show();
@@ -736,7 +736,7 @@ public class ARCoreSharedCamera {
 										+ "because of high speed detected (%f m/s) causing a jump! You can change "
 										+ "ARCore localization filtering speed in Settings->Mapping if you are "
 										+ "indeed moving as fast.", speed);
-								if(!mToast.getView().isShown())
+								if(mToast.getView() == null || !mToast.getView().isShown())
 								{
 									mToast.makeText(mActivity.getApplicationContext(), msg, Toast.LENGTH_LONG).show();
 								}

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

@@ -2,6 +2,7 @@ package com.introlab.rtabmap;
 
 import java.io.File;
 import java.io.FileInputStream;
+import java.io.FileNotFoundException;
 import java.io.FileOutputStream;
 import java.io.IOException;
 import java.io.InputStream;
@@ -25,6 +26,8 @@ import android.app.NotificationManager;
 import android.app.PendingIntent;
 import android.app.ProgressDialog;
 import android.content.ComponentName;
+import android.content.ContentResolver;
+import android.content.ContentValues;
 import android.content.Context;
 import android.content.DialogInterface;
 import android.content.DialogInterface.OnShowListener;
@@ -35,6 +38,7 @@ import android.content.pm.ApplicationInfo;
 import android.content.pm.PackageInfo;
 import android.content.pm.PackageManager;
 import android.content.pm.PackageManager.NameNotFoundException;
+import android.database.Cursor;
 import android.hardware.Camera;
 import android.hardware.Sensor;
 import android.hardware.SensorEvent;
@@ -46,6 +50,7 @@ import android.hardware.display.DisplayManager;
 import android.location.Location;
 import android.location.LocationListener;
 import android.location.LocationManager;
+import android.media.MediaScannerConnection;
 import android.net.Uri;
 import android.net.wifi.WifiInfo;
 import android.net.wifi.WifiManager;
@@ -56,6 +61,8 @@ import android.os.Handler;
 import android.os.IBinder;
 import android.os.Message;
 import android.preference.PreferenceManager;
+import android.provider.MediaStore;
+import android.provider.OpenableColumns;
 import android.support.v4.app.FragmentActivity;
 import android.support.v4.content.FileProvider;
 import android.text.InputType;
@@ -256,6 +263,8 @@ public class RTABMapActivity extends FragmentActivity implements OnClickListener
 
 	ARCoreSharedCamera mArCoreCamera = null;
 	int mCameraDriver = 0;
+	
+	private String mIntentDbToOpen = null;
 
 	//Tango Service connection.
 	boolean mCameraServiceConnectionUsed = false; 
@@ -294,7 +303,7 @@ public class RTABMapActivity extends FragmentActivity implements OnClickListener
 		}
 
 		public void onServiceDisconnected(ComponentName name) {
-			// Handle this if you need to gracefully shutdown/retry
+			// Handle this if you need to gracefully shutsaveDatabasedown/retry
 			// in the event that Tango itself crashes/gets upgraded while running.
 			mToast.makeText(getApplicationContext(), 
 					String.format("Tango disconnected!"), mToast.LENGTH_LONG).show();
@@ -494,10 +503,11 @@ public class RTABMapActivity extends FragmentActivity implements OnClickListener
 		mWorkingDirectoryHuman = "";
 		mTotalLoopClosures = 0;
 		mLastFastMovementNotificationStamp = System.currentTimeMillis()/1000;
-
+		
-		if(Environment.getExternalStorageState().compareTo(Environment.MEDIA_MOUNTED)==0)
+		if(Environment.getExternalStorageState().compareTo(Environment.MEDIA_MOUNTED)==0 && 
+				getActivity().getExternalFilesDirs(null).length >=1)
 		{
-			File extStore = Environment.getExternalStorageDirectory();
+			File extStore = getActivity().getExternalFilesDirs(null)[0];
 			mWorkingDirectory = extStore.getAbsolutePath() + "/" + getString(R.string.app_name) + "/";
 			extStore = new File(mWorkingDirectory);
 			extStore.mkdirs();
@@ -507,7 +517,7 @@ public class RTABMapActivity extends FragmentActivity implements OnClickListener
 		{
 			// show warning that data cannot be saved!
 			mToast.makeText(getApplicationContext(), 
-					String.format("Failed to get external storage path (SD-CARD, state=%s). Saving disabled.", 
+					String.format("Failed to get external storage path (state=%s). Saving disabled.", 
 							Environment.getExternalStorageState()), mToast.LENGTH_LONG).show();
 		}
 
@@ -574,33 +584,141 @@ public class RTABMapActivity extends FragmentActivity implements OnClickListener
 
 		DISABLE_LOG =  !( 0 != ( getApplicationInfo().flags & ApplicationInfo.FLAG_DEBUGGABLE ) );
 
-		if (!PermissionHelper.hasPermission(this, Manifest.permission.WRITE_EXTERNAL_STORAGE)) {
-			PermissionHelper.requestPermission(this, Manifest.permission.WRITE_EXTERNAL_STORAGE);
-		}
-		else
-		{
-			postCreate();
-		}
-
 		SharedPreferences sharedPref = PreferenceManager.getDefaultSharedPreferences(this);
 		String cameraDriverStr = sharedPref.getString(getString(R.string.pref_key_camera_driver), getString(R.string.pref_default_camera_driver));
 		mCameraDriver = Integer.parseInt(cameraDriverStr);
 		
 		isArCoreAvailable();
 		isArEngineAvailable();
+		
+		if (!PermissionHelper.hasPermission(this, Manifest.permission.WRITE_EXTERNAL_STORAGE)) {
+			PermissionHelper.requestPermission(this, Manifest.permission.WRITE_EXTERNAL_STORAGE);
+		}
+		else
+		{
+			// Get intent, action and MIME type
+		    Intent intent = getIntent();
+		    String action = intent.getAction();
+		    String type = intent.getType();
+
+		    if (Intent.ACTION_SEND.equals(action) && type != null) {
+		        if ("application/octet-stream".equals(type)) {
+		        	Uri imageUri = (Uri) intent.getParcelableExtra(Intent.EXTRA_STREAM);
+		            if (imageUri != null) {
+		               String fileName = getFileName(imageUri);
+		               Log.i(TAG, "Intent received: " + imageUri.getPath() + " Name:" + fileName);
+		               if(fileName.endsWith(".db"))
+		               {
+			               File file = new File(mWorkingDirectory+fileName);
+			               if(file.exists())
+			               {
+			            	   mToast.makeText(this, fileName + " already exists in RTAB-Map's library! Cannot be copied.", mToast.LENGTH_LONG).show();
+			               }
+			               else
+			               {
+				               copy(imageUri, file);
+				               mIntentDbToOpen = fileName;
+			               }
+		               }
+		            }
+		        }
+		    } else if (Intent.ACTION_SEND_MULTIPLE.equals(action) && type != null) {
+		    	if (type.startsWith("application/")) {
+		    		ArrayList<Uri> imageUris = intent.getParcelableArrayListExtra(Intent.EXTRA_STREAM);
+		    		if (imageUris != null) {
+		    			boolean added = false;
+		    			for(Uri imageUri: imageUris)
+		    			{
+		    				String fileName = getFileName(imageUri);
+		    				Log.i(TAG, "Intent received: " + imageUri.getPath() + " Name:" + fileName);
+		    				if(fileName.endsWith(".db"))
+		    				{
+		    					File file = new File(mWorkingDirectory+"/"+getFileName(imageUri));
+		    					if(!file.exists())
+		    					{
+			    					copy(imageUri, file);
+			    					added = true;
+		    					}
+		    					else
+		    					{
+		    						Log.e(TAG,  fileName + " already exists in RTAB-Map's library! Cannot be copied.");
+		    					}
+		    				}
+		    			}
+		    			if(added)
+		    			{
+		    				openDatabase();
+		    			}
+		    		}
+
+		    	}
+		    }
+
+			postCreate();
+		}
 	}
 	
+	public void copy(File src, File dst) throws IOException {
+		InputStream in = new FileInputStream(src);
+		OutputStream out = new FileOutputStream(dst);
+
+		// Transfer bytes from in to out
+		byte[] buf = new byte[1024];
+		int len;
+		while ((len = in.read(buf)) > 0) {
+			out.write(buf, 0, len);
+		}
+		in.close();
+		out.close();
+	}
+	
+	public void copy(Uri uri, File file)
+	{
+		InputStream in;
+		try {
+			in = getApplicationContext().getContentResolver().openInputStream(uri);
+			
+			OutputStream out = new FileOutputStream(file);
+    		byte[] buf = new byte[1024];
+    		int len;
+    		while ((len = in.read(buf)) > 0) {
+    			out.write(buf, 0, len);
+    		}
+    		in.close();
+    		out.close();
+		} catch (IOException e) {
+			Log.e(TAG, e.getMessage());
+		}
+	}
+	
+	public String getFileName(Uri uri) {
+		  String result = null;
+		  if (uri.getScheme().equals("content")) {
+		    Cursor cursor = getContentResolver().query(uri, null, null, null, null);
+		    try {
+		      if (cursor != null && cursor.moveToFirst()) {
+		        result = cursor.getString(cursor.getColumnIndex(OpenableColumns.DISPLAY_NAME));
+		      }
+		    } finally {
+		      cursor.close();
+		    }
+		  }
+		  if (result == null) {
+		    result = uri.getPath();
+		    int cut = result.lastIndexOf('/');
+		    if (cut != -1) {
+		      result = result.substring(cut + 1);
+		    }
+		  }
+		  return result;
+		}
+
+	
 	// Should be called only if read/write permissions are granted!
 	private void postCreate()
 	{
 		Log.i(TAG, "postCreate()");
 		
-		String tmpDatabase = mWorkingDirectory+RTABMAP_TMP_DB;
-		(new File(tmpDatabase)).delete();
-		SharedPreferences sharedPref = PreferenceManager.getDefaultSharedPreferences(this);
-		boolean databaseInMemory = sharedPref.getBoolean(getString(R.string.pref_key_db_in_memory), Boolean.parseBoolean(getString(R.string.pref_default_db_in_memory)));
-		RTABMapLib.openDatabase(nativeApplication, tmpDatabase, databaseInMemory, false);
-		
 		final String[] files = Util.loadFileList(mWorkingDirectory, true);
 		if(files.length == 0)
 		{
@@ -613,6 +731,17 @@ public class RTABMapActivity extends FragmentActivity implements OnClickListener
 		Log.i(TAG, String.format("updateCameraDriverSettings() mCameraDriver=%d RTABMapLib.isBuiltWith(%d)=%d", mCameraDriver, mCameraDriver, RTABMapLib.isBuiltWith(nativeApplication, mCameraDriver)?1:0));
 		
 		SharedPreferences sharedPref = PreferenceManager.getDefaultSharedPreferences(this);
+		String cameraDriverStr = sharedPref.getString(getString(R.string.pref_key_camera_driver), getString(R.string.pref_default_camera_driver));
+		mCameraDriver = Integer.parseInt(cameraDriverStr);
+		
+		if(mCameraDriver == -1)
+		{
+			// Prioritize tango if available
+			mCameraDriver = 0;
+			SharedPreferences.Editor editor = sharedPref.edit();
+			editor.putString(getString(R.string.pref_key_camera_driver), "0");
+			editor.commit();
+		}
 		
 		if(mCameraDriver == 0 && (!CheckTangoCoreVersion(MIN_TANGO_CORE_VERSION) || !RTABMapLib.isBuiltWith(nativeApplication, 0)))
 		{
@@ -1167,7 +1296,7 @@ public class RTABMapActivity extends FragmentActivity implements OnClickListener
 		final boolean depthFromMotion = sharedPref.getBoolean(getString(R.string.pref_key_depth_from_motion), Boolean.parseBoolean(getString(R.string.pref_default_depth_from_motion)));
 		mCameraDriver = Integer.parseInt(cameraDriverStr);
 
-		if(!DISABLE_LOG) Log.i(TAG, String.format("startCamera() driver=%d", mCameraDriver));
+		Log.i(TAG, String.format("startCamera() driver=%d", mCameraDriver));
 		if(mCameraDriver == 0) // Tango
 		{
 			// Check if the Tango Core is out dated.
@@ -1490,7 +1619,7 @@ public class RTABMapActivity extends FragmentActivity implements OnClickListener
 		}
 
 		updateState(mState);
-
+		
 		return true;
 	}
 
@@ -1992,6 +2121,7 @@ public class RTABMapActivity extends FragmentActivity implements OnClickListener
 				final int loopDetected = RTABMapLib.postProcessing(nativeApplication, -1);
 				runOnUiThread(new Runnable() {
 					public void run() {
+						updateState(State.STATE_IDLE);
 						if(mExportProgressDialog.isShowing())
 						{
 							mExportProgressDialog.dismiss();
@@ -2024,8 +2154,6 @@ public class RTABMapActivity extends FragmentActivity implements OnClickListener
 							mProgressDialog.dismiss();
 							mToast.makeText(getActivity(), String.format("Optimization canceled"), mToast.LENGTH_LONG).show();
 						}
-
-						updateState(State.STATE_IDLE);
 					}
 				});
 			} 
@@ -2144,7 +2272,7 @@ public class RTABMapActivity extends FragmentActivity implements OnClickListener
 			mButtonCloseVisualization.setVisibility(mHudVisible && mState != State.STATE_VISUALIZING_CAMERA?View.VISIBLE:View.INVISIBLE);
 			mButtonCloseVisualization.setEnabled(true);
 			mButtonSaveOnDevice.setVisibility(mHudVisible && mState != State.STATE_VISUALIZING_CAMERA?View.VISIBLE:View.INVISIBLE);
-			mButtonShareOnSketchfab.setVisibility(mHudVisible && mState != State.STATE_VISUALIZING_CAMERA?View.VISIBLE:View.INVISIBLE);
+			//mButtonShareOnSketchfab.setVisibility(mHudVisible && mState != State.STATE_VISUALIZING_CAMERA?View.VISIBLE:View.INVISIBLE);
 			mButtonLibrary.setVisibility(View.INVISIBLE);
 			mButtonNewScan.setVisibility(View.INVISIBLE);
 			mItemSave.setEnabled(mState != State.STATE_VISUALIZING_CAMERA);
@@ -2217,6 +2345,12 @@ public class RTABMapActivity extends FragmentActivity implements OnClickListener
 		{
 			mGLView.setRenderMode(GLSurfaceView.RENDERMODE_CONTINUOUSLY);
 		}
+		
+		if(mState == State.STATE_WELCOME && mIntentDbToOpen != null)
+		{
+			openDatabase(mIntentDbToOpen, false);
+			mIntentDbToOpen = null;
+		}
 	}
 
 	private void startMapping() {
@@ -2626,7 +2760,7 @@ public class RTABMapActivity extends FragmentActivity 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)));
 							String tmpDatabase = mWorkingDirectory+RTABMAP_TMP_DB;
-							RTABMapLib.openDatabase(nativeApplication, tmpDatabase, databaseInMemory, false);
+							RTABMapLib.openDatabase(nativeApplication, tmpDatabase, databaseInMemory, false, true);
 
 							mItemLocalizationMode.setEnabled(!mItemDataRecorderMode.isChecked());		   
 
@@ -2780,16 +2914,110 @@ public class RTABMapActivity extends FragmentActivity implements OnClickListener
 		mOpenedDatabasePath = "";
 		SharedPreferences sharedPref = PreferenceManager.getDefaultSharedPreferences(this);
 		boolean databaseInMemory = sharedPref.getBoolean(getString(R.string.pref_key_db_in_memory), Boolean.parseBoolean(getString(R.string.pref_default_db_in_memory)));
-		String tmpDatabase = mWorkingDirectory+RTABMAP_TMP_DB;
+		final String tmpDatabase = mWorkingDirectory+RTABMAP_TMP_DB;
-		RTABMapLib.openDatabase(nativeApplication, tmpDatabase, databaseInMemory, false);
+		
-
+		File newFile = new File(tmpDatabase);
-		if(!(mState == State.STATE_CAMERA || mState ==State.STATE_MAPPING))
+		final int fileSizeMB = (int)newFile.length()/(1024 * 1024);
+		if(!(mState == State.STATE_CAMERA || mState ==State.STATE_MAPPING) &&
+			newFile.exists() && 
+			fileSizeMB>1) // >1MB
 		{
-			setCamera(1);
+			AlertDialog d2 = new AlertDialog.Builder(getActivity())
-			startCamera(String.format("Hold Tight! Initializing Camera Service...\n"
+					.setCancelable(false)
-					+ "Tip: If the camera is still drifting just after the mapping has started, do \"Reset\"."));
+					.setTitle("Recovery")
+					.setMessage(String.format("The previous session (%d MB) was not correctly saved, do you want to recover it?", fileSizeMB))
+					.setNegativeButton("Ignore", new DialogInterface.OnClickListener() {
+						public void onClick(DialogInterface dialog, int which) {
+							(new File(tmpDatabase)).delete();
+							newScan();
+						}
+					})
+					.setNeutralButton("Cancel", new DialogInterface.OnClickListener() {
+						public void onClick(DialogInterface dialog, int which) {
+							// do nothing
+						}
+					})
+					.setPositiveButton("Yes", new DialogInterface.OnClickListener() {
+						public void onClick(DialogInterface dialog, int which) {
+							final String fileName = new SimpleDateFormat("yyMMdd-HHmmss").format(new Date()) + ".db";
+							final String outputDbPath = mWorkingDirectory + fileName;
+							
+							mExportProgressDialog.setTitle("Recovering");
+							mExportProgressDialog.setMessage(String.format("Please wait while recovering data..."));
+							mExportProgressDialog.setProgress(0);
+
+							final State previousState = mState;
+
+							mExportProgressDialog.show();
+							updateState(State.STATE_PROCESSING);
+
+							Thread exportThread = new Thread(new Runnable() {
+								public void run() {
+
+									final long startTime = System.currentTimeMillis()/1000;
+
+									final boolean success = RTABMapLib.recover(
+											nativeApplication, 
+											tmpDatabase,
+											outputDbPath);
+									runOnUiThread(new Runnable() {
+										public void run() {
+											if(mExportProgressDialog.isShowing())
+											{							
+												if(success)
+												{								
+													AlertDialog d2 = new AlertDialog.Builder(getActivity())
+															.setCancelable(false)
+															.setTitle("Database saved!")
+															.setMessage(String.format("Database \"%s\" (%d MB) successfully saved!", fileName, fileSizeMB))
+															.setPositiveButton("OK", new DialogInterface.OnClickListener() {
+																public void onClick(DialogInterface dialog, int which) {
+																	openDatabase(fileName, false);
+																}
+															})
+															.create();
+													d2.setCanceledOnTouchOutside(true);
+													d2.show();
+												}
+												else
+												{
+													updateState(previousState);
+													mToast.makeText(getActivity(), String.format("Recovery failed!"), mToast.LENGTH_LONG).show();
+												}
+												mExportProgressDialog.dismiss();
+											}
+											else
+											{
+												mToast.makeText(getActivity(), String.format("Recovery canceled"), mToast.LENGTH_LONG).show();
+												updateState(previousState);
+											}
+										}
+									});
+								} 
+							});
+							exportThread.start();
+							
+							refreshSystemMediaScanDataBase(getActivity(), outputDbPath);
+						}
+					})
+					.create();
+			d2.setCanceledOnTouchOutside(false);
+			d2.show();
+		}
+		else
+		{
+		
+			RTABMapLib.openDatabase(nativeApplication, tmpDatabase, databaseInMemory, false, true);
+	
+			if(!(mState == State.STATE_CAMERA || mState ==State.STATE_MAPPING))
+			{
+				setCamera(0);
+				startCamera(String.format("Hold Tight! Initializing Camera Service...\n"
+						+ "Tip: If the camera is still drifting just after the mapping has started, do \"Reset\"."));
+			}
 		}
 	}
+		
 
 	private void openDatabase()
 	{
@@ -3215,7 +3443,7 @@ public class RTABMapActivity extends FragmentActivity implements OnClickListener
 		context.sendBroadcast(mediaScanIntent);
 	}
 
-	private void saveDatabase(String fileName)
+	private void saveDatabase(final String fileName)
 	{
 		final String newDatabasePath = mWorkingDirectory + fileName + ".db";
 		final String newDatabasePathHuman = mWorkingDirectoryHuman + fileName + ".db";
@@ -3247,7 +3475,7 @@ public class RTABMapActivity extends FragmentActivity implements OnClickListener
 							mSavedStamp = System.currentTimeMillis();
 							msg = String.format("Database saved to \"%s\".", newDatabasePathHuman);	
 						}
-
+									            
 						// build notification
 						Intent intent = new Intent(getActivity(), RTABMapActivity.class);
 						// use System.currentTimeMillis() to have a unique ID for the pending intent
@@ -3280,7 +3508,7 @@ public class RTABMapActivity extends FragmentActivity implements OnClickListener
 						AlertDialog d2 = new AlertDialog.Builder(getActivity())
 								.setCancelable(false)
 								.setTitle("Database saved!")
-								.setMessage(String.format("Database \"%s\" (%d MB) successfully saved on the SD-CARD!", newDatabasePathHuman, fileSizeMB))
+								.setMessage(String.format("Database \"%s\" (%d MB) successfully saved!", newDatabasePathHuman, fileSizeMB))
 								.setPositiveButton("OK", new DialogInterface.OnClickListener() {
 									public void onClick(DialogInterface dialog, int which) {
 										resetNoTouchTimer(true);
@@ -3334,30 +3562,7 @@ public class RTABMapActivity extends FragmentActivity implements OnClickListener
 				dialog.dismiss();
 				if(!fileName.isEmpty())
 				{
-					File newFile = new File(mWorkingDirectory + RTABMAP_EXPORT_DIR + fileName + ".zip");
+					writeExportedFiles(fileName);
-					if(newFile.exists())
-					{
-						AlertDialog ad = new AlertDialog.Builder(getActivity())
-								.setCancelable(false)
-								.setTitle("File Already Exists")
-								.setMessage("Do you want to overwrite the existing file?")
-								.setPositiveButton("Yes", new DialogInterface.OnClickListener() {
-									public void onClick(DialogInterface dialog, int which) {
-										writeExportedFiles(fileName);
-									}
-								})
-								.setNegativeButton("No", new DialogInterface.OnClickListener() {
-									public void onClick(DialogInterface dialog, int which) {
-										saveOnDevice();
-									}
-								}).create();
-						ad.setCanceledOnTouchOutside(false);
-						ad.show();
-					}
-					else
-					{
-						writeExportedFiles(fileName);
-					}
 				}
 			}
 		});
@@ -3371,7 +3576,7 @@ public class RTABMapActivity extends FragmentActivity implements OnClickListener
 	{		
 		Log.i(TAG, String.format("Write exported mesh to \"%s\"", fileName));
 
-		mProgressDialog.setTitle("Saving to sd-card");
+		mProgressDialog.setTitle("Exporting");
 		mProgressDialog.setMessage(String.format("Compressing the files..."));
 		mProgressDialog.show();
 
@@ -3397,8 +3602,23 @@ public class RTABMapActivity extends FragmentActivity implements OnClickListener
 				}
 				File exportDir = new File(mWorkingDirectory + RTABMAP_EXPORT_DIR);
 				exportDir.mkdirs();
+				
+				// cleanup old zip
+				fileNames = Util.loadFileList(mWorkingDirectory + RTABMAP_EXPORT_DIR, false);
+				if(!DISABLE_LOG) Log.i(TAG, String.format("Deleting %d files in \"%s\"", fileNames.length, mWorkingDirectory + RTABMAP_EXPORT_DIR));
+				for(int i=0; i<fileNames.length; ++i)
+				{
+					File f = new File(mWorkingDirectory + RTABMAP_EXPORT_DIR + "/" + fileNames[i]);
+					if(f.delete())
+					{
+						if(!DISABLE_LOG) Log.i(TAG, String.format("Deleted \"%s\"", f.getPath()));
+					}
+					else
+					{
+						if(!DISABLE_LOG) Log.i(TAG, String.format("Failed deleting \"%s\"", f.getPath()));
+					}
+				}
 
-				final String pathHuman = mWorkingDirectoryHuman + RTABMAP_EXPORT_DIR + fileName + ".zip";
 				final String zipOutput = mWorkingDirectory+RTABMAP_EXPORT_DIR+fileName+".zip";
 				if(RTABMapLib.writeExportedMesh(nativeApplication, mWorkingDirectory + RTABMAP_TMP_DIR, RTABMAP_TMP_FILENAME))
 				{							
@@ -3424,7 +3644,7 @@ public class RTABMapActivity extends FragmentActivity implements OnClickListener
 							final String msg = e.getMessage();
 							runOnUiThread(new Runnable() {
 								public void run() {
-									mToast.makeText(getActivity(), String.format("Exporting mesh \"%s\" failed! Error=%s", pathHuman, msg), mToast.LENGTH_LONG).show();
+									mToast.makeText(getActivity(), String.format("Exporting mesh \"%s\" failed! Error=%s", fileName, msg), mToast.LENGTH_LONG).show();
 								}
 							});
 						}
@@ -3440,29 +3660,41 @@ public class RTABMapActivity extends FragmentActivity implements OnClickListener
 							final File f = new File(zipOutput);
 							final int fileSizeMB = (int)f.length()/(1024 * 1024);
 
-							AlertDialog d = new AlertDialog.Builder(getActivity())
+							// Save to public Documents/RTAB-Map folder
-									.setCancelable(false)
+							/*ContentValues values = new ContentValues();
-									.setTitle("Mesh Saved!")
+				            values.put(MediaStore.MediaColumns.DISPLAY_NAME, fileName); //file name                     
-									.setMessage(String.format("Mesh \"%s\" (%d MB) successfully exported on the SD-CARD! Share it?", pathHuman, fileSizeMB))
+				            values.put(MediaStore.MediaColumns.MIME_TYPE, "application/zip");        //file extension, will automatically add to file
-									.setPositiveButton("Yes", new DialogInterface.OnClickListener() {
+				            values.put(MediaStore.MediaColumns.RELATIVE_PATH, Environment.DIRECTORY_DOCUMENTS + "/RTAB-Map");     //end "/" is not mandatory
-										public void onClick(DialogInterface dialog, int which) {
+				            Uri uri = getContentResolver().insert(MediaStore.Files.getContentUri("external"),values);
-											// Send to...
+				            if (uri != null) {
-											Intent shareIntent = new Intent();
+				            	OutputStream out;
-											shareIntent.setAction(Intent.ACTION_SEND);
+								try {
-											shareIntent.putExtra(Intent.EXTRA_STREAM, Uri.fromFile(f));
+									out = getApplicationContext().getContentResolver().openOutputStream(uri);
-											shareIntent.setType("application/zip");
+									
-											startActivity(Intent.createChooser(shareIntent, "Sharing..."));
+									InputStream in = new FileInputStream(zipOutput);
+					        		byte[] buf = new byte[1024];
+					        		int len;
+					        		while ((len = in.read(buf)) > 0) {
+					        			out.write(buf, 0, len);
+					        		}
+					        		in.close();
+					        		out.close();
+					        		
+					        		f.delete(); // remove private file
+								} catch (IOException e) {
+									Log.e(TAG, e.getMessage());
+								}
+				            } */
 
-											resetNoTouchTimer(true);
+							// Send to...
-										}
+							Intent shareIntent = new Intent();
-									})
+							shareIntent.setAction(Intent.ACTION_SEND);
-									.setNegativeButton("No", new DialogInterface.OnClickListener() {
+							shareIntent.putExtra(Intent.EXTRA_STREAM, FileProvider.getUriForFile(getActivity(), getActivity().getApplicationContext().getPackageName() + ".provider", f));
-										public void onClick(DialogInterface dialog, int which) {
+							shareIntent.addFlags(Intent.FLAG_GRANT_READ_URI_PERMISSION);
-											resetNoTouchTimer(true);
+							shareIntent.setType("application/zip");
-										}
+							startActivity(Intent.createChooser(shareIntent, "Sharing..."));
-									}).create();
+
-							d.setCanceledOnTouchOutside(false);
+							resetNoTouchTimer(true);
-							d.show();
 						}
 					});
 				}
@@ -3471,7 +3703,7 @@ public class RTABMapActivity extends FragmentActivity implements OnClickListener
 					runOnUiThread(new Runnable() {
 						public void run() {
 							mProgressDialog.dismiss();
-							mToast.makeText(getActivity(), String.format("Exporting mesh \"%s\" failed! No files found in tmp directory!? Last export may have failed or have been canceled.", pathHuman), mToast.LENGTH_LONG).show();
+							mToast.makeText(getActivity(), String.format("Exporting mesh \"%s\" failed! No files found in tmp directory!? Last export may have failed or have been canceled.", fileName), mToast.LENGTH_LONG).show();
 							resetNoTouchTimer(true);
 						}
 					});
@@ -3498,8 +3730,7 @@ public class RTABMapActivity extends FragmentActivity implements OnClickListener
 		Thread openThread = new Thread(new Runnable() {
 			public void run() {
 
-				final String tmpDatabase = mWorkingDirectory+RTABMAP_TMP_DB;				
+				final int status = RTABMapLib.openDatabase(nativeApplication, mOpenedDatabasePath, databaseInMemory, optimize, false);
-				final int status = RTABMapLib.openDatabase2(nativeApplication, mOpenedDatabasePath, tmpDatabase, databaseInMemory, optimize);
 
 				runOnUiThread(new Runnable() {
 					public void run() {
@@ -3578,20 +3809,6 @@ public class RTABMapActivity extends FragmentActivity implements OnClickListener
 		openThread.start();	
 	}
 
-	public void copy(File src, File dst) throws IOException {
-		InputStream in = new FileInputStream(src);
-		OutputStream out = new FileOutputStream(dst);
-
-		// Transfer bytes from in to out
-		byte[] buf = new byte[1024];
-		int len;
-		while ((len = in.read(buf)) > 0) {
-			out.write(buf, 0, len);
-		}
-		in.close();
-		out.close();
-	}
-
 	private void shareToSketchfab()
 	{
 		if (!PermissionHelper.hasPermission(this, Manifest.permission.INTERNET)) {

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

@@ -34,9 +34,10 @@ public class RTABMapLib
     
     public static native void setScreenRotation(long nativeApplication, int displayRotation, int cameraRotation);
     
-    public static native int openDatabase(long nativeApplication, String databasePath, boolean databaseInMemory, boolean optimize);
+    public static native int openDatabase(long nativeApplication, String databasePath, boolean databaseInMemory, boolean optimize, boolean clearDatabase);
-    public static native int openDatabase2(long nativeApplication, String databaseSource, String databasePath, boolean databaseInMemory, boolean optimize);
     
+    public static native boolean recover(long nativeApplication, String from, String to);
+
     public static native boolean isBuiltWith(long nativeApplication, int cameraDriver);
     public static native boolean startCamera(long nativeApplication, IBinder binder, Context context, Activity activity, int driver);
     public static native void stopCamera(long nativeApplication);

app/ios/RTABMapApp.xcodeproj/project.pbxproj

@@ -979,9 +979,10 @@
 				ASSETCATALOG_COMPILER_APPICON_NAME = AppIcon;
 				ASSETCATALOG_COMPILER_GLOBAL_ACCENT_COLOR_NAME = AccentColor;
 				CLANG_CXX_LIBRARY = "libc++";
+				CLANG_USE_OPTIMIZATION_PROFILE = NO;
 				CODE_SIGN_IDENTITY = "Apple Development";
 				CODE_SIGN_STYLE = Automatic;
-				CURRENT_PROJECT_VERSION = 8;
+				CURRENT_PROJECT_VERSION = 13;
 				DEFINES_MODULE = YES;
 				DEVELOPMENT_TEAM = 3RRB6NV8U9;
 				EXCLUDED_ARCHS = "";
@@ -1006,7 +1007,7 @@
 					"$(PROJECT_DIR)/RTABMapApp/Libraries/lib",
 					"$(PROJECT_DIR)/RTABMapApp/Libraries/share/OpenCV/3rdparty/lib",
 				);
-				MARKETING_VERSION = 0.20.16;
+				MARKETING_VERSION = 0.20.17;
 				OTHER_CFLAGS = "";
 				PRODUCT_BUNDLE_IDENTIFIER = com.introlab.rtabmap;
 				PRODUCT_NAME = "$(TARGET_NAME)";
@@ -1038,7 +1039,7 @@
 				CLANG_USE_OPTIMIZATION_PROFILE = NO;
 				CODE_SIGN_IDENTITY = "Apple Development";
 				CODE_SIGN_STYLE = Automatic;
-				CURRENT_PROJECT_VERSION = 8;
+				CURRENT_PROJECT_VERSION = 13;
 				DEFINES_MODULE = YES;
 				DEVELOPMENT_TEAM = 3RRB6NV8U9;
 				FRAMEWORK_SEARCH_PATHS = (
@@ -1063,7 +1064,7 @@
 					"$(PROJECT_DIR)/RTABMapApp/Libraries/lib",
 					"$(PROJECT_DIR)/RTABMapApp/Libraries/share/OpenCV/3rdparty/lib",
 				);
-				MARKETING_VERSION = 0.20.16;
+				MARKETING_VERSION = 0.20.17;
 				ONLY_ACTIVE_ARCH = YES;
 				OTHER_CFLAGS = "";
 				PRODUCT_BUNDLE_IDENTIFIER = com.introlab.rtabmap;

app/ios/RTABMapApp.xcodeproj/xcshareddata/xcschemes/RTABMapApp.xcscheme

@@ -39,6 +39,7 @@
       ignoresPersistentStateOnLaunch = "NO"
       debugDocumentVersioning = "YES"
       debugServiceExtension = "internal"
+      enableGPUFrameCaptureMode = "2"
       allowLocationSimulation = "YES">
       <BuildableProductRunnable
          runnableDebuggingMode = "0">

app/ios/RTABMapApp/Base.lproj/LaunchScreen.storyboard

@@ -1,8 +1,9 @@
 <?xml version="1.0" encoding="UTF-8"?>
-<document type="com.apple.InterfaceBuilder3.CocoaTouch.Storyboard.XIB" version="3.0" toolsVersion="18122" targetRuntime="iOS.CocoaTouch" propertyAccessControl="none" useAutolayout="YES" launchScreen="YES" useTraitCollections="YES" useSafeAreas="YES" colorMatched="YES" initialViewController="01J-lp-oVM">
+<document type="com.apple.InterfaceBuilder3.CocoaTouch.Storyboard.XIB" version="3.0" toolsVersion="19455" targetRuntime="iOS.CocoaTouch" propertyAccessControl="none" useAutolayout="YES" launchScreen="YES" useTraitCollections="YES" useSafeAreas="YES" colorMatched="YES" initialViewController="01J-lp-oVM">
     <device id="retina6_1" orientation="portrait" appearance="light"/>
     <dependencies>
-        <plugIn identifier="com.apple.InterfaceBuilder.IBCocoaTouchPlugin" version="18093"/>
+        <deployment identifier="iOS"/>
+        <plugIn identifier="com.apple.InterfaceBuilder.IBCocoaTouchPlugin" version="19454"/>
         <capability name="Safe area layout guides" minToolsVersion="9.0"/>
         <capability name="documents saved in the Xcode 8 format" minToolsVersion="8.0"/>
     </dependencies>

app/ios/RTABMapApp/Base.lproj/Main.storyboard

@@ -1,8 +1,9 @@
 <?xml version="1.0" encoding="UTF-8"?>
-<document type="com.apple.InterfaceBuilder3.CocoaTouch.Storyboard.XIB" version="3.0" toolsVersion="18122" targetRuntime="iOS.CocoaTouch" propertyAccessControl="none" useAutolayout="YES" useTraitCollections="YES" useSafeAreas="YES" colorMatched="YES" initialViewController="zah-iI-EPt">
+<document type="com.apple.InterfaceBuilder3.CocoaTouch.Storyboard.XIB" version="3.0" toolsVersion="19455" targetRuntime="iOS.CocoaTouch" propertyAccessControl="none" useAutolayout="YES" useTraitCollections="YES" useSafeAreas="YES" colorMatched="YES" initialViewController="zah-iI-EPt">
     <device id="retina3_5" orientation="portrait" appearance="light"/>
     <dependencies>
-        <plugIn identifier="com.apple.InterfaceBuilder.IBCocoaTouchPlugin" version="18093"/>
+        <deployment identifier="iOS"/>
+        <plugIn identifier="com.apple.InterfaceBuilder.IBCocoaTouchPlugin" version="19454"/>
         <capability name="Image references" minToolsVersion="12.0"/>
         <capability name="Safe area layout guides" minToolsVersion="9.0"/>
         <capability name="System colors in document resources" minToolsVersion="11.0"/>

app/ios/RTABMapApp/NativeWrapper.cpp

@@ -69,24 +69,11 @@ void setScreenRotationNative(const void *object, int displayRotation)
     }
 }
 
-int openDatabaseNative(const void *object, const char * databasePath, bool databaseInMemory, bool optimize)
+int openDatabaseNative(const void *object, const char * databasePath, bool databaseInMemory, bool optimize, bool clearDatabase)
 {
     if(object)
     {
-        return native(object)->openDatabase(databasePath, databaseInMemory, optimize);
+        return native(object)->openDatabase(databasePath, databaseInMemory, optimize, clearDatabase);
-    }
-    else
-    {
-        UERROR("object is null!");
-        return -1;
-    }
-}
-
-int openDatabase2Native(const void *object, const char * databaseSource, const char * databasePath, bool databaseInMemory, bool optimize)
-{
-    if(object)
-    {
-        return native(object)->openDatabase(databasePath, databaseInMemory, optimize, databaseSource);
     }
     else
     {
@@ -107,6 +94,31 @@ void saveNative(const void *object, const char * databasePath)
     }
 }
 
+bool recoverNative(const void *object, const char * from, const char * to)
+{
+    if(object)
+    {
+        return native(object)->recover(from, to);
+    }
+    else
+    {
+        UERROR("object is null!");
+    }
+    return false;
+}
+
+void cancelProcessingNative(const void *object)
+{
+    if(object)
+    {
+        native(object)->cancelProcessing();
+    }
+    else
+    {
+        UERROR("object is null!");
+    }
+}
+
 int postProcessingNative(const void *object, int approach)
 {
     if(object)

app/ios/RTABMapApp/NativeWrapper.hpp

@@ -32,9 +32,10 @@ void setupCallbacksNative(const void *object, void * classPtr,
                                                    float, float, float, float, float, float));
 void destroyNativeApplication(const void *object);
 void setScreenRotationNative(const void *object, int displayRotation);
-int openDatabaseNative(const void *object, const char * databasePath, bool databaseInMemory, bool optimize);
+int openDatabaseNative(const void *object, const char * databasePath, bool databaseInMemory, bool optimize, bool clearDatabase);
-int openDatabase2Native(const void *object, const char * databaseSource, const char * databasePath, bool databaseInMemory, bool optimize);
 void saveNative(const void *object, const char * databasePath);
+bool recoverNative(const void *object, const char * from, const char * to);
+void cancelProcessingNative(const void * object);
 int postProcessingNative(const void *object, int approach);
 bool exportMeshNative(
             const void *object,

app/ios/RTABMapApp/RTABMap.swift

@@ -113,17 +113,9 @@ class RTABMap {
         setupGraphicNative(native_rtabmap, Int32(size.width), Int32(size.height));
     }
     
-    func openDatabase(databasePath:String, databaseInMemory:Bool, optimize:Bool) -> Int {
+    func openDatabase(databasePath:String, databaseInMemory:Bool, optimize:Bool, clearDatabase: Bool) -> Int {
         databasePath.utf8CString.withUnsafeBufferPointer { buffer -> Int in
-            return Int(openDatabaseNative(native_rtabmap, buffer.baseAddress, databaseInMemory, optimize))
+            return Int(openDatabaseNative(native_rtabmap, buffer.baseAddress, databaseInMemory, optimize, clearDatabase))
-        }
-    }
-    
-    func openDatabase(databaseSource:String, databasePath:String, databaseInMemory:Bool, optimize:Bool) -> Int {
-        databasePath.utf8CString.withUnsafeBufferPointer { buffer -> Int in
-            databaseSource.utf8CString.withUnsafeBufferPointer { bufferSource -> Int in
-                return Int(openDatabase2Native(native_rtabmap, bufferSource.baseAddress, buffer.baseAddress, databaseInMemory, optimize))
-            }
         }
     }
     
@@ -133,6 +125,18 @@ class RTABMap {
         }
     }
     
+    func recover(from: String, to: String) -> Bool {
+        from.utf8CString.withUnsafeBufferPointer { bufferFrom -> Bool in
+            to.utf8CString.withUnsafeBufferPointer { bufferTo -> Bool in
+                return recoverNative(native_rtabmap, bufferFrom.baseAddress, bufferTo.baseAddress)
+            }
+        }
+    }
+    
+    func cancelProcessing() {
+        cancelProcessingNative(native_rtabmap);
+    }
+    
     func postProcessing(approach: Int) -> Int {
         return Int(postProcessingNative(native_rtabmap, Int32(approach)))
     }
@@ -486,7 +490,7 @@ func getPreviewImage(databasePath: String) -> UIImage?
             let bitmap = CIImage(bitmapData: data, bytesPerRow: Int(image.width*image.channels), size: CGSize(width: Int(image.width), height: Int(image.height)), format: CIFormat.BGRA8, colorSpace: nil)
             return UIImage(ciImage: bitmap)
         }
-        return nil
+        return UIImage(named: "RTAB-Map1024")
     }
     return imageOut
 }

app/ios/RTABMapApp/ViewController.swift

@@ -8,6 +8,7 @@
 import GLKit
 import ARKit
 import Zip
+import StoreKit
 
 extension Array {
     func size() -> Int {
@@ -41,6 +42,8 @@ class ViewController: GLKViewController, ARSessionDelegate, RTABMapObserver, UIP
     private var mMaxFeatures: Int = 0
     private var mLoopThr = 0.11
     
+    private var mReviewRequested = false
+    
     // UI states
     private enum State {
         case STATE_WELCOME,    // Camera/Motion off - showing only buttons open and start new scan
@@ -1414,12 +1417,19 @@ class ViewController: GLKViewController, ARSessionDelegate, RTABMapObserver, UIP
         
         present(alertController, animated: true)
         
-        setGLCamera(type: 1);
+        setGLCamera(type: 0);
         startCamera();
     }
     
     func newScan()
     {
+        print("databases.size() = \(databases.size())")
+        if(databases.count >= 5 && !mReviewRequested && self.depthSupported)
+        {
+            SKStoreReviewController.requestReviewInCurrentScene()
+            mReviewRequested = true
+        }
+        
         if(mState == State.STATE_VISUALIZING)
         {
             closeVisualization()
@@ -1428,14 +1438,120 @@ class ViewController: GLKViewController, ARSessionDelegate, RTABMapObserver, UIP
         mMapNodes = 0;
         self.openedDatabasePath = nil
         let tmpDatabase = self.getTmpDirectory().appendingPathComponent(self.RTABMAP_TMP_DB)
-        
         let inMemory = UserDefaults.standard.bool(forKey: "DatabaseInMemory")
-        self.rtabmap!.openDatabase(databasePath: tmpDatabase.path, databaseInMemory: inMemory, optimize: false)
+        if(!(self.mState == State.STATE_CAMERA || self.mState == State.STATE_MAPPING) &&
-
+           FileManager.default.fileExists(atPath: tmpDatabase.path) &&
-        if(!(self.mState == State.STATE_CAMERA || self.mState == State.STATE_MAPPING))
+           tmpDatabase.fileSize > 1024*1024) // > 1MB
         {
-            self.setGLCamera(type: 1);
+            dismiss(animated: true, completion: {
-            self.startCamera();
+                let msg = "The previous session (\(tmpDatabase.fileSizeString)) was not correctly saved, do you want to recover it?"
+                let alert = UIAlertController(title: "Recovery", message: msg, preferredStyle: .alert)
+                let alertActionNo = UIAlertAction(title: "Ignore", style: .destructive) {
+                    (UIAlertAction) -> Void in
+                    do {
+                        try FileManager.default.removeItem(at: tmpDatabase)
+                    }
+                    catch {
+                        print("Could not clear tmp database: \(error)")
+                    }
+                    self.newScan()
+                }
+                alert.addAction(alertActionNo)
+                let alertActionCancel = UIAlertAction(title: "Cancel", style: .cancel) {
+                    (UIAlertAction) -> Void in
+                    // do nothing
+                }
+                alert.addAction(alertActionCancel)
+                let alertActionYes = UIAlertAction(title: "Yes", style: .default) {
+                    (UIAlertAction2) -> Void in
+
+                    let fileName = Date().getFormattedDate(format: "yyMMdd-HHmmss") + ".db"
+                    let outputDbPath = self.getDocumentDirectory().appendingPathComponent(fileName).path
+                    
+                    var indicator: UIActivityIndicatorView?
+                    
+                    let alertView = UIAlertController(title: "Recovering", message: "Please wait while recovering data...", preferredStyle: .alert)
+                    let alertViewActionCancel = UIAlertAction(title: "Cancel", style: .cancel) {
+                        (UIAlertAction) -> Void in
+                        self.dismiss(animated: true, completion: {
+                            self.progressView = nil
+                            
+                            indicator = UIActivityIndicatorView(style: .large)
+                            indicator?.frame = CGRect(x: 0.0, y: 0.0, width: 60.0, height: 60.0)
+                            indicator?.center = self.view.center
+                            self.view.addSubview(indicator!)
+                            indicator?.bringSubviewToFront(self.view)
+                            
+                            indicator?.startAnimating()
+                            self.rtabmap!.cancelProcessing();
+                        })
+                    }
+                    alertView.addAction(alertViewActionCancel)
+                    
+                    let previousState = self.mState
+                    self.updateState(state: .STATE_PROCESSING);
+                    
+                    self.present(alertView, animated: true, completion: {
+                        //  Add your progressbar after alert is shown (and measured)
+                        let margin:CGFloat = 8.0
+                        let rect = CGRect(x: margin, y: 84.0, width: alertView.view.frame.width - margin * 2.0 , height: 2.0)
+                        self.progressView = UIProgressView(frame: rect)
+                        self.progressView!.progress = 0
+                        self.progressView!.tintColor = self.view.tintColor
+                        alertView.view.addSubview(self.progressView!)
+                        
+                        var success : Bool = false
+                        DispatchQueue.background(background: {
+                            
+                            success = self.rtabmap!.recover(from: tmpDatabase.path, to: outputDbPath)
+                            
+                        }, completion:{
+                            if(indicator != nil)
+                            {
+                                indicator!.stopAnimating()
+                                indicator!.removeFromSuperview()
+                            }
+                            if self.progressView != nil
+                            {
+                                self.dismiss(animated: self.openedDatabasePath == nil, completion: {
+                                    if(success)
+                                    {
+                                        let alertSaved = UIAlertController(title: "Database saved!", message: String(format: "Database \"%@\" successfully recovered!", fileName), preferredStyle: .alert)
+                                        let yes = UIAlertAction(title: "OK", style: .default) {
+                                            (UIAlertAction) -> Void in
+                                            self.openDatabase(fileUrl: URL(fileURLWithPath: outputDbPath))
+                                        }
+                                        alertSaved.addAction(yes)
+                                        self.present(alertSaved, animated: true, completion: nil)
+                                    }
+                                    else
+                                    {
+                                        self.updateState(state: previousState);
+                                        self.showToast(message: "Recovery failed!", seconds: 4)
+                                    }
+                                })
+                            }
+                            else
+                            {
+                                self.showToast(message: "Recovery canceled", seconds: 2)
+                                self.updateState(state: previousState);
+                            }
+                        })
+                    })
+                }
+                alert.addAction(alertActionYes)
+                self.present(alert, animated: true, completion: nil)
+            })
+        }
+        else
+        {
+            self.rtabmap!.openDatabase(databasePath: tmpDatabase.path, databaseInMemory: inMemory, optimize: false, clearDatabase: true)
+
+            if(!(self.mState == State.STATE_CAMERA || self.mState == State.STATE_MAPPING))
+            {
+                self.setGLCamera(type: 0);
+                self.startCamera();
+            }
         }
     }
     
@@ -1519,12 +1635,19 @@ class ViewController: GLKViewController, ARSessionDelegate, RTABMapObserver, UIP
             
             self.openedDatabasePath = URL(fileURLWithPath: filePath)
             
-            let alert = UIAlertController(title: "Database saved!", message: String(format: "Database \"%@\" successfully saved on the SD-CARD!", fileName), preferredStyle: .alert)
+            let alert = UIAlertController(title: "Database saved!", message: String(format: "Database \"%@\" successfully saved!", fileName), preferredStyle: .alert)
             let yes = UIAlertAction(title: "OK", style: .default) {
                 (UIAlertAction) -> Void in
             }
             alert.addAction(yes)
             self.present(alert, animated: true, completion: nil)
+            do {
+                let tmpDatabase = self.getTmpDirectory().appendingPathComponent(self.RTABMAP_TMP_DB)
+                try FileManager.default.removeItem(at: tmpDatabase)
+            }
+            catch {
+                print("Could not clear tmp database: \(error)")
+            }
             self.updateDatabases()
             self.updateState(state: previousState)
         })
@@ -1560,6 +1683,8 @@ class ViewController: GLKViewController, ARSessionDelegate, RTABMapObserver, UIP
                 indicator?.bringSubviewToFront(self.view)
                 
                 indicator?.startAnimating()
+                
+                self.rtabmap!.cancelProcessing()
             })
             
         }))
@@ -1664,6 +1789,7 @@ class ViewController: GLKViewController, ARSessionDelegate, RTABMapObserver, UIP
         alertView.addAction(UIAlertAction(title: "Cancel", style: .cancel, handler: { _ in
             self.dismiss(animated: true)
             self.progressView = nil
+            self.rtabmap!.cancelProcessing()
         }))
 
         let previousState = mState
@@ -1800,7 +1926,6 @@ class ViewController: GLKViewController, ARSessionDelegate, RTABMapObserver, UIP
             stopMapping(ignoreSaving: true)
         }
         
-        let tmpDatabase = self.getTmpDirectory().appendingPathComponent(self.RTABMAP_TMP_DB)
         openedDatabasePath = fileUrl;
         let fileName: String = self.openedDatabasePath!.lastPathComponent
         
@@ -1812,7 +1937,7 @@ class ViewController: GLKViewController, ARSessionDelegate, RTABMapObserver, UIP
         updateState(state: .STATE_PROCESSING);
         var status = 0
         DispatchQueue.background(background: {
-            status = self.rtabmap!.openDatabase(databaseSource: self.openedDatabasePath!.path, databasePath: tmpDatabase.path, databaseInMemory: true, optimize: false)
+            status = self.rtabmap!.openDatabase(databasePath: self.openedDatabasePath!.path, databaseInMemory: true, optimize: false, clearDatabase: false)
         }, completion:{
             // main thread
             if(status == -1) {
@@ -1974,6 +2099,7 @@ class ViewController: GLKViewController, ARSessionDelegate, RTABMapObserver, UIP
         alertView.addAction(UIAlertAction(title: "Cancel", style: .cancel, handler: { _ in
             self.dismiss(animated: true)
             self.progressView = nil
+            self.rtabmap!.cancelProcessing()
         }))
         
         let previousState = mState;
@@ -2211,6 +2337,14 @@ extension Date {
         dateformat.dateFormat = format
         return dateformat.string(from: self)
     }
+    
+    var millisecondsSince1970:Int64 {
+        Int64((self.timeIntervalSince1970 * 1000.0).rounded())
+    }
+    
+    init(milliseconds:Int64) {
+        self = Date(timeIntervalSince1970: TimeInterval(milliseconds) / 1000)
+    }
 }
 
 extension DispatchQueue {
@@ -2323,3 +2457,11 @@ extension UserDefaults {
         setDefaultsFromSettingsBundle()
     }
 }
+
+extension SKStoreReviewController {
+    public static func requestReviewInCurrentScene() {
+        if let scene = UIApplication.shared.connectedScenes.first(where: { $0.activationState == .foregroundActive }) as? UIWindowScene {
+            requestReview(in: scene)
+        }
+    }
+}

app/ios/Settings.bundle/License.plist

@@ -13,7 +13,7 @@
 			<string>
 ======= RTAB-Map =======
 RTAB-Map - https://github.com/introlab/rtabmap
-Copyright (c) 2010-2021, Mathieu Labbe - IntRoLab - Universite de Sherbrooke, all rights reserved.
+Copyright (c) 2010-2022, Mathieu Labbe - IntRoLab - Universite de Sherbrooke, all rights reserved.
 Copyright (c) XXX, contributors, all rights reserved.
 
 Redistribution and use in source and binary forms, with or without

app/ios/Settings.bundle/Root.plist

@@ -14,7 +14,7 @@
 		</dict>
 		<dict>
 			<key>DefaultValue</key>
-			<integer>2</integer>
+			<integer>1</integer>
 			<key>Key</key>
 			<string>PointCloudDensity</string>
 			<key>Title</key>
@@ -454,7 +454,7 @@
 		</dict>
 		<dict>
 			<key>FooterText</key>
-			<string>Copyright (c) 2010-2021, Mathieu Labbe - IntRoLab - Université de Sherbrooke. All rights reserved.</string>
+			<string>Copyright (c) 2010-2022, Mathieu Labbe - IntRoLab - Université de Sherbrooke. All rights reserved.</string>
 			<key>Title</key>
 			<string>About</string>
 			<key>Type</key>
@@ -462,7 +462,7 @@
 		</dict>
 		<dict>
 			<key>DefaultValue</key>
-			<string>0.20.16</string>
+			<string>0.20.17</string>
 			<key>Key</key>
 			<string>Version</string>
 			<key>Title</key>

app/src/CMakeLists.txt

@@ -135,6 +135,24 @@ IF(BUILD_AS_BUNDLE AND (APPLE OR WIN32))
           COMPONENT runtime)
 	ENDIF(WIN32)
   ENDIF(k4a_FOUND)
+  
+  IF(Torch_FOUND)
+	# Install needed cudnn_ops_infer64_8.dll and cudnn_cnn_infer64_8.dll
+	# TODO: should be a more general way to include them if version is different
+    IF(WIN32 AND CUDA_FOUND)
+		find_file(CUDNN_OPS_DLL NAMES cudnn_ops_infer64_8.dll)
+		find_file(CUDNN_CNN_DLL NAMES cudnn_cnn_infer64_8.dll)
+		IF(CUDNN_OPS_DLL AND CUDNN_CNN_DLL)
+			MESSAGE(STATUS "Found ${CUDNN_OPS_DLL}")
+			MESSAGE(STATUS "Found ${CUDNN_CNN_DLL}")
+			INSTALL(FILES ${CUDNN_OPS_DLL} ${CUDNN_CNN_DLL}
+			  DESTINATION ${plugin_dest_dir} 
+			  COMPONENT runtime)
+		ELSE()
+			MESSAGE(AUTHOR_WARNING "Using Torch with CUDA, but cudnn_ops_infer64_8.dll and cudnn_cnn_infer64_8.dll are not found on the PATH, so it won't be added to package.")
+		ENDIF()
+	ENDIF(WIN32 AND CUDA_FOUND)
+  ENDIF(Torch_FOUND)
 
   # Install needed Qt plugins by copying directories from the qt installation
   # One can cull what gets copied by using 'REGEX "..." EXCLUDE'
@@ -154,17 +172,17 @@ IF(BUILD_AS_BUNDLE AND (APPLE OR WIN32))
 		list(GET loc_list 1 plugin_type)
 		IF(NOT plugin_root)
 			get_filename_component(plugin_root ${plugin_dir} DIRECTORY)
-		ENDIF(NOT plugin_root)
+		ENDIF(NOT plugin_root)
 		#MESSAGE(STATUS "Qt5 plugin \"${plugin_loc}\" installed in \"${plugin_dest_dir}/plugins${plugin_type}\"")
 		INSTALL(FILES ${plugin_loc}
           DESTINATION ${plugin_dest_dir}/plugins${plugin_type}
           COMPONENT runtime)
-	endforeach()
+	endforeach()
 	IF(NOT Qt5Widgets_VERSION VERSION_LESS 5.10.0)
 		IF(WIN32)
-			SET(plugin_loc "${plugin_root}/styles/qwindowsvistastyle.dll")
+			SET(plugin_loc "${plugin_root}/styles/qwindowsvistastyle.dll")
-		ELSEIF(APPLE)
+		ELSEIF(APPLE)
-			SET(plugin_loc "${plugin_root}/styles/libqmacstyle.dylib")
+			SET(plugin_loc "${plugin_root}/styles/libqmacstyle.dylib")
 		ENDIF()
 		IF(EXISTS ${plugin_loc})
 			get_filename_component(plugin_dir ${plugin_loc} DIRECTORY)

cmake_modules/FindG2O.cmake

@@ -1,4 +1,5 @@
 #Pre-requisites: Look for csparse
+MESSAGE(STATUS "RTAB-Map's cmake g2o find module used for convenience (for older g2o versions)")
 FIND_PATH(CSPARSE_INCLUDE_DIR NAMES cs.h PATH_SUFFIXES suitesparse csparse EXTERNAL/suitesparse EXTERNAL/csparse g2o/EXTERNAL/suitesparse g2o/EXTERNAL/csparse
   PATHS "C:\\Program Files\\g2o\\include\\EXTERNAL")
 FIND_LIBRARY(CSPARSE_LIBRARY NAMES cxsparse g2o_ext_csparse 
@@ -21,6 +22,10 @@ FIND_FILE(G2O_CONFIG_FILE g2o/config.h
   PATHS ${G2O_INCLUDE_DIR}
   NO_DEFAULT_PATH)
 
+FIND_FILE(G2O_FACTORY_FILE g2o/core/factory.h 
+  PATHS ${G2O_INCLUDE_DIR}
+  NO_DEFAULT_PATH)
+
 #ifdef G2O_NUMBER_FORMAT_STR
 #define G2O_CPP11 // we assume that if G2O_NUMBER_FORMAT_STR is defined, this is the new g2o code with c++11 interface
 #endif
@@ -83,7 +88,7 @@ ENDIF(G2O_SOLVER_CHOLMOD OR G2O_SOLVER_CSPARSE OR G2O_SOLVER_DENSE OR G2O_SOLVER
 
 # G2O itself declared found if we found the core libraries and at least one solver
 SET(G2O_FOUND "NO")
-IF(G2O_STUFF_LIBRARY AND G2O_CORE_LIBRARY AND G2O_INCLUDE_DIR AND G2O_CONFIG_FILE AND G2O_SOLVERS_FOUND)
+IF(G2O_STUFF_LIBRARY AND G2O_CORE_LIBRARY AND G2O_INCLUDE_DIR AND G2O_CONFIG_FILE AND G2O_FACTORY_FILE AND G2O_SOLVERS_FOUND)
   SET(G2O_INCLUDE_DIRS ${G2O_INCLUDE_DIR})
   SET(G2O_LIBRARIES 
 	${G2O_CORE_LIBRARY}
@@ -120,8 +125,16 @@ IF(G2O_STUFF_LIBRARY AND G2O_CORE_LIBRARY AND G2O_INCLUDE_DIR AND G2O_CONFIG_FIL
     SET(G2O_CPP11 0)
   ELSE()
     MESSAGE(WARNING "Latest g2o version detected with c++11 interface (config file: ${G2O_CONFIG_FILE}). Make sure g2o is built with \"-DBUILD_WITH_MARCH_NATIVE=OFF\" to avoid segmentation faults caused by Eigen.")
-    SET(G2O_CPP11 1)
+    FILE(READ ${G2O_FACTORY_FILE} TMPTXT)
+    STRING(FIND "${TMPTXT}" "shared_ptr" matchres)
+    IF(${matchres} EQUAL -1)
+      MESSAGE(STATUS "Old g2o factory version detected without shared ptr (factory file: ${G2O_FACTORY_FILE}).")
+      SET(G2O_CPP11 2)
+    ELSE()
+      MESSAGE(STATUS "Latest g2o factory version detected with shared ptr (factory file: ${G2O_FACTORY_FILE}).")
+      SET(G2O_CPP11 1)
+    ENDIF()
   ENDIF()
 
   SET(G2O_FOUND "YES")
-ENDIF(G2O_STUFF_LIBRARY AND G2O_CORE_LIBRARY AND G2O_INCLUDE_DIR AND G2O_CONFIG_FILE AND G2O_SOLVERS_FOUND)
+ENDIF(G2O_STUFF_LIBRARY AND G2O_CORE_LIBRARY AND G2O_INCLUDE_DIR AND G2O_CONFIG_FILE AND G2O_FACTORY_FILE AND G2O_SOLVERS_FOUND)

corelib/include/rtabmap/core/Graph.h

@@ -125,7 +125,8 @@ void RTABMAP_EXP computeMaxGraphErrors(
 		float & maxLinearError,
 		float & maxAngularError,
 		const Link ** maxLinearErrorLink = 0,
-		const Link ** maxAngularErrorLink = 0);
+		const Link ** maxAngularErrorLink = 0,
+		bool for3DoF = false);
 
 std::vector<double> RTABMAP_EXP getMaxOdomInf(const std::multimap<int, Link> & links);
 
@@ -266,54 +267,58 @@ std::list<std::pair<int, Transform> > RTABMAP_EXP computePath(
 		float angularVelocity = 0.0f); // rad/sec
 
 /**
- * Get the nearest node of the target pose
+ * Find the nearest node of the target pose
  * @param nodes the nodes to search for
  * @param targetPose the target pose to search around
  * @param distance squared distance of the nearest node found (optional)
  * @return the node id.
  */
 int RTABMAP_EXP findNearestNode(
-		const std::map<int, rtabmap::Transform> & nodes,
+		const std::map<int, rtabmap::Transform> & poses,
 		const rtabmap::Transform & targetPose,
 		float * distance = 0);
 
 /**
- * Get K nearest nodes of the target pose
+ * Find the nearest nodes of the query pose or node
- * @param nodes the nodes to search for
- * @param targetPose the target pose to search around
- * @param k number of nearest neighbors to search for
- * @return the node ids with squared distance to target pose.
- */
-std::map<int, float> RTABMAP_EXP findNearestNodes(
-		const std::map<int, rtabmap::Transform> & nodes,
-		const rtabmap::Transform & targetPose,
-		int k);
-
-/**
- * Get nodes near the query
  * @param nodeId the query id
  * @param nodes the nodes to search for
- * @param radius radius to search for (m)
+ * @param radius radius to search for (m), if 0, k should be > 0.
+ * @param k max nearest neighbors (0=all inside the radius)
  * @return the nodes with squared distance to query node.
  */
-std::map<int, float> RTABMAP_EXP getNodesInRadius(
+std::map<int, float> RTABMAP_EXP findNearestNodes(
 		int nodeId,
-		const std::map<int, Transform> & nodes,
+		const std::map<int, Transform> & poses,
-		float radius);
+		float radius,
-std::map<int, float> RTABMAP_EXP getNodesInRadius(
+		float angle = 0.0f,
+		int k=0);
+std::map<int, float> RTABMAP_EXP findNearestNodes(
 		const Transform & targetPose,
-		const std::map<int, Transform> & nodes,
+		const std::map<int, Transform> & poses,
-		float radius);
+		float radius,
-std::map<int, Transform> RTABMAP_EXP getPosesInRadius(
+		float angle = 0.0f,
+		int k=0);
+std::map<int, Transform> RTABMAP_EXP findNearestPoses(
 		int nodeId,
-		const std::map<int, Transform> & nodes,
+		const std::map<int, Transform> & poses,
 		float radius,
-		float angle = 0.0f);
+		float angle = 0.0f,
-std::map<int, Transform> RTABMAP_EXP getPosesInRadius(
+		int k=0);
+std::map<int, Transform> RTABMAP_EXP findNearestPoses(
 		const Transform & targetPose,
-		const std::map<int, Transform> & nodes,
+		const std::map<int, Transform> & poses,
 		float radius,
-		float angle = 0.0f);
+		float angle = 0.0f,
+		int k=0);
+
+// typedef hack to avoid error with RTABMAP_DEPRECATED
+typedef std::map<int, float> _mapIntFloat;
+typedef std::map<int, Transform> _mapIntTransform;
+RTABMAP_DEPRECATED(_mapIntFloat RTABMAP_EXP findNearestNodes(const std::map<int, rtabmap::Transform> & nodes, const rtabmap::Transform & targetPose, int k), "Use new findNearestNodes() interface with radius=0, angle=0.");
+RTABMAP_DEPRECATED(_mapIntFloat RTABMAP_EXP getNodesInRadius(int nodeId, const std::map<int, Transform> & nodes, float radius), "Renamed to findNearestNodes()");
+RTABMAP_DEPRECATED(_mapIntFloat RTABMAP_EXP getNodesInRadius(const Transform & targetPose, const std::map<int, Transform> & nodes, float radius), "Renamed to findNearestNodes()");
+RTABMAP_DEPRECATED(_mapIntTransform RTABMAP_EXP getPosesInRadius(int nodeId, const std::map<int, Transform> & nodes, float radius, float angle = 0.0f), "Renamed to findNearestNodes()");
+RTABMAP_DEPRECATED(_mapIntTransform RTABMAP_EXP getPosesInRadius(const Transform & targetPose, const std::map<int, Transform> & nodes, float radius, float angle = 0.0f), "Renamed to findNearestNodes()");
 
 float RTABMAP_EXP computePathLength(
 		const std::vector<std::pair<int, Transform> > & path,

corelib/include/rtabmap/core/Memory.h

@@ -320,6 +320,7 @@ private:
 	float _laserScanVoxelSize;
 	int _laserScanNormalK;
 	float _laserScanNormalRadius;
+	float _laserScanGroundNormalsUp;
 	bool _reextractLoopClosureFeatures;
 	bool _localBundleOnLoopClosure;
 	float _rehearsalMaxDistance;

corelib/include/rtabmap/core/OccupancyGrid.h

@@ -122,6 +122,7 @@ private:
 	bool projMapFrame_;
 	float maxObstacleHeight_;
 	int normalKSearch_;
+	float groundNormalsUp_;
 	float maxGroundAngle_;
 	float clusterRadius_;
 	int minClusterSize_;

corelib/include/rtabmap/core/Optimizer.h

@@ -75,7 +75,8 @@ public:
 			const std::map<int, Transform> & posesIn,
 			const std::multimap<int, Link> & linksIn,
 			std::map<int, Transform> & posesOut,
-			std::multimap<int, Link> & linksOut) const;
+			std::multimap<int, Link> & linksOut,
+			bool adjustPosesWithConstraints = true) const;
 
 public:
 	virtual ~Optimizer() {}

corelib/include/rtabmap/core/Parameters.h

@@ -385,7 +385,8 @@ class RTABMAP_EXP Parameters
     RTABMAP_PARAM(RGBD, ProximityPathRawPosesUsed,    bool, true,  "When comparing to a local path for one-to-many proximity detection, merge the scans using the odometry poses (with neighbor link optimizations) instead of the ones in the optimized local graph.");
     RTABMAP_PARAM(RGBD, ProximityAngle,               float, 45,   "Maximum angle (degrees) for one-to-one proximity detection.");
     RTABMAP_PARAM(RGBD, ProximityOdomGuess,           bool, false, "Use odometry as motion guess for one-to-one proximity detection.");
-    RTABMAP_PARAM(RGBD, ProximityGlobalScanMap,       bool, false, uFormat("Create a global assembled map from laser scans for one-to-many proximity detection, replacing the original one-to-many proximity detection (i.e., detection against local paths). Only used in localization mode (%s=false), otherwise original one-to-many proximity detection is done. Note also that if graph is modified (i.e., memory management is enabled or robot jumps from one disjoint session to another in same database), the global scan map is cleared and one-to-many proximity detection is reverted to original approach.", kMemIncrementalMemory().c_str(), kRGBDProximityPathRawPosesUsed().c_str()));
+    RTABMAP_PARAM(RGBD, ProximityGlobalScanMap,       bool, false, uFormat("Create a global assembled map from laser scans for one-to-many proximity detection, replacing the original one-to-many proximity detection (i.e., detection against local paths). Only used in localization mode (%s=false), otherwise original one-to-many proximity detection is done. Note also that if graph is modified (i.e., memory management is enabled or robot jumps from one disjoint session to another in same database), the global scan map is cleared and one-to-many proximity detection is reverted to original approach.", kMemIncrementalMemory().c_str()));
+    RTABMAP_PARAM(RGBD, ProximityMergedScanCovFactor, double, 100.0, uFormat("Covariance factor for one-to-many proximity detection (when %s>0 and scans are used).", kRGBDProximityPathMaxNeighbors().c_str()));
 
     // Graph optimization
 #ifdef RTABMAP_GTSAM
@@ -669,7 +670,7 @@ class RTABMAP_EXP Parameters
     RTABMAP_PARAM(Icp, PointToPlane,                bool, false,  "Use point to plane ICP.");
 #endif
     RTABMAP_PARAM(Icp, PointToPlaneK,               int, 5,       "Number of neighbors to compute normals for point to plane if the cloud doesn't have already normals.");
-    RTABMAP_PARAM(Icp, PointToPlaneRadius,          float, 1.0,   "Search radius to compute normals for point to plane if the cloud doesn't have already normals.");
+    RTABMAP_PARAM(Icp, PointToPlaneRadius,          float, 0.0,   "Search radius to compute normals for point to plane if the cloud doesn't have already normals.");
     RTABMAP_PARAM(Icp, PointToPlaneGroundNormalsUp, float, 0.0,   "Invert normals on ground if they are pointing down (useful for ring-like 3D LiDARs). 0 means disabled, 1 means only normals perfectly aligned with -z axis. This is only done with 3D scans.");
     RTABMAP_PARAM(Icp, PointToPlaneMinComplexity,   float, 0.02,  uFormat("Minimum structural complexity (0.0=low, 1.0=high) of the scan to do PointToPlane registration, otherwise PointToPoint registration is done instead and strategy from %s is used. This check is done only when %s=true.", kIcpPointToPlaneLowComplexityStrategy().c_str(), kIcpPointToPlane().c_str()));
     RTABMAP_PARAM(Icp, PointToPlaneLowComplexityStrategy, int, 1, uFormat("If structural complexity is below %s: set to 0 to so that the transform is automatically rejected, set to 1 to limit ICP correction in axes with most constraints (e.g., for a corridor-like environment, the resulting transform will be limited in y and yaw, x will taken from the guess), set to 2 to accept \"as is\" the transform computed by PointToPoint.", kIcpPointToPlaneMinComplexity().c_str()));

corelib/include/rtabmap/core/Rtabmap.h

@@ -196,8 +196,8 @@ public:
 			bool withGrid = false,
 			bool withWords = true,
 			bool withGlobalDescriptors = true) const;
-	std::map<int, Transform> getNodesInRadius(const Transform & pose, float radius); // If radius=0, RGBD/LocalRadius is used. Can return landmarks.
+	std::map<int, Transform> getNodesInRadius(const Transform & pose, float radius, int k=0, std::map<int, float> * distsSqr=0); // If radius=0 and k=0, RGBD/LocalRadius is used. Can return landmarks.
-	std::map<int, Transform> getNodesInRadius(int nodeId, float radius); // If nodeId==0, return poses around latest node. If radius=0, RGBD/LocalRadius is used. Can return landmarks and use landmark id (negative) as request.
+	std::map<int, Transform> getNodesInRadius(int nodeId, float radius, int k=0, std::map<int, float> * distsSqr=0); // If nodeId==0, return poses around latest node. If radius=0 and k=0, RGBD/LocalRadius is used. Can return landmarks and use landmark id (negative) as request.
 	int detectMoreLoopClosures(
 			float clusterRadiusMax = 0.5f,
 			float clusterAngle = M_PI/6.0f,
@@ -307,6 +307,7 @@ private:
 	bool _proximityRawPosesUsed;
 	float _proximityAngle;
 	bool _proximityOdomGuess;
+	double _proximityMergedScanCovFactor;
 	std::string _databasePath;
 	bool _optimizeFromGraphEnd;
 	float _optimizationMaxError;

corelib/include/rtabmap/core/RtabmapEvent.h

@@ -73,7 +73,8 @@ public:
 			kCmdResume,
 			kCmdGoal,             // params: [string] label or [int] location ID
 			kCmdCancelGoal,
-			kCmdLabel             // params: [string] label, [int] location ID
+			kCmdLabel,            // params: [string] label, [int] location ID
+			kCmdRemoveLabel       // params: [string] label
 	};
 public:
 	RtabmapEventCmd(Cmd cmd, const ParametersMap & parameters = ParametersMap()) :

corelib/include/rtabmap/core/RtabmapThread.h

@@ -68,7 +68,8 @@ public:
 		kStateTriggeringMap,
 		kStateSettingGoal,
 		kStateCancellingGoal,
-		kStateLabelling
+		kStateLabelling,
+		kStateRemovingLabel
 	};
 
 public:

corelib/include/rtabmap/core/Transform.h

@@ -104,6 +104,8 @@ public:
 	Transform translation() const;
 	Transform to3DoF() const;
 	Transform to4DoF() const;
+	bool is3DoF() const;
+	bool is4DoF() const;
 
 	cv::Mat rotationMatrix() const;
 	cv::Mat translationMatrix() const;

corelib/include/rtabmap/core/impl/OccupancyGrid.hpp

@@ -126,6 +126,7 @@ typename pcl::PointCloud<PointT>::Ptr OccupancyGrid::segmentCloud(
 			UDEBUG("Cluster radius=%f", clusterRadius_);
 			UDEBUG("flatObstaclesDetected=%d", flatObstaclesDetected_?1:0);
 			UDEBUG("maxGroundHeight=%f", maxGroundHeight_);
+			UDEBUG("groundNormalsUp=%f", groundNormalsUp_);
 			util3d::segmentObstaclesFromGround<PointT>(
 					cloud,
 					indices,
@@ -138,7 +139,8 @@ typename pcl::PointCloud<PointT>::Ptr OccupancyGrid::segmentCloud(
 					flatObstaclesDetected_,
 					maxGroundHeight_,
 					flatObstacles,
-					Eigen::Vector4f(viewPoint.x, viewPoint.y, viewPoint.z+(projMapFrame_?pose.z():0), 1));
+					Eigen::Vector4f(viewPoint.x, viewPoint.y, viewPoint.z+(projMapFrame_?pose.z():0), 1),
+					groundNormalsUp_);
 			UDEBUG("viewPoint=%f,%f,%f", viewPoint.x, viewPoint.y, viewPoint.z+(projMapFrame_?pose.z():0));
 			//UWARN("Saving ground.pcd and obstacles.pcd");
 			//pcl::io::savePCDFile("ground.pcd", *cloud, *groundIndices);
@@ -166,7 +168,11 @@ typename pcl::PointCloud<PointT>::Ptr OccupancyGrid::segmentCloud(
 		// Do radius filtering after voxel filtering ( a lot faster)
 		if(noiseFilteringRadius_ > 0.0 && noiseFilteringMinNeighbors_ > 0)
 		{
-			UDEBUG("");
+			UDEBUG("Radius filtering (%ld ground %ld obstacles, radius=%f k=%d)",
+					groundIndices->size(),
+					obstaclesIndices->size()+(flatObstacles?(*flatObstacles)->size():0),
+					noiseFilteringRadius_,
+					noiseFilteringMinNeighbors_);
 			if(groundIndices->size())
 			{
 				groundIndices = rtabmap::util3d::radiusFiltering(cloud, groundIndices, noiseFilteringRadius_, noiseFilteringMinNeighbors_);
@@ -179,6 +185,9 @@ typename pcl::PointCloud<PointT>::Ptr OccupancyGrid::segmentCloud(
 			{
 				*flatObstacles = rtabmap::util3d::radiusFiltering(cloud, *flatObstacles, noiseFilteringRadius_, noiseFilteringMinNeighbors_);
 			}
+			UDEBUG("Radius filtering end (%ld ground %ld obstacles)",
+					groundIndices->size(),
+					obstaclesIndices->size()+(flatObstacles?(*flatObstacles)->size():0));
 
 			if(groundIndices->empty() && obstaclesIndices->empty())
 			{

corelib/include/rtabmap/core/impl/util3d_mapping.hpp

@@ -63,7 +63,8 @@ void segmentObstaclesFromGround(
 		bool segmentFlatObstacles,
 		float maxGroundHeight,
 		pcl::IndicesPtr * flatObstacles,
-		const Eigen::Vector4f & viewPoint)
+		const Eigen::Vector4f & viewPoint,
+		float groundNormalsUp)
 {
 	ground.reset(new std::vector<int>);
 	obstacles.reset(new std::vector<int>);
@@ -81,7 +82,8 @@ void segmentObstaclesFromGround(
 				groundNormalAngle,
 				Eigen::Vector4f(0,0,1,0),
 				normalKSearch,
-				viewPoint);
+				viewPoint,
+				groundNormalsUp);
 
 		if(segmentFlatObstacles && flatSurfaces->size())
 		{
@@ -205,7 +207,8 @@ void segmentObstaclesFromGround(
 		bool segmentFlatObstacles,
 		float maxGroundHeight,
 		pcl::IndicesPtr * flatObstacles,
-		const Eigen::Vector4f & viewPoint)
+		const Eigen::Vector4f & viewPoint,
+		float groundNormalsUp)
 {
 	pcl::IndicesPtr indices(new std::vector<int>);
 	segmentObstaclesFromGround<PointT>(
@@ -220,7 +223,8 @@ void segmentObstaclesFromGround(
 			segmentFlatObstacles,
 			maxGroundHeight,
 			flatObstacles,
-			viewPoint);
+			viewPoint,
+			groundNormalsUp);
 }
 
 template<typename PointT>

corelib/include/rtabmap/core/optimizer/OptimizerG2O.h

@@ -42,16 +42,7 @@ public:
 	static bool isCholmodAvailable();
 
 public:
-	OptimizerG2O(const ParametersMap & parameters = ParametersMap()) :
+	OptimizerG2O(const ParametersMap & parameters = ParametersMap());
-		Optimizer(parameters),
-		solver_(Parameters::defaultg2oSolver()),
-		optimizer_(Parameters::defaultg2oOptimizer()),
-		pixelVariance_(Parameters::defaultg2oPixelVariance()),
-		robustKernelDelta_(Parameters::defaultg2oRobustKernelDelta()),
-		baseline_(Parameters::defaultg2oBaseline())
-	{
-		parseParameters(parameters);
-	}
 	virtual ~OptimizerG2O() {}
 
 	virtual Type type() const {return kTypeG2O;}

corelib/include/rtabmap/core/util3d.h

@@ -313,6 +313,7 @@ std::vector<std::pair< std::pair<int, int>, pcl::PointXY> > RTABMAP_EXP projectC
 		float maxDistance = 0.0f,
 		float maxAngle = 0.0f,
 		const std::vector<float> & roiRatios = std::vector<float>(),
+		const cv::Mat & projMask = cv::Mat(),
 		bool distanceToCamPolicy = false,
 		const ProgressState * state = 0);
 /**
@@ -326,6 +327,7 @@ std::vector<std::pair< std::pair<int, int>, pcl::PointXY> > RTABMAP_EXP projectC
 		float maxDistance = 0.0f,
 		float maxAngle = 0.0f,
 		const std::vector<float> & roiRatios = std::vector<float>(),
+		const cv::Mat & projMask = cv::Mat(),
 		bool distanceToCamPolicy = false,
 		const ProgressState * state = 0);
 

corelib/include/rtabmap/core/util3d_filtering.h

@@ -160,9 +160,21 @@ inline pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr uniformSampling(
 pcl::PointCloud<pcl::PointXYZ>::Ptr RTABMAP_EXP randomSampling(
 		const pcl::PointCloud<pcl::PointXYZ>::Ptr & cloud,
 		int samples);
+pcl::PointCloud<pcl::PointNormal>::Ptr RTABMAP_EXP randomSampling(
+		const pcl::PointCloud<pcl::PointNormal>::Ptr & cloud,
+		int samples);
 pcl::PointCloud<pcl::PointXYZRGB>::Ptr RTABMAP_EXP randomSampling(
 		const pcl::PointCloud<pcl::PointXYZRGB>::Ptr & cloud,
 		int samples);
+pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr RTABMAP_EXP randomSampling(
+		const pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr & cloud,
+		int samples);
+pcl::PointCloud<pcl::PointXYZI>::Ptr RTABMAP_EXP randomSampling(
+		const pcl::PointCloud<pcl::PointXYZI>::Ptr & cloud,
+		int samples);
+pcl::PointCloud<pcl::PointXYZINormal>::Ptr RTABMAP_EXP randomSampling(
+		const pcl::PointCloud<pcl::PointXYZINormal>::Ptr & cloud,
+		int samples);
 
 
 pcl::IndicesPtr RTABMAP_EXP passThrough(
@@ -449,6 +461,99 @@ pcl::IndicesPtr RTABMAP_EXP radiusFiltering(
 		float radiusSearch,
 		int minNeighborsInRadius);
 
+/* for convenience */
+pcl::IndicesPtr RTABMAP_EXP proportionalRadiusFiltering(
+		const pcl::PointCloud<pcl::PointXYZ>::Ptr & cloud,
+		const std::vector<int> & viewpointIndices,
+		const std::map<int, Transform> & viewpoints,
+		float factor=0.01f,
+		float neighborScale=2.0f);
+pcl::IndicesPtr RTABMAP_EXP proportionalRadiusFiltering(
+		const pcl::PointCloud<pcl::PointNormal>::Ptr & cloud,
+		const std::vector<int> & viewpointIndices,
+		const std::map<int, Transform> & viewpoints,
+		float factor=0.01f,
+		float neighborScale=2.0f);
+pcl::IndicesPtr RTABMAP_EXP proportionalRadiusFiltering(
+		const pcl::PointCloud<pcl::PointXYZRGB>::Ptr & cloud,
+		const std::vector<int> & viewpointIndices,
+		const std::map<int, Transform> & viewpoints,
+		float factor=0.01f,
+		float neighborScale=2.0f);
+pcl::IndicesPtr RTABMAP_EXP proportionalRadiusFiltering(
+		const pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr & cloud,
+		const std::vector<int> & viewpointIndices,
+		const std::map<int, Transform> & viewpoints,
+		float factor=0.01f,
+		float neighborScale=2.0f);
+pcl::IndicesPtr RTABMAP_EXP proportionalRadiusFiltering(
+		const pcl::PointCloud<pcl::PointXYZI>::Ptr & cloud,
+		const std::vector<int> & viewpointIndices,
+		const std::map<int, Transform> & viewpoints,
+		float factor=0.01f,
+		float neighborScale=2.0f);
+pcl::IndicesPtr RTABMAP_EXP proportionalRadiusFiltering(
+		const pcl::PointCloud<pcl::PointXYZINormal>::Ptr & cloud,
+		const std::vector<int> & viewpointIndices,
+		const std::map<int, Transform> & viewpoints,
+		float factor=0.01f,
+		float neighborScale=2.0f);
+
+/**
+ * @brief Filter points based on distance from their viewpoint.
+ *
+ * @param cloud the input cloud.
+ * @param indices the input indices of the cloud to check, if empty, all points in the cloud are checked.
+ * @param viewpointIndices should be same size than the input cloud, it tells the viewpoint index in viewpoints for each point.
+ * @param viewpoints the viewpoints.
+ * @param factor will determine the search radius based on the distance from a point and its viewpoint. Setting it higher will filter points farther from accurate points (but processing time will be also higher).
+ * @param neighborScale will scale the search radius of neighbors found around a point. Setting it higher will accept more noisy points close to accurate points (but processing time will be also higher).
+ * @return the indices of the points satisfying the parameters.
+ */
+
+pcl::IndicesPtr RTABMAP_EXP proportionalRadiusFiltering(
+		const pcl::PointCloud<pcl::PointXYZ>::Ptr & cloud,
+		const pcl::IndicesPtr & indices,
+		const std::vector<int> & viewpointIndices,
+		const std::map<int, Transform> & viewpoints,
+		float factor=0.01f,
+		float neighborScale=2.0f);
+pcl::IndicesPtr RTABMAP_EXP proportionalRadiusFiltering(
+		const pcl::PointCloud<pcl::PointNormal>::Ptr & cloud,
+		const pcl::IndicesPtr & indices,
+		const std::vector<int> & viewpointIndices,
+		const std::map<int, Transform> & viewpoints,
+		float factor=0.01f,
+		float neighborScale=2.0f);
+pcl::IndicesPtr RTABMAP_EXP proportionalRadiusFiltering(
+		const pcl::PointCloud<pcl::PointXYZRGB>::Ptr & cloud,
+		const pcl::IndicesPtr & indices,
+		const std::vector<int> & viewpointIndices,
+		const std::map<int, Transform> & viewpoints,
+		float factor=0.01f,
+		float neighborScale=2.0f);
+pcl::IndicesPtr RTABMAP_EXP proportionalRadiusFiltering(
+		const pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr & cloud,
+		const pcl::IndicesPtr & indices,
+		const std::vector<int> & viewpointIndices,
+		const std::map<int, Transform> & viewpoints,
+		float factor=0.01f,
+		float neighborScale=2.0f);
+pcl::IndicesPtr RTABMAP_EXP proportionalRadiusFiltering(
+		const pcl::PointCloud<pcl::PointXYZI>::Ptr & cloud,
+		const pcl::IndicesPtr & indices,
+		const std::vector<int> & viewpointIndices,
+		const std::map<int, Transform> & viewpoints,
+		float factor=0.01f,
+		float neighborScale=2.0f);
+pcl::IndicesPtr RTABMAP_EXP proportionalRadiusFiltering(
+		const pcl::PointCloud<pcl::PointXYZINormal>::Ptr & cloud,
+		const pcl::IndicesPtr & indices,
+		const std::vector<int> & viewpointIndices,
+		const std::map<int, Transform> & viewpoints,
+		float factor=0.01f,
+		float neighborScale=2.0f);
+
 /**
  * For convenience.
  */
@@ -592,13 +697,15 @@ pcl::IndicesPtr RTABMAP_EXP normalFiltering(
 		float angleMax,
 		const Eigen::Vector4f & normal,
 		int normalKSearch,
-		const Eigen::Vector4f & viewpoint);
+		const Eigen::Vector4f & viewpoint,
+		float groundNormalsUp = 0.0f);
 pcl::IndicesPtr RTABMAP_EXP normalFiltering(
 		const pcl::PointCloud<pcl::PointXYZRGB>::Ptr & cloud,
 		float angleMax,
 		const Eigen::Vector4f & normal,
 		int normalKSearch,
-		const Eigen::Vector4f & viewpoint);
+		const Eigen::Vector4f & viewpoint,
+		float groundNormalsUp = 0.0f);
 
 /**
  * @brief Given a normal and a maximum angle error, keep all points of the cloud
@@ -622,42 +729,48 @@ pcl::IndicesPtr RTABMAP_EXP normalFiltering(
 		float angleMax,
 		const Eigen::Vector4f & normal,
 		int normalKSearch,
-		const Eigen::Vector4f & viewpoint);
+		const Eigen::Vector4f & viewpoint,
+		float groundNormalsUp = 0.0f);
 pcl::IndicesPtr RTABMAP_EXP normalFiltering(
 		const pcl::PointCloud<pcl::PointXYZRGB>::Ptr & cloud,
 		const pcl::IndicesPtr & indices,
 		float angleMax,
 		const Eigen::Vector4f & normal,
 		int normalKSearch,
-		const Eigen::Vector4f & viewpoint);
+		const Eigen::Vector4f & viewpoint,
+		float groundNormalsUp = 0.0f);
 pcl::IndicesPtr RTABMAP_EXP normalFiltering(
 		const pcl::PointCloud<pcl::PointXYZI>::Ptr & cloud,
 		const pcl::IndicesPtr & indices,
 		float angleMax,
 		const Eigen::Vector4f & normal,
 		int normalKSearch,
-		const Eigen::Vector4f & viewpoint);
+		const Eigen::Vector4f & viewpoint,
+		float groundNormalsUp = 0.0f);
 pcl::IndicesPtr RTABMAP_EXP normalFiltering(
 		const pcl::PointCloud<pcl::PointNormal>::Ptr & cloud,
 		const pcl::IndicesPtr & indices,
 		float angleMax,
 		const Eigen::Vector4f & normal,
 		int normalKSearch,
-		const Eigen::Vector4f & viewpoint);
+		const Eigen::Vector4f & viewpoint,
+		float groundNormalsUp = 0.0f);
 pcl::IndicesPtr RTABMAP_EXP normalFiltering(
 		const pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr & cloud,
 		const pcl::IndicesPtr & indices,
 		float angleMax,
 		const Eigen::Vector4f & normal,
 		int normalKSearch,
-		const Eigen::Vector4f & viewpoint);
+		const Eigen::Vector4f & viewpoint,
+		float groundNormalsUp = 0.0f);
 pcl::IndicesPtr RTABMAP_EXP normalFiltering(
 		const pcl::PointCloud<pcl::PointXYZINormal>::Ptr & cloud,
 		const pcl::IndicesPtr & indices,
 		float angleMax,
 		const Eigen::Vector4f & normal,
 		int normalKSearch,
-		const Eigen::Vector4f & viewpoint);
+		const Eigen::Vector4f & viewpoint,
+		float groundNormalsUp = 0.0f);
 
 /**
  * For convenience.

corelib/include/rtabmap/core/util3d_mapping.h

@@ -152,7 +152,8 @@ void segmentObstaclesFromGround(
 		bool segmentFlatObstacles = false,
 		float maxGroundHeight = 0.0f,
 		pcl::IndicesPtr * flatObstacles = 0,
-		const Eigen::Vector4f & viewPoint = Eigen::Vector4f(0,0,100,0));
+		const Eigen::Vector4f & viewPoint = Eigen::Vector4f(0,0,100,0),
+		float groundNormalsUp = 0);
 template<typename PointT>
 void segmentObstaclesFromGround(
 		const typename pcl::PointCloud<PointT>::Ptr & cloud,
@@ -165,7 +166,8 @@ void segmentObstaclesFromGround(
 		bool segmentFlatObstacles = false,
 		float maxGroundHeight = 0.0f,
 		pcl::IndicesPtr * flatObstacles = 0,
-		const Eigen::Vector4f & viewPoint = Eigen::Vector4f(0,0,100,0));
+		const Eigen::Vector4f & viewPoint = Eigen::Vector4f(0,0,100,0),
+		float groundNormalsUp = 0);
 
 template<typename PointT>
 void occupancy2DFromGroundObstacles(

corelib/include/rtabmap/core/util3d_surface.h

@@ -481,18 +481,27 @@ void RTABMAP_EXP adjustNormalsToViewPoints(
 		const std::map<int, Transform> & poses,
 		const pcl::PointCloud<pcl::PointXYZ>::Ptr & rawCloud,
 		const std::vector<int> & rawCameraIndices,
-		pcl::PointCloud<pcl::PointNormal>::Ptr & cloud);
+		pcl::PointCloud<pcl::PointNormal>::Ptr & cloud,
+		float groundNormalsUp = 0.0f);
 void RTABMAP_EXP adjustNormalsToViewPoints(
 		const std::map<int, Transform> & poses,
 		const pcl::PointCloud<pcl::PointXYZ>::Ptr & rawCloud,
 		const std::vector<int> & rawCameraIndices,
-		pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr & cloud);
+		pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr & cloud,
+		float groundNormalsUp = 0.0f);
+void RTABMAP_EXP adjustNormalsToViewPoints(
+		const std::map<int, Transform> & poses,
+		const pcl::PointCloud<pcl::PointXYZ>::Ptr & rawCloud,
+		const std::vector<int> & rawCameraIndices,
+		pcl::PointCloud<pcl::PointXYZINormal>::Ptr & cloud,
+		float groundNormalsUp = 0.0f);
 
 void RTABMAP_EXP adjustNormalsToViewPoints(
 		const std::map<int, Transform> & viewpoints,
 		const LaserScan & rawScan,
 		const std::vector<int> & viewpointIds,
-		LaserScan & scan);
+		LaserScan & scan,
+		float groundNormalsUp = 0.0f);
 
 pcl::PolygonMesh::Ptr RTABMAP_EXP meshDecimation(const pcl::PolygonMesh::Ptr & mesh, float factor);
 

corelib/src/CMakeLists.txt

@@ -373,18 +373,22 @@ ENDIF(WITH_TORO)
 
 IF(G2O_FOUND)
 	IF(g2o_FOUND)
-	  SET(LIBRARIES
+	  SET(LIBRARIES ${LIBRARIES}
-             ${LIBRARIES}
 	     g2o::core
-	     g2o::solver_cholmod
 	     g2o::solver_eigen
 	     g2o::solver_pcg
-	     g2o::solver_csparse
-	     g2o::csparse_extension
 	     g2o::types_slam2d
 	     g2o::types_slam3d
-	     g2o::types_sba
+	     g2o::types_sba)
-	  )
+	  IF(TARGET g2o::solver_csparse)
+	    SET(LIBRARIES ${LIBRARIES}
+          g2o::solver_csparse
+          g2o::csparse_extension)
+      ENDIF(TARGET g2o::solver_csparse)
+      IF(TARGET g2o::solver_cholmod)
+        SET(LIBRARIES ${LIBRARIES} 
+          g2o::solver_cholmod)
+      ENDIF(TARGET g2o::solver_cholmod)
 	ELSE()
 	  SET(INCLUDE_DIRS 
 		${INCLUDE_DIRS} 
@@ -395,13 +399,11 @@ IF(G2O_FOUND)
 		${G2O_LIBRARIES}
 	  )
 	ENDIF()
-	SET(SRC_FILES
+	SET(SRC_FILES ${SRC_FILES}
-    		${SRC_FILES}
 		optimizer/g2o/edge_se3_xyzprior.cpp
 	)
 	IF(WITH_VERTIGO)
-		SET(SRC_FILES
+		SET(SRC_FILES ${SRC_FILES}
-    			${SRC_FILES}
 			optimizer/vertigo/g2o/edge_se2Switchable.cpp
 			optimizer/vertigo/g2o/edge_se3Switchable.cpp
 			optimizer/vertigo/g2o/edge_switchPrior.cpp

corelib/src/Graph.cpp

@@ -898,7 +898,8 @@ void computeMaxGraphErrors(
 		float & maxLinearError,
 		float & maxAngularError,
 		const Link ** maxLinearErrorLink,
-		const Link ** maxAngularErrorLink)
+		const Link ** maxAngularErrorLink,
+		bool for3DoF)
 {
 	maxLinearErrorRatio = -1;
 	maxAngularErrorRatio = -1;
@@ -918,7 +919,7 @@ void computeMaxGraphErrors(
 			float linearError = uMax3(
 					fabs(iter->second.transform().x() - t.x()),
 					fabs(iter->second.transform().y() - t.y()),
-					fabs(iter->second.transform().z() - t.z()));
+					for3DoF?0:fabs(iter->second.transform().z() - t.z()));
 			UASSERT(iter->second.transVariance(false)>0.0);
 			float stddevLinear = sqrt(iter->second.transVariance(false));
 			float linearErrorRatio = linearError/stddevLinear;
@@ -937,8 +938,8 @@ void computeMaxGraphErrors(
 			t.getEulerAngles(opt_roll, opt_pitch, opt_yaw);
 			iter->second.transform().getEulerAngles(link_roll, link_pitch, link_yaw);
 			float angularError = uMax3(
-					fabs(opt_roll - link_roll),
+					for3DoF?0:fabs(opt_roll - link_roll),
-					fabs(opt_pitch - link_pitch),
+					for3DoF?0:fabs(opt_pitch - link_pitch),
 					fabs(opt_yaw - link_yaw));
 			angularError = angularError>M_PI?2*M_PI-angularError:angularError;
 			UASSERT(iter->second.rotVariance(false)>0.0);
@@ -1336,7 +1337,7 @@ std::map<int, Transform> radiusPosesFiltering(
 
 		//pcl::IndicesPtr indicesOut(new std::vector<int>);
 		//indicesOut->insert(indicesOut->end(), indicesKept.begin(), indicesKept.end());
-		UINFO("Cloud filtered In = %d, Out = %d", cloud->size(), indicesKept.size());
+		UINFO("Cloud filtered In = %d, Out = %d (radius=%f angle=%f keepLatest=%d)", cloud->size(), indicesKept.size(), radius, angle, keepLatest?1:0);
 		//pcl::io::savePCDFile("duplicateIn.pcd", *cloud);
 		//pcl::io::savePCDFile("duplicateOut.pcd", *cloud, *indicesOut);
 
@@ -2079,12 +2080,12 @@ std::list<std::pair<int, Transform> > computePath(
 }
 
 int findNearestNode(
-		const std::map<int, rtabmap::Transform> & nodes,
+		const std::map<int, rtabmap::Transform> & poses,
 		const rtabmap::Transform & targetPose,
 		float * distance)
 {
 	int id = 0;
-	std::map<int, float> nearestNodes = findNearestNodes(nodes, targetPose, 1);
+	std::map<int, float> nearestNodes = findNearestNodes(targetPose, poses, 0, 0, 1);
 	if(!nearestNodes.empty())
 	{
 		id = nearestNodes.begin()->first;
@@ -2096,70 +2097,44 @@ int findNearestNode(
 	return id;
 }
 
+// return <id, sqrd distance>, excluding query
 std::map<int, float> findNearestNodes(
-		const std::map<int, rtabmap::Transform> & nodes,
+		int nodeId,
-		const rtabmap::Transform & targetPose,
+		const std::map<int, Transform> & poses,
+		float radius,
+		float angle,
 		int k)
 {
-	std::map<int, float> nearestIds;
+	UASSERT(uContains(poses, nodeId));
-	if(nodes.size() && !targetPose.isNull())
-	{
-		pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>);
-		cloud->resize(nodes.size());
-		std::vector<int> ids(nodes.size());
-		int oi = 0;
-		for(std::map<int, Transform>::const_iterator iter = nodes.begin(); iter!=nodes.end(); ++iter)
-		{
-			(*cloud)[oi] = pcl::PointXYZ(iter->second.x(), iter->second.y(), iter->second.z());
-			ids[oi++] = iter->first;
-		}
 
-		pcl::search::KdTree<pcl::PointXYZ>::Ptr kdTree(new pcl::search::KdTree<pcl::PointXYZ>);
+	std::map<int, Transform> nodesMinusTarget = poses;
-		kdTree->setInputCloud(cloud);
+	Transform targetPose = poses.at(nodeId);
-		std::vector<int> ind;
-		std::vector<float> dist;
-		pcl::PointXYZ pt(targetPose.x(), targetPose.y(), targetPose.z());
-		kdTree->nearestKSearch(pt, k, ind, dist);
-
-		for(unsigned int i=0; i<ind.size(); ++i)
-		{
-			nearestIds.insert(std::make_pair(ids[ind[i]], dist[i]));
-		}
-	}
-	return nearestIds;
-}
-
-// return <id, sqrd distance>, excluding query
-std::map<int, float> getNodesInRadius(
-		int nodeId,
-		const std::map<int, Transform> & nodes,
-		float radius)
-{
-	UASSERT(uContains(nodes, nodeId));
-
-	std::map<int, Transform> nodesMinusTarget = nodes;
-	Transform targetPose = nodes.at(nodeId);
 	nodesMinusTarget.erase(nodeId);
-	return getNodesInRadius(targetPose, nodesMinusTarget, radius);
+	return findNearestNodes(targetPose, nodesMinusTarget, radius, angle, k);
 }
 
-// return <id, sqrd distance>, excluding query
+// return <id, sqrd distance>
-std::map<int, float> getNodesInRadius(
+std::map<int, float> findNearestNodes(
 		const Transform & targetPose,
-		const std::map<int, Transform> & nodes,
+		const std::map<int, Transform> & poses,
-		float radius)
+		float radius,
+		float angle,
+		int k)
 {
+	UASSERT(radius>=0.0f);
+	UASSERT(k>=0);
+	UASSERT(radius > 0.0f || k>0);
 	std::map<int, float> foundNodes;
-	if(nodes.empty())
+	if(poses.empty())
 	{
 		return foundNodes;
 	}
 
 	pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>);
-	cloud->resize(nodes.size());
+	cloud->resize(poses.size());
-	std::vector<int> ids(nodes.size());
+	std::vector<int> ids(poses.size());
 	int oi = 0;
-	for(std::map<int, Transform>::const_iterator iter = nodes.begin(); iter!=nodes.end(); ++iter)
+	for(std::map<int, Transform>::const_iterator iter = poses.begin(); iter!=poses.end(); ++iter)
 	{
 		(*cloud)[oi] = pcl::PointXYZ(iter->second.x(), iter->second.y(), iter->second.z());
 		UASSERT_MSG(pcl::isFinite((*cloud)[oi]), uFormat("Invalid pose (%d) %s", iter->first, iter->second.prettyPrint().c_str()).c_str());
@@ -2176,89 +2151,33 @@ std::map<int, float> getNodesInRadius(
 		std::vector<int> ind;
 		std::vector<float> sqrdDist;
 		pcl::PointXYZ pt(targetPose.x(), targetPose.y(), targetPose.z());
-		kdTree->radiusSearch(pt, radius, ind, sqrdDist, 0);
+		if(radius>0.0f)
-		for(unsigned int i=0; i<ind.size(); ++i)
 		{
-			if(ind[i] >=0)
+			kdTree->radiusSearch(pt, radius, ind, sqrdDist, k);
-			{
+		}
-				foundNodes.insert(std::make_pair(ids[ind[i]], sqrdDist[i]));
+		else
-			}
+		{
+			kdTree->nearestKSearch(pt, k, ind, sqrdDist);
 		}
-	}
-	UDEBUG("found nodes=%d", (int)foundNodes.size());
-	return foundNodes;
-}
-
-// return <id, Transform>, excluding query
-std::map<int, Transform> getPosesInRadius(
-		int nodeId,
-		const std::map<int, Transform> & nodes,
-		float radius,
-		float angle)
-{
-	UASSERT(uContains(nodes, nodeId));
-
-	std::map<int, Transform> nodesMinusTarget = nodes;
-	Transform targetPose = nodes.at(nodeId);
-	nodesMinusTarget.erase(nodeId);
-	return getPosesInRadius(targetPose, nodesMinusTarget, radius, angle);
-}
-// return <id, Transform>, excluding query
-std::map<int, Transform> getPosesInRadius(
-		const Transform & targetPose,
-		const std::map<int, Transform> & nodes,
-		float radius,
-		float angle)
-{
-	std::map<int, Transform> foundNodes;
-	if(nodes.empty())
-	{
-		return foundNodes;
-	}
-
-	pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>);
-	cloud->resize(nodes.size());
-	std::vector<int> ids(nodes.size());
-	int oi = 0;
-	for(std::map<int, Transform>::const_iterator iter = nodes.begin(); iter!=nodes.end(); ++iter)
-	{
-		(*cloud)[oi] = pcl::PointXYZ(iter->second.x(), iter->second.y(), iter->second.z());
-		UASSERT_MSG(pcl::isFinite((*cloud)[oi]), uFormat("Invalid pose (%d) %s", iter->first, iter->second.prettyPrint().c_str()).c_str());
-		ids[oi] = iter->first;
-		++oi;
-	}
-	cloud->resize(oi);
-	ids.resize(oi);
-
-	if(cloud->size())
-	{
-		pcl::search::KdTree<pcl::PointXYZ>::Ptr kdTree(new pcl::search::KdTree<pcl::PointXYZ>);
-		kdTree->setInputCloud(cloud);
-		std::vector<int> ind;
-		std::vector<float> sqrdDist;
-		pcl::PointXYZ pt(targetPose.x(), targetPose.y(), targetPose.z());
-		kdTree->radiusSearch(pt, radius, ind, sqrdDist, 0);
-
 		Eigen::Vector3f vA = targetPose.toEigen3f().linear()*Eigen::Vector3f(1,0,0);
-
 		for(unsigned int i=0; i<ind.size(); ++i)
 		{
 			if(ind[i] >=0)
 			{
 				if(angle > 0.0f)
 				{
-					const Transform & checkT = nodes.at(ids[ind[i]]);
+					const Transform & checkT = poses.at(ids[ind[i]]);
 					// same orientation?
 					Eigen::Vector3f vB = checkT.toEigen3f().linear()*Eigen::Vector3f(1,0,0);
 					double a = pcl::getAngle3D(Eigen::Vector4f(vA[0], vA[1], vA[2], 0), Eigen::Vector4f(vB[0], vB[1], vB[2], 0));
 					if(a <= angle)
 					{
-						foundNodes.insert(std::make_pair(ids[ind[i]], nodes.at(ids[ind[i]])));
+						foundNodes.insert(std::make_pair(ids[ind[i]], sqrdDist[i]));
 					}
 				}
 				else
 				{
-					foundNodes.insert(std::make_pair(ids[ind[i]], nodes.at(ids[ind[i]])));
+					foundNodes.insert(std::make_pair(ids[ind[i]], sqrdDist[i]));
 				}
 			}
 		}
@@ -2267,6 +2186,62 @@ std::map<int, Transform> getPosesInRadius(
 	return foundNodes;
 }
 
+// return <id, Transform>, excluding query
+std::map<int, Transform> findNearestPoses(
+		int nodeId,
+		const std::map<int, Transform> & poses,
+		float radius,
+		float angle,
+		int k)
+{
+	UASSERT(uContains(poses, nodeId));
+
+	std::map<int, Transform> nodesMinusTarget = poses;
+	Transform targetPose = poses.at(nodeId);
+	nodesMinusTarget.erase(nodeId);
+	return findNearestPoses(targetPose, nodesMinusTarget, radius, angle, k);
+}
+// return <id, Transform>
+std::map<int, Transform> findNearestPoses(
+		const Transform & targetPose,
+		const std::map<int, Transform> & poses,
+		float radius,
+		float angle,
+		int k)
+{
+	std::map<int, float> nearestNodes = findNearestNodes(targetPose, poses, radius, angle, k);
+	std::map<int, Transform> foundPoses;
+	for(std::map<int, float>::iterator iter=nearestNodes.begin(); iter!=nearestNodes.end(); ++iter)
+	{
+		foundPoses.insert(*poses.find(iter->first));
+	}
+	UDEBUG("found nodes=%d", (int)foundPoses.size());
+	return foundPoses;
+}
+
+// deprecated stuff
+std::map<int, float> findNearestNodes(const std::map<int, rtabmap::Transform> & nodes, const rtabmap::Transform & targetPose, int k)
+{
+	return findNearestNodes(targetPose, nodes, 0, 0, k);
+}
+std::map<int, float> getNodesInRadius(int nodeId, const std::map<int, Transform> & nodes, float radius)
+{
+	return findNearestNodes(nodeId, nodes, radius);
+}
+std::map<int, float> getNodesInRadius(const Transform & targetPose, const std::map<int, Transform> & nodes, float radius)
+{
+	return findNearestNodes(targetPose, nodes, radius);
+}
+std::map<int, Transform> getPosesInRadius(int nodeId, const std::map<int, Transform> & nodes, float radius, float angle)
+{
+	return findNearestPoses(nodeId, nodes, radius, angle);
+}
+std::map<int, Transform> getPosesInRadius(const Transform & targetPose, const std::map<int, Transform> & nodes, float radius, float angle)
+{
+	return findNearestPoses(targetPose, nodes, radius, angle);
+}
+
+
 float computePathLength(
 		const std::vector<std::pair<int, Transform> > & path,
 		unsigned int fromIndex,

corelib/src/LaserScan.cpp

@@ -368,8 +368,8 @@ LaserScan LaserScan::clone() const
 
 float & LaserScan::field(unsigned int pointIndex, unsigned int channelOffset)
 {
-	UASSERT(pointIndex < data_.cols);
+	UASSERT(pointIndex < (unsigned int)data_.cols);
-	UASSERT(channelOffset < data_.channels());
+	UASSERT(channelOffset < (unsigned int)data_.channels());
 	return data_.ptr<float>(0, pointIndex)[channelOffset];
 }
 
@@ -387,7 +387,7 @@ LaserScan LaserScan::operator+(const LaserScan & scan)
 	{
 		if(this->empty())
 		{
-			dest = LaserScan(scan.data().clone(), 0, 0, this->format());
+			dest = scan.clone();
 		}
 		else
 		{

corelib/src/Memory.cpp

@@ -98,6 +98,7 @@ Memory::Memory(const ParametersMap & parameters) :
 	_laserScanVoxelSize(Parameters::defaultMemLaserScanVoxelSize()),
 	_laserScanNormalK(Parameters::defaultMemLaserScanNormalK()),
 	_laserScanNormalRadius(Parameters::defaultMemLaserScanNormalRadius()),
+	_laserScanGroundNormalsUp(Parameters::defaultIcpPointToPlaneGroundNormalsUp()),
 	_reextractLoopClosureFeatures(Parameters::defaultRGBDLoopClosureReextractFeatures()),
 	_localBundleOnLoopClosure(Parameters::defaultRGBDLocalBundleOnLoopClosure()),
 	_rehearsalMaxDistance(Parameters::defaultRGBDLinearUpdate()),
@@ -565,6 +566,7 @@ void Memory::parseParameters(const ParametersMap & parameters)
 	Parameters::parse(params, Parameters::kMemLaserScanVoxelSize(), _laserScanVoxelSize);
 	Parameters::parse(params, Parameters::kMemLaserScanNormalK(), _laserScanNormalK);
 	Parameters::parse(params, Parameters::kMemLaserScanNormalRadius(), _laserScanNormalRadius);
+	Parameters::parse(params, Parameters::kIcpPointToPlaneGroundNormalsUp(), _laserScanGroundNormalsUp);
 	Parameters::parse(params, Parameters::kRGBDLoopClosureReextractFeatures(), _reextractLoopClosureFeatures);
 	Parameters::parse(params, Parameters::kRGBDLocalBundleOnLoopClosure(), _localBundleOnLoopClosure);
 	Parameters::parse(params, Parameters::kRGBDLinearUpdate(), _rehearsalMaxDistance);
@@ -2523,6 +2525,14 @@ int Memory::getSignatureIdByLabel(const std::string & label, bool lookInDatabase
 		if(id == 0 && _dbDriver && lookInDatabase)
 		{
 			_dbDriver->getNodeIdByLabel(label, id);
+			if(_signatures.find(id) != _signatures.end())
+			{
+				// The signature is already in WM, but label was not
+				// found above. It means the label has been cleared in
+				// current session (not yet saved to database), so return
+				// not found.
+				id = 0;
+			}
 		}
 	}
 	return id;
@@ -2532,15 +2542,35 @@ bool Memory::labelSignature(int id, const std::string & label)
 {
 	// verify that this label is not used
 	int idFound=getSignatureIdByLabel(label);
+	if(idFound == 0 && label.empty() && _labels.find(id)==_labels.end())
+	{
+		UWARN("Trying to remove label from node %d but it has already no label", id);
+		return false;
+	}
 	if(idFound == 0 || idFound == id)
 	{
 		Signature * s  = this->_getSignature(id);
 		if(s)
 		{
-			uInsert(_labels, std::make_pair(s->id(), label));
+			if(label.empty())
+			{
+				UWARN("Label \"%s\" removed from node %d", _labels.at(id).c_str(), id);
+				_labels.erase(id);
+			}
+			else
+			{
+				if(_labels.find(id)!=_labels.end())
+				{
+					UWARN("Label \"%s\" set to node %d (previously labeled \"%s\")", label.c_str(), id, _labels.at(id).c_str());
+				}
+				else
+				{
+					UWARN("Label \"%s\" set to node %d", label.c_str(), id);
+				}
+				uInsert(_labels, std::make_pair(s->id(), label));
+			}
 			s->setLabel(label);
 			_linksChanged = s->isSaved(); // HACK to get label updated in Localization mode
-			UWARN("Label \"%s\" set to node %d", label.c_str(), id);
 			return true;
 		}
 		else if(_dbDriver)
@@ -2551,9 +2581,25 @@ bool Memory::labelSignature(int id, const std::string & label)
 			_dbDriver->loadSignatures(ids,signatures);
 			if(signatures.size())
 			{
-				uInsert(_labels, std::make_pair(signatures.front()->id(), label));
+				if(label.empty())
+				{
+					UWARN("Label \"%s\" removed from node %d", _labels.at(id).c_str(), id);
+					_labels.erase(id);
+				}
+				else
+				{
+					if(_labels.find(id)!=_labels.end())
+					{
+						UWARN("Label \"%s\" set to node %d (previously labeled \"%s\")", label.c_str(), id, _labels.at(id).c_str());
+					}
+					else
+					{
+						UWARN("Label \"%s\" set to node %d", label.c_str(), id);
+					}
+					uInsert(_labels, std::make_pair(id, label));
+				}
+
 				signatures.front()->setLabel(label);
-				UWARN("Label \"%s\" set to node %d", label.c_str(), id);
 				_dbDriver->asyncSave(signatures.front()); // move it again to trash
 				return true;
 			}
@@ -2565,7 +2611,7 @@ bool Memory::labelSignature(int id, const std::string & label)
 	}
 	else if(idFound)
 	{
-		UWARN("Node %d has already label \"%s\"", idFound, label.c_str());
+		UWARN("Another node %d has already label \"%s\", cannot set it to node %d", idFound, label.c_str(), id);
 	}
 	return false;
 }
@@ -5436,7 +5482,8 @@ Signature * Memory::createSignature(const SensorData & inputData, const Transfor
 				0,
 				_laserScanVoxelSize,
 				_laserScanNormalK,
-				_laserScanNormalRadius);
+				_laserScanNormalRadius,
+				_laserScanGroundNormalsUp);
 		t = timer.ticks();
 		if(stats) stats->addStatistic(Statistics::kTimingMemScan_filtering(), t*1000.0f);
 		UDEBUG("time normals scan = %fs", t);

corelib/src/OccupancyGrid.cpp

@@ -56,6 +56,7 @@ OccupancyGrid::OccupancyGrid(const ParametersMap & parameters) :
 	projMapFrame_(Parameters::defaultGridMapFrameProjection()),
 	maxObstacleHeight_(Parameters::defaultGridMaxObstacleHeight()),
 	normalKSearch_(Parameters::defaultGridNormalK()),
+	groundNormalsUp_(Parameters::defaultIcpPointToPlaneGroundNormalsUp()),
 	maxGroundAngle_(Parameters::defaultGridMaxGroundAngle()*M_PI/180.0f),
 	clusterRadius_(Parameters::defaultGridClusterRadius()),
 	minClusterSize_(Parameters::defaultGridMinClusterSize()),
@@ -115,6 +116,7 @@ void OccupancyGrid::parseParameters(const ParametersMap & parameters)
 	Parameters::parse(parameters, Parameters::kGridMinGroundHeight(), minGroundHeight_);
 	Parameters::parse(parameters, Parameters::kGridMaxGroundHeight(), maxGroundHeight_);
 	Parameters::parse(parameters, Parameters::kGridNormalK(), normalKSearch_);
+	Parameters::parse(parameters, Parameters::kIcpPointToPlaneGroundNormalsUp(), groundNormalsUp_);
 	if(Parameters::parse(parameters, Parameters::kGridMaxGroundAngle(), maxGroundAngle_))
 	{
 		maxGroundAngle_ *= M_PI/180.0f;
@@ -336,10 +338,11 @@ void OccupancyGrid::createLocalMap(
 				const Transform & t = node.sensorData().laserScanRaw().localTransform();
 				LaserScan scan = util3d::downsample(node.sensorData().laserScanRaw(), scanDecimation_);
 #ifdef RTABMAP_OCTOMAP
-				// clipping will be done in OctoMap
+				// If ray tracing enabled, clipping will be done in OctoMap or in occupancy2DFromLaserScan()
-				float maxRange = grid3D_&&rayTracing_?0.0f:cloudMaxDepth_;
+				float maxRange = rayTracing_?0.0f:cloudMaxDepth_;
 #else
-				float maxRange = cloudMaxDepth_;
+				// If ray tracing enabled, clipping will be done in occupancy2DFromLaserScan()
+				float maxRange = !grid3D_ && rayTracing_?0.0f:cloudMaxDepth_;
 #endif
 				if(cloudMinDepth_ > 0.0f || maxRange > 0.0f)
 				{
@@ -390,10 +393,11 @@ void OccupancyGrid::createLocalMap(
 					node.sensorData(),
 					cloudDecimation_,
 #ifdef RTABMAP_OCTOMAP
-					// clipping will be done in OctoMap
+					// If ray tracing enabled, clipping will be done in OctoMap or in occupancy2DFromLaserScan()
-					grid3D_&&rayTracing_?0.0f:cloudMaxDepth_,
+					rayTracing_?0.0f:cloudMaxDepth_,
 #else
-					cloudMaxDepth_,
+					// If ray tracing enabled, clipping will be done in occupancy2DFromLaserScan()
+					!grid3D_&&rayTracing_?0.0f:cloudMaxDepth_,
 #endif
 					cloudMinDepth_,
 					indices.get(),
@@ -401,6 +405,7 @@ void OccupancyGrid::createLocalMap(
 					roiRatios_);
 
 			// update viewpoint
+			viewPoint = cv::Point3f(0,0,0);
 			if(node.sensorData().cameraModels().size())
 			{
 				// average of all local transforms

corelib/src/OctoMap.cpp

@@ -891,7 +891,7 @@ bool OctoMap::update(const std::map<int, Transform> & poses)
 						}
 					}
 
-					RtabmapColorOcTreeNode * n = octree_->updateNode(*it, false, orderedPoses.size() == 1);
+					RtabmapColorOcTreeNode * n = octree_->updateNode(*it, false,  true);
 					if(n && n->getOccupancyType() == RtabmapColorOcTreeNode::kTypeUnknown)
 					{
 						n->setOccupancyType(RtabmapColorOcTreeNode::kTypeEmpty);
@@ -957,6 +957,10 @@ bool OctoMap::update(const std::map<int, Transform> & poses)
 							}
 						}
 					}
+				}
+
+				if((occupancyIter != cache_.end() && occupancyIter->second.second.cols) || !free_cells.empty())
+				{
 					octree_->updateInnerOccupancy();
 				}
 

corelib/src/Optimizer.cpp

@@ -190,7 +190,8 @@ void Optimizer::getConnectedGraph(
 		const std::map<int, Transform> & posesIn,
 		const std::multimap<int, Link> & linksIn,
 		std::map<int, Transform> & posesOut,
-		std::multimap<int, Link> & linksOut) const
+		std::multimap<int, Link> & linksOut,
+		bool adjustPosesWithConstraints) const
 {
 	UDEBUG("IN: fromId=%d poses=%d links=%d priorsIgnored=%d landmarksIgnored=%d", fromId, (int)posesIn.size(), (int)linksIn.size(), priorsIgnored()?1:0, landmarksIgnored()?1:0);
 	UASSERT(fromId>0);
@@ -243,23 +244,30 @@ void Optimizer::getConnectedGraph(
 				{
 					if(!uContains(posesOut, toId))
 					{
-						if(isSlam2d() && kter->second.type() == Link::kLandmark && toId>0)
+						if(adjustPosesWithConstraints)
 						{
-							Transform t;
+							if(isSlam2d() && kter->second.type() == Link::kLandmark && toId>0)
-							if(kter->second.from()==currentId)
 							{
-								t = kter->second.transform();
+								Transform t;
+								if(kter->second.from()==currentId)
+								{
+									t = kter->second.transform();
+								}
+								else
+								{
+									t = kter->second.transform().inverse();
+								}
+								posesOut.insert(std::make_pair(toId, (posesOut.at(currentId) * t).to3DoF()));
 							}
 							else
 							{
-								t = kter->second.transform().inverse();
+								Transform t = posesOut.at(currentId) * (kter->second.from()==currentId?kter->second.transform():kter->second.transform().inverse());
+								posesOut.insert(std::make_pair(toId, t));
 							}
-							posesOut.insert(std::make_pair(toId, (posesOut.at(currentId) * t).to3DoF()));
 						}
 						else
 						{
-							Transform t = posesOut.at(currentId) * (kter->second.from()==currentId?kter->second.transform():kter->second.transform().inverse());
+							posesOut.insert(*posesIn.find(toId));
-							posesOut.insert(std::make_pair(toId, t));
 						}
 						// add prior links
 						for(std::multimap<int, Link>::const_iterator pter=linksIn.find(toId); pter!=linksIn.end() && pter->first==toId; ++pter)

corelib/src/RegistrationIcp.cpp

@@ -136,7 +136,7 @@ void RegistrationIcp::parseParameters(const ParametersMap & parameters)
 	ParametersMap::const_iterator iter;
 	if((iter=parameters.find(Parameters::kRtabmapWorkingDirectory())) != parameters.end())
 	{
-		_workingDir = iter->second;
+		_workingDir = uReplaceChar(iter->second, '~', UDirectory::homeDir());
 	}
 
 	bool pointToPlane = _pointToPlane;
@@ -277,8 +277,12 @@ void RegistrationIcp::parseParameters(const ParametersMap & parameters)
 #ifndef RTABMAP_CCCORELIB
 	if(_strategy==2)
 	{
-		UWARN("Parameter %s is set to true but RTAB-Map has not been built with CCCoreLib support. Setting to 0.", Parameters::kIcpStrategy().c_str());
+#ifdef RTABMAP_POINTMATCHER
+		_strategy = 1;
+#else
 		_strategy = 0;
+#endif
+		UWARN("Parameter %s is set to 2 but RTAB-Map has not been built with CCCoreLib support. Setting to %d.", Parameters::kIcpStrategy().c_str(), _strategy);
 	}
 #else
 	if(_strategy==2 && _pointToPlane)
@@ -329,6 +333,7 @@ Transform RegistrationIcp::computeTransformationImpl(
 	UDEBUG("Max translation=%f", _maxTranslation);
 	UDEBUG("Max rotation=%f", _maxRotation);
 	UDEBUG("Downsampling step=%d", _downsamplingStep);
+	UDEBUG("Force 3DoF=%s", this->force3DoF()?"true":"false");
 	UDEBUG("Force 4DoF=%s", _force4DoF?"true":"false");
 	UDEBUG("Min Complexity=%f", _pointToPlaneMinComplexity);
 	UDEBUG("libpointmatcher (knn=%d, outlier ratio=%f)", _libpointmatcherKnn, _outlierRatio);

corelib/src/Rtabmap.cpp

@@ -121,6 +121,7 @@ Rtabmap::Rtabmap() :
 	_proximityRawPosesUsed(Parameters::defaultRGBDProximityPathRawPosesUsed()),
 	_proximityAngle(Parameters::defaultRGBDProximityAngle()*M_PI/180.0f),
 	_proximityOdomGuess(Parameters::defaultRGBDProximityOdomGuess()),
+	_proximityMergedScanCovFactor(Parameters::defaultRGBDProximityMergedScanCovFactor()),
 	_databasePath(""),
 	_optimizeFromGraphEnd(Parameters::defaultRGBDOptimizeFromGraphEnd()),
 	_optimizationMaxError(Parameters::defaultRGBDOptimizeMaxError()),
@@ -573,6 +574,9 @@ void Rtabmap::parseParameters(const ParametersMap & parameters)
 		_proximityAngle *= M_PI/180.0f;
 	}
 	Parameters::parse(parameters, Parameters::kRGBDProximityOdomGuess(), _proximityOdomGuess);
+	Parameters::parse(parameters, Parameters::kRGBDProximityMergedScanCovFactor(), _proximityMergedScanCovFactor);
+	UASSERT(_proximityMergedScanCovFactor>0.0);
+
 	bool optimizeFromGraphEndPrevious = _optimizeFromGraphEnd;
 	Parameters::parse(parameters, Parameters::kRGBDOptimizeFromGraphEnd(), _optimizeFromGraphEnd);
 	if(optimizeFromGraphEndPrevious != _optimizeFromGraphEnd && !_optimizedPoses.empty())
@@ -869,10 +873,26 @@ bool Rtabmap::labelLocation(int id, const std::string & label)
 		{
 			return _memory->labelSignature(id, label);
 		}
-		else if(_memory->getLastWorkingSignature())
+		else if(_memory->isIncremental() && _memory->getLastWorkingSignature())
 		{
 			return _memory->labelSignature(_memory->getLastWorkingSignature()->id(), label);
 		}
+		else if(!_memory->isIncremental() && !_lastLocalizationPose.isNull() && !_lastLocalizationPose.isIdentity())
+		{
+			std::map<int, Transform> nearestNodes = getNodesInRadius(_lastLocalizationPose, _localRadius, 1);
+			if(!nearestNodes.empty())
+			{
+				return _memory->labelSignature(nearestNodes.begin()->first, label);
+			}
+			else
+			{
+				UERROR("No nodes found inside %s=%fm of the current pose (%s). Cannot set label \"%s\"",
+						Parameters::kRGBDLocalRadius().c_str(),
+						_localRadius,
+						_lastLocalizationPose.prettyPrint().c_str(),
+						label.c_str());
+			}
+		}
 		else
 		{
 			UERROR("Last signature is null! Cannot set label \"%s\"", label.c_str());
@@ -1403,6 +1423,7 @@ bool Rtabmap::process(
 
 		// Update optimizedPoses with the newly added node
 		Transform newPose;
+		bool intermediateNodeRefining = false;
 		if(_neighborLinkRefining &&
 			signature->getLinks().size() &&
 			signature->getLinks().begin()->second.type() == Link::kNeighbor &&
@@ -1503,9 +1524,8 @@ bool Rtabmap::process(
 			}
 			else
 			{
-				UWARN("Neighbor link refining is activated but there are intermediate nodes (%d=%d %d=%d), aborting refining...",
-						signature->id(), signature->getWeight(), oldS->id(), oldS->getWeight());
 				newPose = _mapCorrection * signature->getPose();
+				intermediateNodeRefining = true;
 			}
 		}
 		else
@@ -1559,7 +1579,7 @@ bool Rtabmap::process(
 		}
 		// Localization mode stuff
 		_lastLocalizationPose = newPose; // keep in cache the latest corrected pose
-		if(!_memory->isIncremental())
+		if(!_memory->isIncremental() && signature->getWeight() >= 0)
 		{
 			UDEBUG("Update odometry localization cache (size=%d/%d)", (int)_odomCachePoses.size(), _maxOdomCacheSize);
 			if(!_odomCachePoses.empty())
@@ -1629,7 +1649,7 @@ bool Rtabmap::process(
 		//============================================================
 		// Local loop closure in TIME
 		//============================================================
-		if(_proximityByTime &&
+		if((_proximityByTime || intermediateNodeRefining) &&
 		   rehearsedId == 0 && // don't do it if rehearsal happened
 		   _memory->isIncremental() && // don't do it in localization mode
 		   signature->getWeight()>=0)
@@ -1680,6 +1700,12 @@ bool Rtabmap::process(
 						UINFO("Local loop closure (time) between %d and %d rejected: %s",
 								*iter, signature->id(), rejectedMsg.c_str());
 					}
+
+					if(!_proximityByTime && intermediateNodeRefining)
+					{
+						// Do it only with the latest non-intermediate node
+						break;
+					}
 				}
 			}
 		}
@@ -1719,7 +1745,7 @@ bool Rtabmap::process(
 					if(_optimizedPoses.size() && _memory->isIncremental())
 					{
 						//Search for latest node having GPS linked to current signature not too far.
-						std::map<int, float> nearestIds = graph::getNodesInRadius(signature->id(), _optimizedPoses, _localRadius);
+						std::map<int, float> nearestIds = graph::findNearestNodes(signature->id(), _optimizedPoses, _localRadius);
 						for(std::map<int, float>::reverse_iterator iter=nearestIds.rbegin(); iter!=nearestIds.rend() && iter->first>0; ++iter)
 						{
 							const Signature * s = _memory->getSignature(iter->first);
@@ -2215,7 +2241,7 @@ bool Rtabmap::process(
 
 			// retrieval based on the nodes close the the nearest pose in WM
 			// immunize closest nodes
-			std::map<int, float> nearNodes = graph::getNodesInRadius(signature->id(), _optimizedPoses, _localRadius);
+			std::map<int, float> nearNodes = graph::findNearestNodes(signature->id(), _optimizedPoses, _localRadius);
 			// sort by distance
 			std::multimap<float, int> nearNodesByDist;
 			for(std::map<int, float>::iterator iter=nearNodes.lower_bound(1); iter!=nearNodes.end(); ++iter)
@@ -2402,7 +2428,7 @@ bool Rtabmap::process(
 				}
 				else
 				{
-					nearestIds = graph::getNodesInRadius(signature->id(), _optimizedPoses, _localRadius);
+					nearestIds = graph::findNearestNodes(signature->id(), _optimizedPoses, _localRadius);
 				}
 				UDEBUG("nearestIds=%d/%d", (int)nearestIds.size(), (int)_optimizedPoses.size());
 				std::map<int, Transform> nearestPoses;
@@ -2453,7 +2479,7 @@ bool Rtabmap::process(
 
 					//find the nearest pose on the path looking in the same direction
 					path.insert(std::make_pair(signature->id(), _optimizedPoses.at(signature->id())));
-					path = graph::getPosesInRadius(signature->id(), path, _localRadius, _proximityAngle);
+					path = graph::findNearestPoses(signature->id(), path, _localRadius, _proximityAngle);
 					//take the one with highest likelihood if not null
 					int nearestId = 0;
 					if(iter->first.likelihood > 0.0f &&
@@ -2492,9 +2518,6 @@ bool Rtabmap::process(
 											nearestId,
 											transform.prettyPrint().c_str());
 									UASSERT(info.covariance.at<double>(0,0) > 0.0 && info.covariance.at<double>(5,5) > 0.0);
-									cv::Mat information = getInformation(info.covariance);
-									_memory->addLink(Link(signature->id(), nearestId, Link::kLocalSpaceClosure, transform, information));
-									loopClosureLinksAdded.push_back(std::make_pair(signature->id(), nearestId));
 
 									//for statistics
 									loopClosureVisualInliersMeanDist = info.inliersMeanDistance;
@@ -2507,20 +2530,29 @@ bool Rtabmap::process(
 									loopClosureVisualInliersRatio = info.inliersRatio;
 									loopClosureVisualMatches = info.matches;
 
+									cv::Mat information = getInformation(info.covariance);
 									loopClosureLinearVariance = 1.0/information.at<double>(0,0);
 									loopClosureAngularVariance = 1.0/information.at<double>(5,5);
 
+									Link::Type type = Link::kLocalSpaceClosure;
 									if(_loopClosureHypothesis.first>0 &&
 										nearestIds.find(_loopClosureHypothesis.first)!=nearestIds.end())
 									{
 										UDEBUG("Proximity detection on %d is close to loop closure %d, ignoring loop closure transform estimation...",
 												nearestId, _loopClosureHypothesis.first);
+										if(nearestId == _loopClosureHypothesis.first)
+										{
+											type = Link::kGlobalClosure;
+										}
 										// In localization mode, avoid transform
 										// computation on the global loop closure if a visual proximity
 										// one has been detected close (inside proximity radius) to that hypothesis.
 										loopIdSuppressedByProximity = _loopClosureHypothesis.first;
 										_loopClosureHypothesis.first = 0;
 									}
+
+									_memory->addLink(Link(signature->id(), nearestId, type, transform, information));
+									loopClosureLinksAdded.push_back(std::make_pair(signature->id(), nearestId));
 								}
 								else
 								{
@@ -2586,7 +2618,7 @@ bool Rtabmap::process(
 
 							// Assemble scans in the path and do ICP only
 							std::map<int, Transform> optimizedLocalPath;
-							if(_proximityRawPosesUsed)
+							if(_globalScanMap.empty() && _proximityRawPosesUsed)
 							{
 								//optimize the path's poses locally
 								cv::Mat covariance;
@@ -2610,7 +2642,7 @@ bool Rtabmap::process(
 							}
 
 							std::map<int, Transform> filteredPath;
-							if(optimizedLocalPath.size() > 2 && proximityFilteringRadius > 0.0f)
+							if(_globalScanMap.empty() && optimizedLocalPath.size() > 2 && proximityFilteringRadius > 0.0f)
 							{
 								// path filtering
 								filteredPath = graph::radiusPosesFiltering(optimizedLocalPath, proximityFilteringRadius, 0, true);
@@ -2639,6 +2671,7 @@ bool Rtabmap::process(
 									}
 									else
 									{
+										UASSERT_MSG(_globalScanMapPoses.find(nearestId) != _globalScanMapPoses.end(), uFormat("Pose of %d not found in global scan poses", nearestId).c_str());
 										icpMulti = false;
 										// use pre-assembled scan map
 										SensorData assembledData;
@@ -2684,7 +2717,7 @@ bool Rtabmap::process(
 
 										// set Identify covariance for laser scan matching only
 										UASSERT(info.covariance.at<double>(0,0) > 0.0 && info.covariance.at<double>(5,5) > 0.0);
-										_memory->addLink(Link(signature->id(), nearestId, Link::kLocalSpaceClosure, transform, getInformation(info.covariance)/100.0, scanMatchingIds));
+										_memory->addLink(Link(signature->id(), nearestId, Link::kLocalSpaceClosure, transform, getInformation(info.covariance)/_proximityMergedScanCovFactor, scanMatchingIds));
 										loopClosureLinksAdded.push_back(std::make_pair(signature->id(), nearestId));
 
 										if(icpMulti)
@@ -2925,7 +2958,8 @@ bool Rtabmap::process(
 				bool priorsIgnored = _graphOptimizer->priorsIgnored();
 				UDEBUG("priorsIgnored was %s", priorsIgnored?"true":"false");
 				_graphOptimizer->setPriorsIgnored(false); //temporary set false to use priors above to fix nodes of the map
-				_graphOptimizer->getConnectedGraph(signature->id(), poses, constraints, posesOut, edgeConstraintsOut);
+				// If slam2d: get connected graph while keeping original roll,pitch,z values.
+				_graphOptimizer->getConnectedGraph(signature->id(), poses, constraints, posesOut, edgeConstraintsOut, !_graphOptimizer->isSlam2d());
 				std::map<int, Transform> optPoses = _graphOptimizer->optimize(poses.begin()->first, posesOut, edgeConstraintsOut);
 				_graphOptimizer->setPriorsIgnored(priorsIgnored); // set back
 				for(std::map<int, Transform>::iterator iter=optPoses.begin(); iter!=optPoses.end(); ++iter)
@@ -2951,7 +2985,8 @@ bool Rtabmap::process(
 							maxLinearError,
 							maxAngularError,
 							&maxLinearLink,
-							&maxAngularLink);
+							&maxAngularLink,
+							_graphOptimizer->isSlam2d());
 					if(maxLinearLink == 0 && maxAngularLink==0 && _maxOdomCacheSize>0)
 					{
 						UWARN("Could not compute graph errors! Wrong loop closures could be accepted!");
@@ -3018,8 +3053,141 @@ bool Rtabmap::process(
 					}
 				}
 
+				bool hasGlobalLoopClosuresOrLandmarks = false;
+				if(rejectLocalization)
+				{
+					// Let's try again without local loop closures
+					localizationLinks = graph::filterLinks(localizationLinks, Link::kLocalSpaceClosure);
+					constraints = graph::filterLinks(constraints, Link::kLocalSpaceClosure);
+					for(std::multimap<int, Link>::iterator iter=constraints.begin(); iter!=constraints.end() && !hasGlobalLoopClosuresOrLandmarks; ++iter)
+					{
+						hasGlobalLoopClosuresOrLandmarks =
+								iter->second.type() == Link::kGlobalClosure ||
+								iter->second.type() == Link::kLandmark;
+					}
+					if(hasGlobalLoopClosuresOrLandmarks && !localizationLinks.empty())
+					{
+						rejectLocalization = false;
+						UWARN("Global and loop closures seem not tallying together, try again to optimize without local loop closures...");
+						priorsIgnored = _graphOptimizer->priorsIgnored();
+						UDEBUG("priorsIgnored was %s", priorsIgnored?"true":"false");
+						_graphOptimizer->setPriorsIgnored(false); //temporary set false to use priors above to fix nodes of the map
+						// If slam2d: get connected graph while keeping original roll,pitch,z values.
+						_graphOptimizer->getConnectedGraph(signature->id(), poses, constraints, posesOut, edgeConstraintsOut, !_graphOptimizer->isSlam2d());
+						optPoses = _graphOptimizer->optimize(poses.begin()->first, posesOut, edgeConstraintsOut);
+						_graphOptimizer->setPriorsIgnored(priorsIgnored); // set back
+						for(std::map<int, Transform>::iterator iter=optPoses.begin(); iter!=optPoses.end(); ++iter)
+						{
+							UDEBUG("Opt2  %d %s", iter->first, iter->second.prettyPrint().c_str());
+						}
+
+						if(optPoses.empty())
+						{
+							UWARN("Optimization failed, rejecting localization!");
+							rejectLocalization = true;
+						}
+						else if(_optimizationMaxError > 0.0f)
+						{
+							UINFO("Compute max graph errors...");
+							const Link * maxLinearLink = 0;
+							const Link * maxAngularLink = 0;
+							graph::computeMaxGraphErrors(
+									optPoses,
+									edgeConstraintsOut,
+									maxLinearErrorRatio,
+									maxAngularErrorRatio,
+									maxLinearError,
+									maxAngularError,
+									&maxLinearLink,
+									&maxAngularLink,
+									_graphOptimizer->isSlam2d());
+							if(maxLinearLink == 0 && maxAngularLink==0 && _maxOdomCacheSize>0)
+							{
+								UWARN("Could not compute graph errors! Wrong loop closures could be accepted!");
+								optPoses = posesOut;
+							}
+
+							if(maxLinearLink)
+							{
+								UINFO("Max optimization linear error = %f m (link %d->%d, var=%f, ratio error/std=%f, thr=%f)",
+										maxLinearError,
+										maxLinearLink->from(),
+										maxLinearLink->to(),
+										maxLinearLink->transVariance(),
+										maxLinearError/sqrt(maxLinearLink->transVariance()),
+										_optimizationMaxError);
+								if(maxLinearErrorRatio > _optimizationMaxError)
+								{
+									UWARN("Rejecting localization (%d <-> %d) in this "
+											"iteration because a wrong loop closure has been "
+											"detected after graph optimization, resulting in "
+											"a maximum graph error ratio of %f (edge %d->%d, type=%d, abs error=%f m, stddev=%f). The "
+											"maximum error ratio parameter \"%s\" is %f of std deviation.",
+											localizationLinks.rbegin()->second.from(),
+											localizationLinks.rbegin()->second.to(),
+											maxLinearErrorRatio,
+											maxLinearLink->from(),
+											maxLinearLink->to(),
+											maxLinearLink->type(),
+											maxLinearError,
+											sqrt(maxLinearLink->transVariance()),
+											Parameters::kRGBDOptimizeMaxError().c_str(),
+											_optimizationMaxError);
+									rejectLocalization = true;
+								}
+							}
+							if(maxAngularLink)
+							{
+								UINFO("Max optimization angular error = %f deg (link %d->%d, var=%f, ratio error/std=%f, thr=%f)",
+										maxAngularError*180.0f/CV_PI,
+										maxAngularLink->from(),
+										maxAngularLink->to(),
+										maxAngularLink->rotVariance(),
+										maxAngularError/sqrt(maxAngularLink->rotVariance()),
+										_optimizationMaxError);
+								if(maxAngularErrorRatio > _optimizationMaxError)
+								{
+									UWARN("Rejecting localization (%d <-> %d) in this "
+											"iteration because a wrong loop closure has been "
+											"detected after graph optimization, resulting in "
+											"a maximum graph error ratio of %f (edge %d->%d, type=%d, abs error=%f deg, stddev=%f). The "
+											"maximum error ratio parameter \"%s\" is %f of std deviation.",
+											localizationLinks.rbegin()->second.from(),
+											localizationLinks.rbegin()->second.to(),
+											maxAngularErrorRatio,
+											maxAngularLink->from(),
+											maxAngularLink->to(),
+											maxAngularLink->type(),
+											maxAngularError*180.0f/CV_PI,
+											sqrt(maxAngularLink->rotVariance()),
+											Parameters::kRGBDOptimizeMaxError().c_str(),
+											_optimizationMaxError);
+									rejectLocalization = true;
+								}
+							}
+						}
+					}
+				}
+
 				if(!rejectLocalization)
 				{
+					if(hasGlobalLoopClosuresOrLandmarks)
+					{
+						// We successfully optimize the graph without local loop closures,
+						// clear them as some of them may be wrong.
+						size_t before = _odomCacheConstraints.size();
+						_odomCacheConstraints = graph::filterLinks(_odomCacheConstraints, Link::kLocalSpaceClosure);
+						if(before != _odomCacheConstraints.size())
+						{
+							UWARN("Successfully optimized without local loop closures! Clear them from local odometry cache. %ld/%ld have been removed.",
+									before - _odomCacheConstraints.size(), before);
+						}
+						else
+						{
+							UWARN("Successfully optimized without local loop closures!");
+						}
+					}
+
 					// Count how many localization links are in the constraints
 					bool hadAlreadyLocalizationLinks = false;
 					for(std::multimap<int, Link>::iterator iter=_odomCacheConstraints.begin();
@@ -3054,11 +3222,7 @@ bool Rtabmap::process(
 					// At least 2 localizations at 2 different time required
 					if(hadAlreadyLocalizationLinks || _maxOdomCacheSize == 0)
 					{
-						// If there are no signatures retrieved, we don't
+						UINFO("Update localization");
-						// need to re-optimize the graph. Just update the last
-						// position if OptimizeFromGraphEnd=false or transform the
-						// whole graph if OptimizeFromGraphEnd=true
-						UINFO("Localization without map optimization");
 						if(_optimizeFromGraphEnd)
 						{
 							// update all previous nodes
@@ -3067,12 +3231,7 @@ bool Rtabmap::process(
 							Transform oldPose = _optimizedPoses.at(localizationLinks.rbegin()->first);
 							Transform mapCorrectionInv = _mapCorrection.inverse();
 							Transform u = signature->getPose() * localizationLinks.rbegin()->second.transform();
-							if(_graphOptimizer->isSlam2d())
+							if(_graphOptimizer->gravitySigma() > 0)
-							{
-								// in case of 3d landmarks, transform constraint to 2D
-								u = u.to3DoF();
-							}
-							else if(_graphOptimizer->gravitySigma() > 0)
 							{
                                 // Adjust transform with gravity
                                 Transform transform = localizationLinks.rbegin()->second.transform();
@@ -3122,6 +3281,11 @@ bool Rtabmap::process(
                                 }
 							}
 							Transform up = u * oldPose.inverse();
+							if(_graphOptimizer->isSlam2d())
+							{
+								// in case of 3d landmarks, transform constraint to 2D
+								up.to3DoF();
+							}
 							for(std::map<int, Transform>::iterator iter=_optimizedPoses.begin(); iter!=_optimizedPoses.end(); ++iter)
 							{
 								iter->second = mapCorrectionInv * up * iter->second;
@@ -3132,7 +3296,7 @@ bool Rtabmap::process(
 						{
 							Transform newPose = _optimizedPoses.at(localizationLinks.rbegin()->first) * localizationLinks.rbegin()->second.transform().inverse();
 							UDEBUG("newPose=%s", newPose.prettyPrint().c_str());
-							if(_graphOptimizer->isSlam2d())
+							if(_graphOptimizer->isSlam2d() && signature->getPose().is3DoF())
 							{
 								// in case of 3d landmarks, transform constraint to 2D
 								newPose = newPose.to3DoF();
@@ -3607,10 +3771,8 @@ bool Rtabmap::process(
 
 	Signature lastSignatureData(signature->id());
 	Transform lastSignatureLocalizedPose;
-	if(_optimizedPoses.find(signature->id()) != _optimizedPoses.end() &&
+	if(_optimizedPoses.find(signature->id()) != _optimizedPoses.end())
-		graph::filterLinks(signature->getLinks(), Link::kSelfRefLink).size())
 	{
-		// only if localized set it
 		lastSignatureLocalizedPose = _optimizedPoses.at(signature->id());
 	}
 	if(_publishLastSignatureData)
@@ -3762,8 +3924,8 @@ bool Rtabmap::process(
 
 			if(signaturesRemoved.size() == 1 && signaturesRemoved.front() == lastSignatureData.id())
 			{
-				UDEBUG("Detected that only last signature has been removed");
 				int lastId = signaturesRemoved.front();
+				UDEBUG("Detected that only last signature has been removed (lastId=%d)", lastId);
 				_optimizedPoses.erase(lastId);
 				for(std::multimap<int, Link>::iterator iter=_constraints.find(lastId); iter!=_constraints.end() && iter->first==lastId;++iter)
 				{
@@ -4275,7 +4437,7 @@ std::map<int, Transform> Rtabmap::getForwardWMPoses(
 		}
 		else
 		{
-			foundIds = graph::getNodesInRadius(fromId, _optimizedPoses, radius);
+			foundIds = graph::findNearestNodes(fromId, _optimizedPoses, radius);
 		}
 
 		float radiusSqrd = radius * radius;
@@ -4861,24 +5023,48 @@ void Rtabmap::getGraph(
 	}
 }
 
-std::map<int, Transform> Rtabmap::getNodesInRadius(const Transform & pose, float radius)
+std::map<int, Transform> Rtabmap::getNodesInRadius(const Transform & pose, float radius, int k, std::map<int, float> * distsSqr)
 {
-	return graph::getPosesInRadius(pose, _optimizedPoses, radius<=0?_localRadius:radius);
+	std::map<int, float> nearestNodesTmp;
+	std::map<int, float> * nearestNodesPtr = distsSqr == 0? &nearestNodesTmp : distsSqr;
+	*nearestNodesPtr = graph::findNearestNodes(pose, _optimizedPoses, radius<=0?_localRadius:radius, 0, k);
+	std::map<int, Transform> nearestPoses;
+	for(std::map<int, float>::iterator iter=nearestNodesPtr->begin(); iter!=nearestNodesPtr->end(); ++iter)
+	{
+		nearestPoses.insert(*_optimizedPoses.find(iter->first));
+	}
+	return nearestPoses;
 }
 
-std::map<int, Transform> Rtabmap::getNodesInRadius(int nodeId, float radius)
+std::map<int, Transform> Rtabmap::getNodesInRadius(int nodeId, float radius, int k, std::map<int, float> * distsSqr)
 {
 	UDEBUG("nodeId=%d, radius=%f", nodeId, radius);
-	std::map<int, Transform> nearNodes;
+	std::map<int, float> nearestNodesTmp;
-	if(nodeId==0 && !_optimizedPoses.empty())
+	std::map<int, float> * nearestNodesPtr = distsSqr == 0? &nearestNodesTmp : distsSqr;
+	if(nodeId==0 && !_lastLocalizationPose.isNull() && !_lastLocalizationPose.isIdentity())
 	{
-		nodeId = _optimizedPoses.rbegin()->first;
+		*nearestNodesPtr = graph::findNearestNodes(_lastLocalizationPose, _optimizedPoses, radius<=0?_localRadius:radius, 0, k);
 	}
-	if(_optimizedPoses.find(nodeId) != _optimizedPoses.end())
+	else
 	{
-		nearNodes = graph::getPosesInRadius(nodeId, _optimizedPoses, radius<=0?_localRadius:radius);
+		if(nodeId==0 && !_optimizedPoses.empty())
+		{
+			nodeId = _optimizedPoses.rbegin()->first;
+		}
+
+		if(_optimizedPoses.find(nodeId) != _optimizedPoses.end())
+		{
+			*nearestNodesPtr = graph::findNearestNodes(nodeId, _optimizedPoses, radius<=0?_localRadius:radius, 0, k);
+		}
 	}
-	return nearNodes;
+
+	std::map<int, Transform> nearestPoses;
+	for(std::map<int, float>::iterator iter=nearestNodesPtr->begin(); iter!=nearestNodesPtr->end(); ++iter)
+	{
+		nearestPoses.insert(*_optimizedPoses.find(iter->first));
+	}
+
+	return nearestPoses;
 }
 
 int Rtabmap::detectMoreLoopClosures(
@@ -5564,7 +5750,8 @@ bool Rtabmap::addLink(const Link & link)
 					maxLinearError,
 					maxAngularError,
 					&maxLinearLink,
-					&maxAngularLink);
+					&maxAngularLink,
+					_graphOptimizer->isSlam2d());
 			if(maxLinearLink == 0 && maxAngularLink==0)
 			{
 				UWARN("Could not compute graph errors! Wrong loop closures could be accepted!");
@@ -6300,6 +6487,7 @@ void Rtabmap::createGlobalScanMap()
 			data.uncompressDataConst(0, 0, &scan, 0, 0, 0, 0);
 			if(!scan.empty())
 			{
+				UDEBUG("Adding scan %d (format=%s, points=%d)", iter->first, scan.formatName().c_str(), scan.size());
 				scan = util3d::transformLaserScan(scan, iter->second*scan.localTransform());
 				if(_globalScanMap.empty() || _globalScanMap.format() == scan.format())
 				{
@@ -6318,6 +6506,16 @@ void Rtabmap::createGlobalScanMap()
 					break;
 				}
 			}
+			else
+			{
+				UDEBUG("Ignored %d (scan is empty), pose still added.", iter->first);
+				_globalScanMapPoses.insert(*iter);
+			}
+		}
+		else
+		{
+			UDEBUG("Ignored %d (no scan), pose still added.", iter->first);
+			_globalScanMapPoses.insert(*iter);
 		}
 	}
 	if(_globalScanMap.size() > 3)
@@ -6345,11 +6543,11 @@ void Rtabmap::createGlobalScanMap()
 		}
 
 		UINFO("Global scan map has been assembled (size=%d points, %d poses) "
-				"for proximity detection (only in localization mode %s=false and with %s=false)",
+				"for proximity detection (only in localization mode %s=false and with %s=true)",
 				(int)_globalScanMap.size(),
 				(int)_globalScanMapPoses.size(),
 				Parameters::kMemIncrementalMemory().c_str(),
-				Parameters::kRGBDProximityPathRawPosesUsed().c_str());
+				Parameters::kRGBDProximityGlobalScanMap().c_str());
 
 		//for debugging...
 		if(!_globalScanMap.empty() && ULogger::level() == ULogger::kDebug)

corelib/src/RtabmapThread.cpp

@@ -292,9 +292,16 @@ void RtabmapThread::mainLoop()
 		_rtabmap->clearPath(0);
 		break;
 	case kStateLabelling:
-		if(!_rtabmap->labelLocation(atoi(parameters.at("id").c_str()), parameters.at("label").c_str()))
+		if(!_rtabmap->labelLocation(atoi(parameters.at("id").c_str()), parameters.at("label")))
 		{
-			this->post(new RtabmapLabelErrorEvent(atoi(parameters.at("id").c_str()), parameters.at("label").c_str()));
+			this->post(new RtabmapLabelErrorEvent(atoi(parameters.at("id").c_str()), parameters.at("label")));
+		}
+		break;
+	case kStateRemovingLabel:
+		id = _rtabmap->getMemory()->getSignatureIdByLabel(parameters.at("label"), true);
+		if(!_rtabmap->labelLocation(id, ""))
+		{
+			this->post(new RtabmapLabelErrorEvent(id, parameters.at("label")));
 		}
 		break;
 	default:

corelib/src/Transform.cpp

@@ -221,6 +221,20 @@ Transform Transform::to4DoF() const
 	return Transform(x,y,z, 0,0,yaw);
 }
 
+bool Transform::is3DoF() const
+{
+	return is4DoF() && z() == 0.0;
+}
+
+bool Transform::is4DoF() const
+{
+	return r13() == 0.0 &&
+		   r23() == 0.0 &&
+		   r31() == 0.0 &&
+		   r32() == 0.0 &&
+		   r33() == 0.0;
+}
+
 cv::Mat Transform::rotationMatrix() const
 {
 	return data_.colRange(0, 3).clone();

corelib/src/icp/cccorelib.h

@@ -137,15 +137,14 @@ rtabmap::Transform icpCC(
 		icpTransformation.setNull();
 		return icpTransformation;
 	}
-	else if(finalPointCount < 50)
+	else if(!transform.R.isValid())
 	{
-		std::string msg = uFormat("CCCoreLib has failed: Rejecting transform as finalPointCount %d < 50 ", finalPointCount);
+		std::string msg = uFormat("CCCoreLib has failed: Rotation matrix is invalid");
 		UDEBUG(msg.c_str());
 		if(errorMsg)
 		{
 			*errorMsg = msg;
 		}
-
 		icpTransformation.setNull();
 		return icpTransformation;
 	}

corelib/src/odometry/OdometryMono.cpp

@@ -205,7 +205,14 @@ Transform OdometryMono::computeTransform(SensorData & data, const Transform & gu
 	{
 		cv::Mat newFrame;
 		cv::cvtColor(data.imageRaw(), newFrame, cv::COLOR_BGR2GRAY);
-		data.setImageRaw(newFrame);
+		if(data.stereoCameraModel().isValidForProjection())
+		{
+			data.setStereoImage(newFrame, data.rightRaw(), data.stereoCameraModel());
+		}
+		else
+		{
+			data.setRGBDImage(newFrame, data.depthRaw(), data.cameraModels());
+		}
 	}
 
 	if(!localMap_.empty())

corelib/src/optimizer/OptimizerG2O.cpp

@@ -137,6 +137,28 @@ bool OptimizerG2O::isCholmodAvailable()
 #endif
 }
 
+OptimizerG2O::OptimizerG2O(const ParametersMap & parameters) :
+		Optimizer(parameters),
+		solver_(Parameters::defaultg2oSolver()),
+		optimizer_(Parameters::defaultg2oOptimizer()),
+		pixelVariance_(Parameters::defaultg2oPixelVariance()),
+		robustKernelDelta_(Parameters::defaultg2oRobustKernelDelta()),
+		baseline_(Parameters::defaultg2oBaseline())
+{
+#ifdef RTABMAP_G2O
+	// Issue on android, have to explicitly register this type when using fixed root prior below
+	if(!g2o::Factory::instance()->knowsTag("CACHE_SE3_OFFSET"))
+	{
+#if defined(RTABMAP_G2O_CPP11) and RTABMAP_G2O_CPP11 == 1
+		g2o::Factory::instance()->registerType("CACHE_SE3_OFFSET", g2o::make_unique<g2o::HyperGraphElementCreator<g2o::CacheSE3Offset> >());
+#else
+		g2o::Factory::instance()->registerType("CACHE_SE3_OFFSET", new g2o::HyperGraphElementCreator<g2o::CacheSE3Offset>);
+#endif
+	}
+#endif
+	parseParameters(parameters);
+}
+
 void OptimizerG2O::parseParameters(const ParametersMap & parameters)
 {
 	Optimizer::parseParameters(parameters);
@@ -322,7 +344,9 @@ std::map<int, Transform> OptimizerG2O::optimize(
 						rootId = 0;
 						break;
 					}
-					else if(iter->second.type() == Link::kGravity)
+					else if(!isSlam2d() &&
+							gravitySigma() > 0 &&
+							iter->second.type() == Link::kGravity)
 					{
 						hasGravityConstraints = true;
 						if(priorsIgnored())
@@ -337,7 +361,7 @@ std::map<int, Transform> OptimizerG2O::optimize(
 		int landmarkVertexOffset = poses.rbegin()->first+1;
 		std::map<int, bool> isLandmarkWithRotation;
 
-		UDEBUG("fill poses to g2o... (rootId=%d hasGravityConstraints=%d)", rootId, hasGravityConstraints?1:0);
+		UDEBUG("fill poses to g2o... (rootId=%d hasGravityConstraints=%d isSlam2d=%d)", rootId, hasGravityConstraints?1:0, isSlam2d()?1:0);
 		for(std::map<int, Transform>::const_iterator iter = poses.begin(); iter!=poses.end(); ++iter)
 		{
 			UASSERT(!iter->second.isNull());
@@ -454,32 +478,41 @@ std::map<int, Transform> OptimizerG2O::optimize(
 			{
 				vertex->setId(id);
 				UASSERT_MSG(optimizer.addVertex(vertex), uFormat("cannot insert vertex %d!?", iter->first).c_str());
-				if(!isSlam2d() && id == rootId && hasGravityConstraints)
+			}
+		}
+
+		// Setup root prior (fixed x,y,z,yaw)
+		if(!isSlam2d() && rootId !=0 && hasGravityConstraints)
+		{
+			g2o::VertexSE3* v1 = dynamic_cast<g2o::VertexSE3*>(optimizer.vertex(rootId));
+			if(v1)
+			{
+				g2o::EdgeSE3Prior * e = new g2o::EdgeSE3Prior();
+				e->setVertex(0, v1);
+				Eigen::Affine3d a = poses.at(rootId).toEigen3d();
+				Eigen::Isometry3d pose;
+				pose = a.linear();
+				pose.translation() = a.translation();
+				e->setMeasurement(pose);
+				e->setParameterId(0, PARAM_OFFSET);
+				Eigen::Matrix<double, 6, 6> information = Eigen::Matrix<double, 6, 6>::Identity()*10e6;
+				// pitch and roll not fixed
+				information(3,3) = information(4,4) = 1;
+				e->setInformation(information);
+				if (!optimizer.addEdge(e))
 				{
-					g2o::EdgeSE3Prior * priorEdge = new g2o::EdgeSE3Prior();
+					delete e;
-					g2o::VertexSE3* v1 = (g2o::VertexSE3*)vertex;
+					UERROR("Map: Failed adding fixed constraint of rootid %d, set as fixed instead", rootId);
-					priorEdge->setVertex(0, v1);
+					v1->setFixed(true);
-					Eigen::Affine3d a = iter->second.toEigen3d();
-					Eigen::Isometry3d pose;
-					pose = a.linear();
-					pose.translation() = a.translation();
-					priorEdge->setMeasurement(pose);
-					priorEdge->setParameterId(0, PARAM_OFFSET);
-					Eigen::Matrix<double, 6, 6> information = Eigen::Matrix<double, 6, 6>::Identity()*10e6;
-					// pitch and roll not fixed
-					information(3,3) = information(4,4) = 1;
-					priorEdge->setInformation(information);
-					if (priorEdge && !optimizer.addEdge(priorEdge))
-					{
-						delete priorEdge;
-						UERROR("Map: Failed adding fixed constraint of rootid %d, set as fixed instead", id);
-						v1->setFixed(true);
-					}
-					else
-					{
-						UDEBUG("Set %d fixed with prior (have gravity constraints)", id);
-					}
 				}
+				else
+				{
+					UDEBUG("Set %d fixed with prior (have gravity constraints)", rootId);
+				}
+			}
+			else
+			{
+				UERROR("Map: Failed adding fixed constraint of rootid %d (not found in added vertices)", rootId);
 			}
 		}
 

corelib/src/optimizer/OptimizerGTSAM.cpp

@@ -625,8 +625,9 @@ std::map<int, Transform> OptimizerGTSAM::optimize(
 				{
 					if(key > 0)
 					{
+						poses.at(key).getTranslationAndEulerAngles(x,y,z,roll,pitch,yaw);
 						gtsam::Pose2 p = iter->value.cast<gtsam::Pose2>();
-						optimizedPoses.insert(std::make_pair(key, Transform(p.x(), p.y(), p.theta())));
+						optimizedPoses.insert(std::make_pair(key, Transform(p.x(), p.y(), z, roll, pitch, p.theta())));
 					}
 					else if(!landmarksIgnored() && isLandmarkWithRotation.find(key)!=isLandmarkWithRotation.end())
 					{

corelib/src/rtflann/util/any.h

@@ -78,7 +78,9 @@ struct big_any_policy : typed_base_any_policy<T>
 {
     virtual void static_delete(void** x)
     {
-        if (* x) delete (* reinterpret_cast<T**>(x)); *x = NULL;
+        if (* x) 
+            delete (* reinterpret_cast<T**>(x)); 
+        *x = NULL;
     }
     virtual void copy_from_value(void const* src, void** dest)
     {

corelib/src/util3d.cpp

@@ -2848,6 +2848,7 @@ std::vector<std::pair< std::pair<int, int>, pcl::PointXY> > projectCloudToCamera
 		float maxDistance,
 		float maxAngle,
 		const std::vector<float> & roiRatios,
+		const cv::Mat & projMask,
 		bool distanceToCamPolicy,
 		const ProgressState * state)
 {
@@ -2857,6 +2858,8 @@ std::vector<std::pair< std::pair<int, int>, pcl::PointXY> > projectCloudToCamera
 	UINFO("maxDistance=%f", maxDistance);
 	UINFO("maxAngle=%f", maxAngle);
 	UINFO("distanceToCamPolicy=%s", distanceToCamPolicy?"true":"false");
+	UINFO("roiRatios=%s", roiRatios.size() == 4?uFormat("%f %f %f %f", roiRatios[0], roiRatios[1], roiRatios[2], roiRatios[3]):"");
+	UINFO("projMask=%dx%d", projMask.cols, projMask.rows);
 	std::vector<std::pair< std::pair<int, int>, pcl::PointXY> > pointToPixel;
 
 	if (cloud.empty() || cameraPoses.empty() || cameraModels.empty())
@@ -2873,18 +2876,46 @@ std::vector<std::pair< std::pair<int, int>, pcl::PointXY> > projectCloudToCamera
 
 	std::vector<ProjectionInfo> invertedIndex(cloud.size()); // For each point: list of cameras
 	int cameraProcessed = 0;
+	bool wrongMaskFormatWarned = false;
 	for(std::map<int, Transform>::const_iterator pter = cameraPoses.lower_bound(0); pter!=cameraPoses.end(); ++pter)
 	{
 		std::map<int, std::vector<CameraModel> >::const_iterator iter=cameraModels.find(pter->first);
 		if(iter!=cameraModels.end() && !iter->second.empty())
 		{
-			for(size_t i=0; i<iter->second.size(); ++i)
+			cv::Mat validProjMask;
+			if(!projMask.empty())
 			{
-				Transform cameraTransform = (pter->second * iter->second[i].localTransform());
+				if(projMask.type() != CV_8UC1)
+				{
+					if(!wrongMaskFormatWarned)
+						UERROR("Wrong camera projection mask type %d, should be CV_8UC1", projMask.type());
+					wrongMaskFormatWarned = true;
+				}
+				else if(projMask.cols == iter->second[0].imageWidth() * (int)iter->second.size() &&
+				        projMask.rows == iter->second[0].imageHeight())
+				{
+					validProjMask = projMask;
+				}
+				else
+				{
+					UWARN("Camera projection mask (%dx%d) is not valid for current "
+						   "camera model(s) (count=%ld, image size=%dx%d). It will be "
+						   "ignored for node %d",
+						   projMask.cols, projMask.rows,
+						   iter->second.size(),
+						   iter->second[0].imageWidth(),
+						   iter->second[0].imageHeight(),
+						   pter->first);
+				}
+			}
+
+			for(size_t camIndex=0; camIndex<iter->second.size(); ++camIndex)
+			{
+				Transform cameraTransform = (pter->second * iter->second[camIndex].localTransform());
 				UASSERT(!cameraTransform.isNull());
-				cv::Mat cameraMatrixK = iter->second[i].K();
+				cv::Mat cameraMatrixK = iter->second[camIndex].K();
 				UASSERT(cameraMatrixK.type() == CV_64FC1 && cameraMatrixK.cols == 3 && cameraMatrixK.cols == 3);
-				const cv::Size & imageSize = iter->second[i].imageSize();
+				const cv::Size & imageSize = iter->second[camIndex].imageSize();
 
 				float fx = cameraMatrixK.at<double>(0,0);
 				float fy = cameraMatrixK.at<double>(1,1);
@@ -2921,7 +2952,8 @@ std::vector<std::pair< std::pair<int, int>, pcl::PointXY> > projectCloudToCamera
 						int dx_high = dx + 0.5f;
 						int dy_high = dy + 0.5f;
 						int zMM = z * 1000;
-						if(uIsInBounds(dx_low, roi.x, roi.x+roi.width) && uIsInBounds(dy_low, roi.y, roi.y+roi.height))
+						if(uIsInBounds(dx_low, roi.x, roi.x+roi.width) && uIsInBounds(dy_low, roi.y, roi.y+roi.height) &&
+						   (validProjMask.empty() || validProjMask.at<unsigned char>(dy_low, imageSize.width*camIndex+dx_low) > 0))
 						{
 							set = true;
 							cv::Vec2i &zReg = registered.at<cv::Vec2i>(dy_low, dx_low);
@@ -2932,7 +2964,8 @@ std::vector<std::pair< std::pair<int, int>, pcl::PointXY> > projectCloudToCamera
 							}
 						}
 						if((dx_low != dx_high || dy_low != dy_high) &&
-							uIsInBounds(dx_high, roi.x, roi.x+roi.width) && uIsInBounds(dy_high, roi.y, roi.y+roi.height))
+							uIsInBounds(dx_high, roi.x, roi.x+roi.width) && uIsInBounds(dy_high, roi.y, roi.y+roi.height) &&
+							(validProjMask.empty() || validProjMask.at<unsigned char>(dy_high, imageSize.width*camIndex+dx_high) > 0))
 						{
 							set = true;
 							cv::Vec2i &zReg = registered.at<cv::Vec2i>(dy_high, dx_high);
@@ -2951,11 +2984,11 @@ std::vector<std::pair< std::pair<int, int>, pcl::PointXY> > projectCloudToCamera
 				if(count == 0)
 				{
 					registered = cv::Mat();
-					UINFO("No points projected in camera %d/%d", pter->first, i);
+					UINFO("No points projected in camera %d/%d", pter->first, camIndex);
 				}
 				else
 				{
-					UDEBUG("%d points projected in camera %d/%d", count, pter->first, i);
+					UDEBUG("%d points projected in camera %d/%d", count, pter->first, camIndex);
 				}
 				for(int u=0; u<registered.cols; ++u)
 				{
@@ -2966,7 +2999,7 @@ std::vector<std::pair< std::pair<int, int>, pcl::PointXY> > projectCloudToCamera
 						{
 							ProjectionInfo info;
 							info.nodeID = pter->first;
-							info.cameraIndex = i;
+							info.cameraIndex = camIndex;
 							info.uv.x = float(u)/float(imageSize.width);
 							info.uv.y = float(v)/float(imageSize.height);
 							const Transform & cam = cameraPoses.at(info.nodeID);
@@ -3070,6 +3103,7 @@ std::vector<std::pair< std::pair<int, int>, pcl::PointXY> > projectCloudToCamera
 		float maxDistance,
 		float maxAngle,
 		const std::vector<float> & roiRatios,
+		const cv::Mat & projMask,
 		bool distanceToCamPolicy,
 		const ProgressState * state)
 {
@@ -3079,6 +3113,7 @@ std::vector<std::pair< std::pair<int, int>, pcl::PointXY> > projectCloudToCamera
 			maxDistance,
 			maxAngle,
 			roiRatios,
+			projMask,
 			distanceToCamPolicy,
 			state);
 }
@@ -3090,6 +3125,7 @@ std::vector<std::pair< std::pair<int, int>, pcl::PointXY> > projectCloudToCamera
 		float maxDistance,
 		float maxAngle,
 		const std::vector<float> & roiRatios,
+		const cv::Mat & projMask,
 		bool distanceToCamPolicy,
 		const ProgressState * state)
 {
@@ -3099,6 +3135,7 @@ std::vector<std::pair< std::pair<int, int>, pcl::PointXY> > projectCloudToCamera
 			maxDistance,
 			maxAngle,
 			roiRatios,
+			projMask,
 			distanceToCamPolicy,
 			state);
 }

corelib/src/util3d_filtering.cpp

@@ -664,6 +664,7 @@ typename pcl::PointCloud<PointT>::Ptr randomSamplingImpl(
 	typename pcl::PointCloud<PointT>::Ptr output(new pcl::PointCloud<PointT>);
 	pcl::RandomSample<PointT> filter;
 	filter.setSample(samples);
+	filter.setSeed (std::rand ());
 	filter.setInputCloud(cloud);
 	filter.filter(*output);
 	return output;
@@ -672,10 +673,26 @@ pcl::PointCloud<pcl::PointXYZ>::Ptr randomSampling(const pcl::PointCloud<pcl::Po
 {
 	return randomSamplingImpl<pcl::PointXYZ>(cloud, samples);
 }
+pcl::PointCloud<pcl::PointNormal>::Ptr randomSampling(const pcl::PointCloud<pcl::PointNormal>::Ptr & cloud, int samples)
+{
+	return randomSamplingImpl<pcl::PointNormal>(cloud, samples);
+}
 pcl::PointCloud<pcl::PointXYZRGB>::Ptr randomSampling(const pcl::PointCloud<pcl::PointXYZRGB>::Ptr & cloud, int samples)
 {
 	return randomSamplingImpl<pcl::PointXYZRGB>(cloud, samples);
 }
+pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr randomSampling(const pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr & cloud, int samples)
+{
+	return randomSamplingImpl<pcl::PointXYZRGBNormal>(cloud, samples);
+}
+pcl::PointCloud<pcl::PointXYZI>::Ptr randomSampling(const pcl::PointCloud<pcl::PointXYZI>::Ptr & cloud, int samples)
+{
+	return randomSamplingImpl<pcl::PointXYZI>(cloud, samples);
+}
+pcl::PointCloud<pcl::PointXYZINormal>::Ptr randomSampling(const pcl::PointCloud<pcl::PointXYZINormal>::Ptr & cloud, int samples)
+{
+	return randomSamplingImpl<pcl::PointXYZINormal>(cloud, samples);
+}
 
 template<typename PointT>
 pcl::IndicesPtr passThroughImpl(
@@ -1106,7 +1123,7 @@ pcl::IndicesPtr radiusFilteringImpl(
 		{
 			std::vector<int> kIndices;
 			std::vector<float> kDistances;
-			int k = tree->radiusSearch(cloud->at(indices->at(i)), radiusSearch, kIndices, kDistances);
+			int k = tree->radiusSearch(cloud->at(indices->at(i)), radiusSearch, kIndices, kDistances, minNeighborsInRadius+1);
 			if(k > minNeighborsInRadius)
 			{
 				output->at(oi++) = indices->at(i);
@@ -1124,7 +1141,7 @@ pcl::IndicesPtr radiusFilteringImpl(
 		{
 			std::vector<int> kIndices;
 			std::vector<float> kDistances;
-			int k = tree->radiusSearch(cloud->at(i), radiusSearch, kIndices, kDistances);
+			int k = tree->radiusSearch(cloud->at(i), radiusSearch, kIndices, kDistances, minNeighborsInRadius+1);
 			if(k > minNeighborsInRadius)
 			{
 				output->at(oi++) = i;
@@ -1160,6 +1177,239 @@ pcl::IndicesPtr radiusFiltering(const pcl::PointCloud<pcl::PointXYZINormal>::Ptr
 	return radiusFilteringImpl<pcl::PointXYZINormal>(cloud, indices, radiusSearch, minNeighborsInRadius);
 }
 
+pcl::IndicesPtr proportionalRadiusFiltering(
+		const pcl::PointCloud<pcl::PointXYZ>::Ptr & cloud,
+		const std::vector<int> & viewpointIndices,
+		const std::map<int, Transform> & viewpoints,
+		float factor,
+		float neighborScale)
+{
+	pcl::IndicesPtr indices(new std::vector<int>);
+	return proportionalRadiusFiltering(cloud, indices, viewpointIndices, viewpoints, factor, neighborScale);
+}
+pcl::IndicesPtr proportionalRadiusFiltering(
+		const pcl::PointCloud<pcl::PointNormal>::Ptr & cloud,
+		const std::vector<int> & viewpointIndices,
+		const std::map<int, Transform> & viewpoints,
+		float factor,
+		float neighborScale)
+{
+	pcl::IndicesPtr indices(new std::vector<int>);
+	return proportionalRadiusFiltering(cloud, indices, viewpointIndices, viewpoints, factor, neighborScale);
+}
+pcl::IndicesPtr proportionalRadiusFiltering(
+		const pcl::PointCloud<pcl::PointXYZRGB>::Ptr & cloud,
+		const std::vector<int> & viewpointIndices,
+		const std::map<int, Transform> & viewpoints,
+		float factor,
+		float neighborScale)
+{
+	pcl::IndicesPtr indices(new std::vector<int>);
+	return proportionalRadiusFiltering(cloud, indices, viewpointIndices, viewpoints, factor, neighborScale);
+}
+pcl::IndicesPtr proportionalRadiusFiltering(
+		const pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr & cloud,
+		const std::vector<int> & viewpointIndices,
+		const std::map<int, Transform> & viewpoints,
+		float factor,
+		float neighborScale)
+{
+	pcl::IndicesPtr indices(new std::vector<int>);
+	return proportionalRadiusFiltering(cloud, indices, viewpointIndices, viewpoints, factor, neighborScale);
+}
+pcl::IndicesPtr proportionalRadiusFiltering(
+		const pcl::PointCloud<pcl::PointXYZI>::Ptr & cloud,
+		const std::vector<int> & viewpointIndices,
+		const std::map<int, Transform> & viewpoints,
+		float factor,
+		float neighborScale)
+{
+	pcl::IndicesPtr indices(new std::vector<int>);
+	return proportionalRadiusFiltering(cloud, indices, viewpointIndices, viewpoints, factor, neighborScale);
+}
+pcl::IndicesPtr proportionalRadiusFiltering(
+		const pcl::PointCloud<pcl::PointXYZINormal>::Ptr & cloud,
+		const std::vector<int> & viewpointIndices,
+		const std::map<int, Transform> & viewpoints,
+		float factor,
+		float neighborScale)
+{
+	pcl::IndicesPtr indices(new std::vector<int>);
+	return proportionalRadiusFiltering(cloud, indices, viewpointIndices, viewpoints, factor, neighborScale);
+}
+
+template<typename PointT>
+pcl::IndicesPtr proportionalRadiusFilteringImpl(
+		const typename pcl::PointCloud<PointT>::Ptr & cloud,
+		const pcl::IndicesPtr & indices,
+		const std::vector<int> & viewpointIndices,
+		const std::map<int, Transform> & viewpoints,
+		float factor,
+		float neighborScale)
+{
+	typename pcl::search::KdTree<PointT>::Ptr tree (new pcl::search::KdTree<PointT>(false));
+
+	UASSERT(cloud->size() == viewpointIndices.size());
+	UASSERT(factor>0.0f);
+	UASSERT(neighborScale>=1.0f);
+
+	if(!indices->empty())
+	{
+		std::vector<bool> kept(indices->size());
+		tree->setInputCloud(cloud, indices);
+		for(size_t i=0; i<indices->size(); ++i)
+		{
+			int index = indices->at(i);
+			std::vector<int> kIndices;
+			std::vector<float> kDistances;
+			std::map<int, Transform>::const_iterator viewpointIter = viewpoints.find(viewpointIndices[index]);
+			UASSERT(viewpointIter != viewpoints.end());
+			cv::Point3f viewpoint(viewpointIter->second.x(), viewpointIter->second.y(), viewpointIter->second.z());
+			cv::Point3f point = cv::Point3f(cloud->at(index).x,cloud->at(index).y, cloud->at(index).z);
+			float radiusSearch = factor * cv::norm(viewpoint-point);
+			int k = tree->radiusSearch(cloud->at(index), radiusSearch, kIndices, kDistances);
+			bool keep = k>0;
+			for(int j=0; j<k && keep; ++j)
+			{
+				if(kIndices[j] != index)
+				{
+					cv::Point3f pointTmp(cloud->at(kIndices[j]).x,cloud->at(kIndices[j]).y, cloud->at(kIndices[j]).z);
+					cv::Point3f tmp = pointTmp - point;
+					float distPtSqr = tmp.dot(tmp); // L2sqr
+					viewpointIter = viewpoints.find(viewpointIndices[kIndices[j]]);
+					UASSERT(viewpointIter != viewpoints.end());
+					viewpoint = cv::Point3f(viewpointIter->second.x(), viewpointIter->second.y(), viewpointIter->second.z());
+					float radiusSearchTmp = factor * cv::norm(viewpoint-pointTmp) * neighborScale;
+					if(distPtSqr > radiusSearchTmp*radiusSearchTmp)
+					{
+						keep = false;
+					}
+				}
+			}
+			kept[i] = keep;
+		}
+		pcl::IndicesPtr output(new std::vector<int>(indices->size()));
+		int oi = 0;
+		for(size_t i=0; i<indices->size(); ++i)
+		{
+			if(kept[i])
+			{
+				output->at(oi++) = indices->at(i);
+			}
+		}
+		output->resize(oi);
+		return output;
+	}
+	else
+	{
+		std::vector<bool> kept(cloud->size());
+		tree->setInputCloud(cloud);
+		#pragma omp parallel for
+		for(int i=0; i<(int)cloud->size(); ++i)
+		{
+			std::vector<int> kIndices;
+			std::vector<float> kDistances;
+			std::map<int, Transform>::const_iterator viewpointIter = viewpoints.find(viewpointIndices[i]);
+			UASSERT(viewpointIter != viewpoints.end());
+			cv::Point3f viewpoint(viewpointIter->second.x(), viewpointIter->second.y(), viewpointIter->second.z());
+			cv::Point3f point = cv::Point3f(cloud->at(i).x,cloud->at(i).y, cloud->at(i).z);
+			float radiusSearch = factor * cv::norm(viewpoint-point);
+			int k = tree->radiusSearch(cloud->at(i), radiusSearch, kIndices, kDistances);
+			bool keep = k>0;
+			for(int j=0; j<k && keep; ++j)
+			{
+				if(kIndices[j] != (int)i)
+				{
+					cv::Point3f pointTmp(cloud->at(kIndices[j]).x,cloud->at(kIndices[j]).y, cloud->at(kIndices[j]).z);
+					cv::Point3f tmp = pointTmp - point;
+					float distPtSqr = tmp.dot(tmp); // L2sqr
+					viewpointIter = viewpoints.find(viewpointIndices[kIndices[j]]);
+					UASSERT(viewpointIter != viewpoints.end());
+					viewpoint = cv::Point3f(viewpointIter->second.x(), viewpointIter->second.y(), viewpointIter->second.z());
+					float radiusSearchTmp = factor * cv::norm(viewpoint-pointTmp) * neighborScale;
+					if(distPtSqr > radiusSearchTmp*radiusSearchTmp)
+					{
+						keep = false;
+					}
+				}
+			}
+			kept[i] = keep;
+		}
+		pcl::IndicesPtr output(new std::vector<int>(cloud->size()));
+		int oi = 0;
+		for(size_t i=0; i<cloud->size(); ++i)
+		{
+			if(kept[i])
+			{
+				output->at(oi++) = i;
+			}
+		}
+		output->resize(oi);
+		return output;
+	}
+}
+
+pcl::IndicesPtr proportionalRadiusFiltering(
+		const pcl::PointCloud<pcl::PointXYZ>::Ptr & cloud,
+		const pcl::IndicesPtr & indices,
+		const std::vector<int> & viewpointIndices,
+		const std::map<int, Transform> & viewpoints,
+		float factor,
+		float neighborScale)
+{
+	return proportionalRadiusFilteringImpl<pcl::PointXYZ>(cloud, indices, viewpointIndices, viewpoints, factor, neighborScale);
+}
+pcl::IndicesPtr proportionalRadiusFiltering(
+		const pcl::PointCloud<pcl::PointNormal>::Ptr & cloud,
+		const pcl::IndicesPtr & indices,
+		const std::vector<int> & viewpointIndices,
+		const std::map<int, Transform> & viewpoints,
+		float factor,
+		float neighborScale)
+{
+	return proportionalRadiusFilteringImpl<pcl::PointNormal>(cloud, indices, viewpointIndices, viewpoints, factor, neighborScale);
+}
+pcl::IndicesPtr proportionalRadiusFiltering(
+		const pcl::PointCloud<pcl::PointXYZRGB>::Ptr & cloud,
+		const pcl::IndicesPtr & indices,
+		const std::vector<int> & viewpointIndices,
+		const std::map<int, Transform> & viewpoints,
+		float factor,
+		float neighborScale)
+{
+	return proportionalRadiusFilteringImpl<pcl::PointXYZRGB>(cloud, indices, viewpointIndices, viewpoints, factor, neighborScale);
+}
+pcl::IndicesPtr proportionalRadiusFiltering(
+		const pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr & cloud,
+		const pcl::IndicesPtr & indices,
+		const std::vector<int> & viewpointIndices,
+		const std::map<int, Transform> & viewpoints,
+		float factor,
+		float neighborScale)
+{
+	return proportionalRadiusFilteringImpl<pcl::PointXYZRGBNormal>(cloud, indices, viewpointIndices, viewpoints, factor, neighborScale);
+}
+pcl::IndicesPtr proportionalRadiusFiltering(
+		const pcl::PointCloud<pcl::PointXYZI>::Ptr & cloud,
+		const pcl::IndicesPtr & indices,
+		const std::vector<int> & viewpointIndices,
+		const std::map<int, Transform> & viewpoints,
+		float factor,
+		float neighborScale)
+{
+	return proportionalRadiusFilteringImpl<pcl::PointXYZI>(cloud, indices, viewpointIndices, viewpoints, factor, neighborScale);
+}
+pcl::IndicesPtr proportionalRadiusFiltering(
+		const pcl::PointCloud<pcl::PointXYZINormal>::Ptr & cloud,
+		const pcl::IndicesPtr & indices,
+		const std::vector<int> & viewpointIndices,
+		const std::map<int, Transform> & viewpoints,
+		float factor,
+		float neighborScale)
+{
+	return proportionalRadiusFilteringImpl<pcl::PointXYZINormal>(cloud, indices, viewpointIndices, viewpoints, factor, neighborScale);
+}
+
 pcl::PointCloud<pcl::PointXYZRGB>::Ptr subtractFiltering(
 		const pcl::PointCloud<pcl::PointXYZRGB>::Ptr & cloud,
 		const pcl::PointCloud<pcl::PointXYZRGB>::Ptr & substractCloud,
@@ -1680,20 +1930,22 @@ pcl::IndicesPtr normalFiltering(
 		float angleMax,
 		const Eigen::Vector4f & normal,
 		int normalKSearch,
-		const Eigen::Vector4f & viewpoint)
+		const Eigen::Vector4f & viewpoint,
+		float groundNormalsUp)
 {
 	pcl::IndicesPtr indices(new std::vector<int>);
-	return normalFiltering(cloud, indices, angleMax, normal, normalKSearch, viewpoint);
+	return normalFiltering(cloud, indices, angleMax, normal, normalKSearch, viewpoint, groundNormalsUp);
 }
 pcl::IndicesPtr normalFiltering(
 		const pcl::PointCloud<pcl::PointXYZRGB>::Ptr & cloud,
 		float angleMax,
 		const Eigen::Vector4f & normal,
 		int normalKSearch,
-		const Eigen::Vector4f & viewpoint)
+		const Eigen::Vector4f & viewpoint,
+		float groundNormalsUp)
 {
 	pcl::IndicesPtr indices(new std::vector<int>);
-	return normalFiltering(cloud, indices, angleMax, normal, normalKSearch, viewpoint);
+	return normalFiltering(cloud, indices, angleMax, normal, normalKSearch, viewpoint, groundNormalsUp);
 }
 
 
@@ -1704,7 +1956,8 @@ pcl::IndicesPtr normalFilteringImpl(
 		float angleMax,
 		const Eigen::Vector4f & normal,
 		int normalKSearch,
-		const Eigen::Vector4f & viewpoint)
+		const Eigen::Vector4f & viewpoint,
+		float groundNormalsUp)
 {
 	pcl::IndicesPtr output(new std::vector<int>());
 
@@ -1744,6 +1997,12 @@ pcl::IndicesPtr normalFilteringImpl(
 		for(unsigned int i=0; i<cloud_normals->size(); ++i)
 		{
 			Eigen::Vector4f v(cloud_normals->at(i).normal_x, cloud_normals->at(i).normal_y, cloud_normals->at(i).normal_z, 0.0f);
+			if(groundNormalsUp>0.0f && v[2] < -groundNormalsUp && cloud->at(indices->size()!=0?indices->at(i):i).z < viewpoint[3]) // some far velodyne rays on road can have normals toward ground
+			{
+				//reverse normal
+				v *= -1.0f;
+			}
+
 			float angle = pcl::getAngle3D(normal, v);
 			if(angle < angleMax)
 			{
@@ -1761,10 +2020,10 @@ pcl::IndicesPtr normalFiltering(
 		float angleMax,
 		const Eigen::Vector4f & normal,
 		int normalKSearch,
-		const Eigen::Vector4f & viewpoint)
+		const Eigen::Vector4f & viewpoint,
-
+		float groundNormalsUp)
 {
-	return normalFilteringImpl<pcl::PointXYZ>(cloud, indices, angleMax, normal, normalKSearch, viewpoint);
+	return normalFilteringImpl<pcl::PointXYZ>(cloud, indices, angleMax, normal, normalKSearch, viewpoint, groundNormalsUp);
 }
 pcl::IndicesPtr normalFiltering(
 		const pcl::PointCloud<pcl::PointXYZRGB>::Ptr & cloud,
@@ -1772,9 +2031,10 @@ pcl::IndicesPtr normalFiltering(
 		float angleMax,
 		const Eigen::Vector4f & normal,
 		int normalKSearch,
-		const Eigen::Vector4f & viewpoint)
+		const Eigen::Vector4f & viewpoint,
+		float groundNormalsUp)
 {
-	return normalFilteringImpl<pcl::PointXYZRGB>(cloud, indices, angleMax, normal, normalKSearch, viewpoint);
+	return normalFilteringImpl<pcl::PointXYZRGB>(cloud, indices, angleMax, normal, normalKSearch, viewpoint, groundNormalsUp);
 }
 pcl::IndicesPtr normalFiltering(
 		const pcl::PointCloud<pcl::PointXYZI>::Ptr & cloud,
@@ -1782,17 +2042,20 @@ pcl::IndicesPtr normalFiltering(
 		float angleMax,
 		const Eigen::Vector4f & normal,
 		int normalKSearch,
-		const Eigen::Vector4f & viewpoint)
+		const Eigen::Vector4f & viewpoint,
+		float groundNormalsUp)
 {
-	return normalFilteringImpl<pcl::PointXYZI>(cloud, indices, angleMax, normal, normalKSearch, viewpoint);
+	return normalFilteringImpl<pcl::PointXYZI>(cloud, indices, angleMax, normal, normalKSearch, viewpoint, groundNormalsUp);
 }
 
-template<typename PointT>
+template<typename PointNormalT>
 pcl::IndicesPtr normalFilteringImpl(
-		const typename pcl::PointCloud<PointT>::Ptr & cloud,
+		const typename pcl::PointCloud<PointNormalT>::Ptr & cloud,
 		const pcl::IndicesPtr & indices,
 		float angleMax,
-		const Eigen::Vector4f & normal)
+		const Eigen::Vector4f & normal,
+		const Eigen::Vector4f & viewpoint,
+		float groundNormalsUp)
 {
 	pcl::IndicesPtr output(new std::vector<int>());
 
@@ -1805,6 +2068,11 @@ pcl::IndicesPtr normalFilteringImpl(
 			for(unsigned int i=0; i<indices->size(); ++i)
 			{
 				Eigen::Vector4f v(cloud->at(indices->at(i)).normal_x, cloud->at(indices->at(i)).normal_y, cloud->at(indices->at(i)).normal_z, 0.0f);
+				if(groundNormalsUp>0.0f && v[2] < -groundNormalsUp && cloud->at(indices->at(i)).z < viewpoint[3]) // some far velodyne rays on road can have normals toward ground
+				{
+					//reverse normal
+					v *= -1.0f;
+				}
 				float angle = pcl::getAngle3D(normal, v);
 				if(angle < angleMax)
 				{
@@ -1818,6 +2086,11 @@ pcl::IndicesPtr normalFilteringImpl(
 			for(unsigned int i=0; i<cloud->size(); ++i)
 			{
 				Eigen::Vector4f v(cloud->at(i).normal_x, cloud->at(i).normal_y, cloud->at(i).normal_z, 0.0f);
+				if(groundNormalsUp>0.0f && v[2] < -groundNormalsUp && cloud->at(i).z < viewpoint[3]) // some far velodyne rays on road can have normals toward ground
+				{
+					//reverse normal
+					v *= -1.0f;
+				}
 				float angle = pcl::getAngle3D(normal, v);
 				if(angle < angleMax)
 				{
@@ -1836,30 +2109,33 @@ pcl::IndicesPtr normalFiltering(
 		const pcl::IndicesPtr & indices,
 		float angleMax,
 		const Eigen::Vector4f & normal,
-		int normalKSearch,
+		int,
-		const Eigen::Vector4f & viewpoint)
+		const Eigen::Vector4f & viewpoint,
+		float groundNormalsUp)
 {
-	return normalFilteringImpl<pcl::PointNormal>(cloud, indices, angleMax, normal);
+	return normalFilteringImpl<pcl::PointNormal>(cloud, indices, angleMax, normal, viewpoint, groundNormalsUp);
 }
 pcl::IndicesPtr normalFiltering(
 		const pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr & cloud,
 		const pcl::IndicesPtr & indices,
 		float angleMax,
 		const Eigen::Vector4f & normal,
-		int normalKSearch,
+		int,
-		const Eigen::Vector4f & viewpoint)
+		const Eigen::Vector4f & viewpoint,
+		float groundNormalsUp)
 {
-	return normalFilteringImpl<pcl::PointXYZRGBNormal>(cloud, indices, angleMax, normal);
+	return normalFilteringImpl<pcl::PointXYZRGBNormal>(cloud, indices, angleMax, normal, viewpoint, groundNormalsUp);
 }
 pcl::IndicesPtr normalFiltering(
 		const pcl::PointCloud<pcl::PointXYZINormal>::Ptr & cloud,
 		const pcl::IndicesPtr & indices,
 		float angleMax,
 		const Eigen::Vector4f & normal,
-		int normalKSearch,
+		int,
-		const Eigen::Vector4f & viewpoint)
+		const Eigen::Vector4f & viewpoint,
+		float groundNormalsUp)
 {
-	return normalFilteringImpl<pcl::PointXYZINormal>(cloud, indices, angleMax, normal);
+	return normalFilteringImpl<pcl::PointXYZINormal>(cloud, indices, angleMax, normal, viewpoint, groundNormalsUp);
 }
 
 std::vector<pcl::IndicesPtr> extractClusters(

corelib/src/util3d_mapping.cpp

@@ -122,7 +122,7 @@ void occupancy2DFromLaserScan(
 	}
 	else
 	{
-		scanNoHit = scanHit;
+		scanNoHit = scanNoHitIn;
 	}
 
 	std::map<int, Transform> poses;

corelib/src/util3d_surface.cpp

@@ -3493,7 +3493,8 @@ void adjustNormalsToViewPointImpl(
 		const Eigen::Vector3f & viewpoint,
 		float groundNormalsUp)
 {
-	for(unsigned int i=0; i<cloud->size(); ++i)
+	#pragma omp parallel for
+	for(int i=0; i<(int)cloud->size(); ++i)
 	{
 		pcl::PointXYZ normal(cloud->points[i].normal_x, cloud->points[i].normal_y, cloud->points[i].normal_z);
 		if(pcl::isFinite(normal))
@@ -3559,18 +3560,22 @@ void adjustNormalsToViewPoint(
 	adjustNormalsToViewPointImpl<pcl::PointXYZINormal>(cloud, viewpoint, groundNormalsUp);
 }
 
-void adjustNormalsToViewPoints(
+
+template<typename PointT>
+void adjustNormalsToViewPointsImpl(
 		const std::map<int, Transform> & poses,
 		const pcl::PointCloud<pcl::PointXYZ>::Ptr & rawCloud,
 		const std::vector<int> & rawCameraIndices,
-		pcl::PointCloud<pcl::PointNormal>::Ptr & cloud)
+		typename pcl::PointCloud<PointT>::Ptr & cloud,
+		float groundNormalsUp)
 {
 	if(poses.size() && rawCloud->size() && rawCloud->size() == rawCameraIndices.size() && cloud->size())
 	{
 		pcl::search::KdTree<pcl::PointXYZ>::Ptr rawTree (new pcl::search::KdTree<pcl::PointXYZ>);
 		rawTree->setInputCloud (rawCloud);
 
-		for(unsigned int i=0; i<cloud->size(); ++i)
+		#pragma omp parallel for
+		for(int i=0; i<(int)cloud->size(); ++i)
 		{
 			pcl::PointXYZ normal(cloud->points[i].normal_x, cloud->points[i].normal_y, cloud->points[i].normal_z);
 			if(pcl::isFinite(normal))
@@ -3581,14 +3586,15 @@ void adjustNormalsToViewPoints(
 				UASSERT(indices.size() == 1);
 				if(indices.size() && indices[0]>=0)
 				{
-					Transform p = poses.at(rawCameraIndices[indices[0]]);
+					const Transform & p = poses.at(rawCameraIndices[indices[0]]);
 					pcl::PointXYZ viewpoint(p.x(), p.y(), p.z());
 					Eigen::Vector3f v = viewpoint.getVector3fMap() - cloud->points[i].getVector3fMap();
 
 					Eigen::Vector3f n(normal.x, normal.y, normal.z);
 
 					float result = v.dot(n);
-					if(result < 0)
+					if(result < 0 ||
+					   (groundNormalsUp>0.0f && normal.z < -groundNormalsUp && cloud->points[i].z < viewpoint.z)) // some far velodyne rays on road can have normals toward ground)
 					{
 						//reverse normal
 						cloud->points[i].normal_x *= -1.0f;
@@ -3609,55 +3615,38 @@ void adjustNormalsToViewPoints(
 		const std::map<int, Transform> & poses,
 		const pcl::PointCloud<pcl::PointXYZ>::Ptr & rawCloud,
 		const std::vector<int> & rawCameraIndices,
-		pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr & cloud)
+		pcl::PointCloud<pcl::PointNormal>::Ptr & cloud,
+		float groundNormalsUp)
 {
-	UASSERT(rawCloud.get() && cloud.get());
+	adjustNormalsToViewPointsImpl<pcl::PointNormal>(poses, rawCloud, rawCameraIndices, cloud, groundNormalsUp);
-	UDEBUG("poses=%d, rawCloud=%d, rawCameraIndices=%d, cloud=%d", (int)poses.size(), (int)rawCloud->size(), (int)rawCameraIndices.size(), (int)cloud->size());
+}
-	if(poses.size() && rawCloud->size() && rawCloud->size() == rawCameraIndices.size() && cloud->size())
-	{
-		pcl::search::KdTree<pcl::PointXYZ>::Ptr rawTree (new pcl::search::KdTree<pcl::PointXYZ>);
-		rawTree->setInputCloud (rawCloud);
-		for(unsigned int i=0; i<cloud->size(); ++i)
-		{
-			pcl::PointXYZ normal(cloud->points[i].normal_x, cloud->points[i].normal_y, cloud->points[i].normal_z);
-			if(pcl::isFinite(normal))
-			{
-				std::vector<int> indices;
-				std::vector<float> dist;
-				rawTree->nearestKSearch(pcl::PointXYZ(cloud->points[i].x, cloud->points[i].y, cloud->points[i].z), 1, indices, dist);
-				if(indices.size() && indices[0]>=0)
-				{
-					UASSERT_MSG(indices[0]<(int)rawCameraIndices.size(), uFormat("indices[0]=%d rawCameraIndices.size()=%d", indices[0], (int)rawCameraIndices.size()).c_str());
-					UASSERT(uContains(poses, rawCameraIndices[indices[0]]));
-					Transform p = poses.at(rawCameraIndices[indices[0]]);
-					pcl::PointXYZ viewpoint(p.x(), p.y(), p.z());
-					Eigen::Vector3f v = viewpoint.getVector3fMap() - cloud->points[i].getVector3fMap();
 
-					Eigen::Vector3f n(normal.x, normal.y, normal.z);
+void adjustNormalsToViewPoints(
+		const std::map<int, Transform> & poses,
+		const pcl::PointCloud<pcl::PointXYZ>::Ptr & rawCloud,
+		const std::vector<int> & rawCameraIndices,
+		pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr & cloud,
+		float groundNormalsUp)
+{
+	adjustNormalsToViewPointsImpl<pcl::PointXYZRGBNormal>(poses, rawCloud, rawCameraIndices, cloud, groundNormalsUp);
+}
 
-					float result = v.dot(n);
+void adjustNormalsToViewPoints(
-					if(result < 0)
+		const std::map<int, Transform> & poses,
-					{
+		const pcl::PointCloud<pcl::PointXYZ>::Ptr & rawCloud,
-						//reverse normal
+		const std::vector<int> & rawCameraIndices,
-						cloud->points[i].normal_x *= -1.0f;
+		pcl::PointCloud<pcl::PointXYZINormal>::Ptr & cloud,
-						cloud->points[i].normal_y *= -1.0f;
+		float groundNormalsUp)
-						cloud->points[i].normal_z *= -1.0f;
+{
-					}
+	adjustNormalsToViewPointsImpl<pcl::PointXYZINormal>(poses, rawCloud, rawCameraIndices, cloud, groundNormalsUp);
-				}
-				else
-				{
-					UWARN("Not found camera viewpoint for point %d!?", i);
-				}
-			}
-		}
-	}
 }
 
 void adjustNormalsToViewPoints(
 		const std::map<int, Transform> & viewpoints,
 		const LaserScan & rawScan,
 		const std::vector<int> & viewpointIds,
-		LaserScan & scan)
+		LaserScan & scan,
+		float groundNormalsUp)
 {
 	UDEBUG("poses=%d, rawCloud=%d, rawCameraIndices=%d, cloud=%d", (int)viewpoints.size(), (int)rawScan.size(), (int)viewpointIds.size(), (int)scan.size());
 	if(viewpoints.size() && rawScan.size() && rawScan.size() == (int)viewpointIds.size() && scan.size() && scan.hasNormals())
@@ -3665,6 +3654,7 @@ void adjustNormalsToViewPoints(
 		pcl::PointCloud<pcl::PointXYZ>::Ptr rawCloud = util3d::laserScanToPointCloud(rawScan);
 		pcl::search::KdTree<pcl::PointXYZ>::Ptr rawTree (new pcl::search::KdTree<pcl::PointXYZ>);
 		rawTree->setInputCloud (rawCloud);
+		#pragma omp parallel for
 		for(int i=0; i<scan.size(); ++i)
 		{
 			pcl::PointNormal point = util3d::laserScanToPointNormal(scan, i);
@@ -3685,7 +3675,8 @@ void adjustNormalsToViewPoints(
 					Eigen::Vector3f n(normal.x, normal.y, normal.z);
 
 					float result = v.dot(n);
-					if(result < 0)
+					if(result < 0 ||
+					   (groundNormalsUp>0.0f && normal.z < -groundNormalsUp && point.z < viewpoint.z)) // some far velodyne rays on road can have normals toward ground))
 					{
 						//reverse normal
 						scan.field(i, scan.getNormalsOffset()) *= -1.0f;

docker/bionic/android/Dockerfile

@@ -9,29 +9,32 @@ RUN apt-get update && apt-get install -y \
       g++ lib32stdc++6 lib32z1 \
       software-properties-common \
       freeglut3-dev \
-      openjdk-8-jdk openjdk-8-jre
+      openjdk-8-jdk openjdk-8-jre \
+      curl
 
-ENV ANDROID_NDK_VERSION=r21
 ENV ANDROID_HOME=/opt/android-sdk
-ENV PATH=$PATH:/opt/android-sdk/tools:/opt/android-sdk/platform-tools:/opt/android-ndk-$ANDROID_NDK_VERSION
+ENV PATH=$PATH:/opt/android-sdk/cmdline-tools/latest/bin:/opt/android-sdk/tools:/opt/android-sdk/platform-tools:/opt/android-sdk/ndk/21.4.7075529
-ENV ANDROID_NDK=/opt/android-ndk-$ANDROID_NDK_VERSION
+ENV ANDROID_NDK=/opt/android-sdk/ndk/21.4.7075529
 ENV JAVA_HOME=/usr/lib/jvm/java-8-openjdk-amd64
 
 WORKDIR /root/
 
 # Setup android sdk
-RUN wget -nv https://dl.google.com/android/repository/tools_r25.2.3-linux.zip && \
+RUN wget -nv https://dl.google.com/android/repository/commandlinetools-linux-7583922_latest.zip && \
-    unzip -qq tools_r25.2.3-linux.zip && \
+    unzip -qq commandlinetools-linux-7583922_latest.zip && \
-    rm tools_r25.2.3-linux.zip && \
+    rm commandlinetools-linux-7583922_latest.zip && \
     mkdir $ANDROID_HOME && \
-    mv tools $ANDROID_HOME/.
+    mkdir $ANDROID_HOME/cmdline-tools && \
-RUN echo y | android update sdk --no-ui --all --filter platform-tools,android-23,android-24,android-26,build-tools-29.0.3
+    mv cmdline-tools $ANDROID_HOME/cmdline-tools/latest
+# We should use build-tools <=30 to avoid dx missing error
+RUN echo y | sdkmanager --install "platform-tools" "platforms;android-23" "platforms;android-24" "platforms;android-26" "platforms;android-30" "build-tools;30.0.3" "ndk;21.4.7075529" && \
+    rm -r $ANDROID_HOME/tools
 
-# Setup android ndk
+# we need <=r25 tools to use "android" command (now deprecated)
-RUN wget -nv https://dl.google.com/android/repository/android-ndk-$ANDROID_NDK_VERSION-linux-x86_64.zip && \
+RUN wget -nv http://dl-ssl.google.com/android/repository/tools_r25.2.5-linux.zip && \
-    unzip -qq android-ndk-$ANDROID_NDK_VERSION-linux-x86_64.zip && \
+    unzip -qq tools_r25.2.5-linux.zip && \
-    rm android-ndk-$ANDROID_NDK_VERSION-linux-x86_64.zip && \
+    mv tools $ANDROID_HOME/. && \
-    mv android-ndk-$ANDROID_NDK_VERSION /opt/.
+    rm tools_r25.2.5-linux.zip
 
 ADD deps.bash /root/deps.bash
 RUN chmod +x deps.bash

docker/bionic/android/deps.bash

@@ -5,7 +5,7 @@ if [ $# -ne 1 ]; then
     exit 1
 fi
 
-cpus=-j4
+cpus=-j12
 prefix=$1
 pwd=$(pwd)
 ANDROID_NATIVE_API_LEVEL=23
@@ -16,7 +16,7 @@ wget -nv https://github.com/Kitware/CMake/releases/download/v3.17.0/cmake-3.17.0
 echo "Downloading boost... (2/10)"
 wget -nv https://downloads.sourceforge.net/project/boost/boost/1.59.0/boost_1_59_0.tar.gz
 echo "Downloading eigen... (3/10)"
-wget -nv http://bitbucket.org/eigen/eigen/get/3.2.7.tar.gz
+curl -L https://gitlab.com/libeigen/eigen/-/archive/3.3.9/eigen-3.3.9.tar.gz -o 3.3.9.tar.gz
 echo "Downloading flann... (4/10)"
 git clone -b 1.8.4 https://github.com/mariusmuja/flann.git
 echo "Downloading gtsam... (5/10)"
@@ -57,15 +57,15 @@ rm -r boost_1_59_0.tar.gz boost_1_59_0
 
 # eigen
 echo "Install eigen..."
-tar -xzf 3.2.7.tar.gz
+tar -xzf 3.3.9.tar.gz
-cd eigen-eigen-b30b87236a1b
+cd eigen-3.3.9
 mkdir build
 cd build
 $pwd/cmake-3.17.0-Linux-x86_64/bin/cmake -DCMAKE_TOOLCHAIN_FILE=$ANDROID_NDK/build/cmake/android.toolchain.cmake -DANDROID_ABI=arm64-v8a -DANDROID_NDK=$ANDROID_NDK -DANDROID_NATIVE_API_LEVEL=$ANDROID_NATIVE_API_LEVEL -DBUILD_SHARED_LIBS=OFF -DCMAKE_BUILD_TYPE=Release -DCMAKE_INSTALL_PREFIX=$prefix/arm64-v8a -DCMAKE_FIND_ROOT_PATH="$prefix/arm64-v8a/bin;$prefix/arm64-v8a;$prefix/arm64-v8a/share" ..
 make $cpus
 make install
 cd $pwd
-rm -r 3.2.7.tar.gz eigen-eigen-b30b87236a1b
+rm -r 3.3.9.tar.gz eigen-3.3.9
 
 # FLANN
 echo "Install flann..."
@@ -74,7 +74,7 @@ wget -nv https://gist.githubusercontent.com/matlabbe/cacff9f8271d0c42acd622939a2
 git apply flann_1_8_4_android_fix.patch
 mkdir build
 cd build
-$pwd/cmake-3.17.0-Linux-x86_64/bin/cmake -DCMAKE_TOOLCHAIN_FILE=$ANDROID_NDK/build/cmake/android.toolchain.cmake -DANDROID_ABI=arm64-v8a -DANDROID_NDK=$ANDROID_NDK -DANDROID_NATIVE_API_LEVEL=$ANDROID_NATIVE_API_LEVEL -DBUILD_SHARED_LIBS=OFF -DCMAKE_BUILD_TYPE=Release -DCMAKE_INSTALL_PREFIX=$prefix/arm64-v8a -DCMAKE_FIND_ROOT_PATH="$prefix/arm64-v8a/bin;$prefix/arm64-v8a;$prefix/arm64-v8a/share" ..
+$pwd/cmake-3.17.0-Linux-x86_64/bin/cmake -DCMAKE_TOOLCHAIN_FILE=$ANDROID_NDK/build/cmake/android.toolchain.cmake -DANDROID_ABI=arm64-v8a -DANDROID_NDK=$ANDROID_NDK -DANDROID_NATIVE_API_LEVEL=$ANDROID_NATIVE_API_LEVEL -DBUILD_SHARED_LIBS=OFF -DBUILD_PYTHON_BINDINGS=OFF -DCMAKE_BUILD_TYPE=Release -DCMAKE_INSTALL_PREFIX=$prefix/arm64-v8a -DCMAKE_FIND_ROOT_PATH="$prefix/arm64-v8a/bin;$prefix/arm64-v8a;$prefix/arm64-v8a/share" ..
 make $cpus
 make install
 cd $pwd
@@ -101,7 +101,7 @@ cd g2o
 git checkout a3f7706bdbb849b2808dc3e1b7aee189f63b498e
 mkdir build
 cd build
-$pwd/cmake-3.17.0-Linux-x86_64/bin/cmake -DCMAKE_TOOLCHAIN_FILE=$ANDROID_NDK/build/cmake/android.toolchain.cmake -DANDROID_ABI=arm64-v8a -DANDROID_NDK=$ANDROID_NDK -DANDROID_NATIVE_API_LEVEL=$ANDROID_NATIVE_API_LEVEL -DBUILD_SHARED_LIBS=OFF -DCMAKE_BUILD_TYPE=Release -DCMAKE_INSTALL_PREFIX=$prefix/arm64-v8a -DCMAKE_FIND_ROOT_PATH="$prefix/arm64-v8a/bin;$prefix/arm64-v8a;$prefix/arm64-v8a/share" -DBUILD_LGPL_SHARED_LIBS=OFF -DG2O_BUILD_APPS=OFF -DG2O_BUILD_EXAMPLES=OFF ..
+$pwd/cmake-3.17.0-Linux-x86_64/bin/cmake -DCMAKE_TOOLCHAIN_FILE=$ANDROID_NDK/build/cmake/android.toolchain.cmake -DANDROID_ABI=arm64-v8a -DANDROID_NDK=$ANDROID_NDK -DANDROID_NATIVE_API_LEVEL=$ANDROID_NATIVE_API_LEVEL -DBUILD_SHARED_LIBS=OFF -DCMAKE_BUILD_TYPE=Release -DCMAKE_INSTALL_PREFIX=$prefix/arm64-v8a -DCMAKE_FIND_ROOT_PATH="$prefix/arm64-v8a/bin;$prefix/arm64-v8a;$prefix/arm64-v8a/share" -DBUILD_LGPL_SHARED_LIBS=OFF -DG2O_BUILD_APPS=OFF -DG2O_BUILD_EXAMPLES=OFF -DG2O_USE_OPENGL=OFF ..
 make $cpus
 make install
 cd $pwd
@@ -110,12 +110,12 @@ rm -rf g2o
 # VTK
 echo "Install VTK..."
 cd VTK
-git checkout tags/v7.1.1
+git checkout tags/v8.2.0
-wget https://gist.github.com/matlabbe/f133f555d650e3ab5c51d3b772f02448/raw/be135b74cabb1cc85f90b8605f9db218d1e3f71b/vtk_7_1_1_android_r21_fix.patch
+wget https://gist.github.com/matlabbe/e217259fb8ece9ee6daf5a8f70e896a0/raw/2214b503a537d6431d764526b5b780f07d6f168d/vtk_8_2_0_android_r21_fix.patch
-git apply vtk_7_1_1_android_r21_fix.patch
+git apply vtk_8_2_0_android_r21_fix.patch
 mkdir build
 cd build
-$pwd/cmake-3.17.0-Linux-x86_64/bin/cmake -DBUILD_EXAMPLES=OFF -DBUILD_TESTING=OFF -DVTK_ANDROID_BUILD=ON -DBUILD_SHARED_LIBS=OFF -DCMAKE_BUILD_TYPE=Release -DCMAKE_INSTALL_PREFIX=$prefix/arm64-v8a -DCMAKE_FIND_ROOT_PATH="$prefix/arm64-v8a/bin;$prefix/arm64-v8a;$prefix/arm64-v8a/share" ..
+$pwd/cmake-3.17.0-Linux-x86_64/bin/cmake -DBUILD_EXAMPLES=OFF -DBUILD_TESTING=OFF -DVTK_ANDROID_BUILD=ON -DBUILD_SHARED_LIBS=OFF -DCMAKE_BUILD_TYPE=Release -DCMAKE_INSTALL_PREFIX=$prefix/arm64-v8a -DANDROID_ARCH_ABI=arm64-v8a -DCMAKE_FIND_ROOT_PATH="$prefix/arm64-v8a/bin;$prefix/arm64-v8a;$prefix/arm64-v8a/share" ..
 make $cpus
 cp -r CMakeExternals/Install/vtk-android/* $prefix/arm64-v8a/.
 cd $pwd
@@ -126,13 +126,13 @@ echo "Install pcl..."
 cd pcl
 git checkout tags/pcl-1.8.0
 # patch
-wget https://gist.github.com/matlabbe/41812e50e459b2f27b331a2343569e5d/raw/4cc7dd13b36e2f8c623e8faea7cc731b2899b0e4/pcl_1_8_0_vtk_android_support.patch
+wget https://gist.github.com/matlabbe/41812e50e459b2f27b331a2343569e5d/raw/b2fc0c4d1cfffb3a9f2811abae782e317c539bfb/pcl_1_8_0_vtk_android_support.patch
 git apply pcl_1_8_0_vtk_android_support.patch
 mkdir build
 cd build
 # do it 2 times because there is a cmake error on the first time and not the second time!?
-$pwd/cmake-3.17.0-Linux-x86_64/bin/cmake -DCMAKE_TOOLCHAIN_FILE=$ANDROID_NDK/build/cmake/android.toolchain.cmake -DANDROID_ABI=arm64-v8a -DANDROID_NDK=$ANDROID_NDK -DANDROID_NATIVE_API_LEVEL=$ANDROID_NATIVE_API_LEVEL -DBUILD_SHARED_LIBS=OFF -DCMAKE_BUILD_TYPE=Release -DCMAKE_INSTALL_PREFIX=$prefix/arm64-v8a -DCMAKE_FIND_ROOT_PATH="$prefix/arm64-v8a/bin;$prefix/arm64-v8a;$prefix/arm64-v8a/share" -DBUILD_apps=OFF -DBUILD_examples=OFF -DBUILD_tools=OFF -DBUILD_visualization=OFF -DBUILD_tracking=OFF -DBUILD_people=OFF -DBUILD_global_tests=OFF -DWITH_QT=OFF -DWITH_OPENGL=OFF -DWITH_VTK=ON -DPCL_SHARED_LIBS=OFF ..
+$pwd/cmake-3.17.0-Linux-x86_64/bin/cmake -DCMAKE_TOOLCHAIN_FILE=$ANDROID_NDK/build/cmake/android.toolchain.cmake -DANDROID_ABI=arm64-v8a -DANDROID_NDK=$ANDROID_NDK -DANDROID_NATIVE_API_LEVEL=$ANDROID_NATIVE_API_LEVEL -DBUILD_SHARED_LIBS=OFF -DCMAKE_BUILD_TYPE=Release -DCMAKE_INSTALL_PREFIX=$prefix/arm64-v8a -DCMAKE_FIND_ROOT_PATH="$prefix/arm64-v8a/bin;$prefix/arm64-v8a;$prefix/arm64-v8a/share" -DBUILD_apps=OFF -DBUILD_examples=OFF -DBUILD_tools=OFF -DBUILD_visualization=OFF -DBUILD_tracking=OFF -DBUILD_people=OFF -DBUILD_tools=OFF -DBUILD_global_tests=OFF -DWITH_QT=OFF -DWITH_OPENGL=OFF -DWITH_VTK=ON -DPCL_SHARED_LIBS=OFF ..
-$pwd/cmake-3.17.0-Linux-x86_64/bin/cmake -DCMAKE_TOOLCHAIN_FILE=$ANDROID_NDK/build/cmake/android.toolchain.cmake -DANDROID_ABI=arm64-v8a -DANDROID_NDK=$ANDROID_NDK -DANDROID_NATIVE_API_LEVEL=$ANDROID_NATIVE_API_LEVEL -DBUILD_SHARED_LIBS=OFF -DCMAKE_BUILD_TYPE=Release -DCMAKE_INSTALL_PREFIX=$prefix/arm64-v8a -DCMAKE_FIND_ROOT_PATH="$prefix/arm64-v8a/bin;$prefix/arm64-v8a;$prefix/arm64-v8a/share" -DBUILD_apps=OFF -DBUILD_examples=OFF -DBUILD_tools=OFF -DBUILD_visualization=OFF -DBUILD_tracking=OFF -DBUILD_people=OFF -DBUILD_global_tests=OFF -DWITH_QT=OFF -DWITH_OPENGL=OFF -DWITH_VTK=ON -DPCL_SHARED_LIBS=OFF ..
+$pwd/cmake-3.17.0-Linux-x86_64/bin/cmake -DCMAKE_TOOLCHAIN_FILE=$ANDROID_NDK/build/cmake/android.toolchain.cmake -DANDROID_ABI=arm64-v8a -DANDROID_NDK=$ANDROID_NDK -DANDROID_NATIVE_API_LEVEL=$ANDROID_NATIVE_API_LEVEL -DBUILD_SHARED_LIBS=OFF -DCMAKE_BUILD_TYPE=Release -DCMAKE_INSTALL_PREFIX=$prefix/arm64-v8a -DCMAKE_FIND_ROOT_PATH="$prefix/arm64-v8a/bin;$prefix/arm64-v8a;$prefix/arm64-v8a/share" -DBUILD_apps=OFF -DBUILD_examples=OFF -DBUILD_tools=OFF -DBUILD_visualization=OFF -DBUILD_tracking=OFF -DBUILD_people=OFF -DBUILD_tools=OFF -DBUILD_global_tests=OFF -DWITH_QT=OFF -DWITH_OPENGL=OFF -DWITH_VTK=ON -DPCL_SHARED_LIBS=OFF ..
 make $cpus
 make install
 cd $pwd
@@ -140,23 +140,23 @@ rm -rf pcl
 
 # make sure opencv is using the shared version of zlib
 # see https://github.com/android/ndk/issues/1179
-mv /opt/android-ndk-r21/toolchains/llvm/prebuilt/linux-x86_64/sysroot/usr/lib/aarch64-linux-android/libz.a /opt/android-ndk-r21/toolchains/llvm/prebuilt/linux-x86_64/sysroot/usr/lib/aarch64-linux-android/libz.a.back
+mv $ANDROID_NDK/toolchains/llvm/prebuilt/linux-x86_64/sysroot/usr/lib/aarch64-linux-android/libz.a $ANDROID_NDK/toolchains/llvm/prebuilt/linux-x86_64/sysroot/usr/lib/aarch64-linux-android/libz.a.back
 
 # OpenCV
 echo "Install OpenCV..."
 cd opencv_contrib
-git checkout tags/3.4.2
+git checkout tags/4.5.5
 cd $pwd
 cd opencv
-git checkout tags/3.4.2
+git checkout tags/4.5.5
 mkdir build
 cd build
-$pwd/cmake-3.17.0-Linux-x86_64/bin/cmake -DCMAKE_TOOLCHAIN_FILE=$ANDROID_NDK/build/cmake/android.toolchain.cmake -DANDROID_ABI=arm64-v8a -DANDROID_NDK=$ANDROID_NDK -DANDROID_NATIVE_API_LEVEL=$ANDROID_NATIVE_API_LEVEL -DBUILD_SHARED_LIBS=OFF -DCMAKE_BUILD_TYPE=Release -DCMAKE_INSTALL_PREFIX=$prefix/arm64-v8a -DCMAKE_FIND_ROOT_PATH="$prefix/arm64-v8a/bin;$prefix/arm64-v8a;$prefix/arm64-v8a/share" -DOPENCV_EXTRA_MODULES_PATH=$pwd/opencv_contrib/modules -DBUILD_TESTS=OFF -DBUILD_PERF_TESTS=OFF -DWITH_CUDA=OFF -DBUILD_opencv_structured_light=OFF -DBUILD_ANDROID_PROJECTS=ON -DBUILD_ANDROID_EXAMPLES=OFF -DWITH_PROTOBUF=OFF -DBUILD_opencv_stereo=OFF ..
+$pwd/cmake-3.17.0-Linux-x86_64/bin/cmake -DCMAKE_TOOLCHAIN_FILE=$ANDROID_NDK/build/cmake/android.toolchain.cmake -DANDROID_ABI=arm64-v8a -DANDROID_NDK=$ANDROID_NDK -DANDROID_NATIVE_API_LEVEL=$ANDROID_NATIVE_API_LEVEL -DBUILD_SHARED_LIBS=OFF -DCMAKE_BUILD_TYPE=Release -DCMAKE_INSTALL_PREFIX=$prefix/arm64-v8a -DCMAKE_FIND_ROOT_PATH="$prefix/arm64-v8a/bin;$prefix/arm64-v8a;$prefix/arm64-v8a/share" -DOPENCV_EXTRA_MODULES_PATH=$pwd/opencv_contrib/modules -DBUILD_TESTS=OFF -DBUILD_PERF_TESTS=OFF -DWITH_CUDA=OFF -DBUILD_opencv_structured_light=OFF -DBUILD_ANDROID_PROJECTS=OFF -DOPENCV_ENABLE_NONFREE=ON -DBUILD_ANDROID_EXAMPLES=OFF -DWITH_PROTOBUF=OFF -DBUILD_opencv_stereo=OFF -DBUILD_JAVA=OFF -DWITH_QUIRC=OFF -DBUILD_opencv_js_bindings_generator=OFF -DBUILD_opencv_objc_bindings_generator=OFF -DBUILD_opencv_objdetect=OFF -DBUILD_opencv_xobjdetect=OFF ..
 make $cpus
 make install
 cd $pwd
 rm -rf opencv opencv_contrib
 
 echo "Strip libraries..."
-/opt/android-ndk-$ANDROID_NDK_VERSION/toolchains/llvm/prebuilt/linux-x86_64/bin/aarch64-linux-android-strip -g -S -d --strip-debug --verbose /opt/android/arm64-v8a/lib/*.a
+$ANDROID_NDK/toolchains/llvm/prebuilt/linux-x86_64/bin/aarch64-linux-android-strip -g -S -d --strip-debug --verbose /opt/android/arm64-v8a/lib/*.a
-/opt/android-ndk-$ANDROID_NDK_VERSION/toolchains/llvm/prebuilt/linux-x86_64/bin/aarch64-linux-android-strip -g -S -d --strip-debug --verbose /opt/android/arm64-v8a/sdk/native/staticlibs/arm64-v8a/*.a
+$ANDROID_NDK/toolchains/llvm/prebuilt/linux-x86_64/bin/aarch64-linux-android-strip -g -S -d --strip-debug --verbose /opt/android/arm64-v8a/sdk/native/staticlibs/arm64-v8a/*.a

docker/bionic/android/rtabmap_api30/Dockerfile

@@ -0,0 +1,11 @@
+# Image: introlab3it/rtabmap:android30
+
+FROM introlab3it/rtabmap:android-deps
+
+WORKDIR /root/
+
+ARG CACHE_DATE=2016-01-01
+ADD rtabmap.bash /root/rtabmap.bash
+RUN chmod +x rtabmap.bash
+RUN /bin/bash -c "./rtabmap.bash /opt/android 30"
+

docker/bionic/android/rtabmap_api30/rtabmap.bash

@@ -0,0 +1,65 @@
+#!/bin/bash
+
+if [ $# -ne 2 ]; then
+    echo "rtabmap.bash android_install_prefix api_level (23 for tango, 24 for arengine)   # Example: build.bash /opt/android 24"
+    exit 1
+fi
+
+prefix=$1
+api=$2
+pwd=$(pwd)
+
+
+# get rtabmap
+git clone https://github.com/introlab/rtabmap.git rtabmap-tango
+cd 
+
+# tango
+wget 'https://docs.google.com/uc?authuser=0&id=12rHHkYM5k-UnQn-xGXs9JqYWhSXrgJr3&export=download' -O TangoSDK_Ikariotikos_C.zip
+unzip -qq TangoSDK_Ikariotikos_C.zip
+rm TangoSDK_Ikariotikos_C.zip
+cp -r lib_tango_client_api/include/* $prefix/arm64-v8a/include/.
+cp -r lib_tango_client_api/lib/arm64-v8a/* $prefix/arm64-v8a/lib/.
+rm -r lib_tango_client_api
+wget 'https://docs.google.com/uc?authuser=0&id=1AqVuEVu5284X6OgrGWu12VTrx4pY99Jb&export=download' -O TangoSupport_Ikariotikos_C.zip
+unzip -qq TangoSupport_Ikariotikos_C.zip
+rm TangoSupport_Ikariotikos_C.zip
+cp -r lib_tango_support_api/include/* $prefix/arm64-v8a/include/.
+cp -r lib_tango_support_api/lib/arm64-v8a/* $prefix/arm64-v8a/lib/.
+rm -r lib_tango_support_api
+wget 'https://docs.google.com/uc?authuser=0&id=1s5iPJ7xiridj9Jj--gCy2XiQFniheVm6&export=download' -O TangoSDK_Ikariotikos_Java.jar
+mv TangoSDK_Ikariotikos_Java.jar rtabmap-tango/app/android/libs/.
+
+# ARCore
+wget 'https://docs.google.com/uc?authuser=0&id=1VsibeqRYpS5pjmrG-vYTXyiPg8kbIfVN&export=download' -O arcore.zip
+unzip -qq arcore.zip
+rm arcore.zip
+cp -r arcore1_18/include/* $prefix/arm64-v8a/include/.
+cp -r arcore1_18/arm64-v8a/* $prefix/arm64-v8a/lib/.
+cp arcore1_18/*.jar rtabmap-tango/app/android/libs/.
+rm -r arcore1_18
+
+# AREngine
+wget 'https://docs.google.com/uc?authuser=0&id=1rdaD2Z1QBv-SUeUy0oBmg3C2odfxTHgR&export=download' -O arengine.zip
+unzip -qq arengine.zip
+rm arengine.zip
+cp -r arengine/include/* $prefix/arm64-v8a/include/.
+cp -r arengine/arm64-v8a/* $prefix/arm64-v8a/lib/.
+cp arengine/*.jar rtabmap-tango/app/android/libs/.
+rm -r arengine
+
+# resource tool
+cd rtabmap-tango/build
+cmake -DANDROID_PREBUILD=ON ..
+make
+cd ../..
+
+# rtabmap
+mkdir rtabmap-tango/build/arm64-v8a
+cd rtabmap-tango/build/arm64-v8a
+$pwd/cmake-3.17.0-Linux-x86_64/bin/cmake -DCMAKE_TOOLCHAIN_FILE=$ANDROID_NDK/build/cmake/android.toolchain.cmake -DANDROID_ABI=arm64-v8a -DANDROID_NDK=$ANDROID_NDK -DANDROID_NATIVE_API_LEVEL=$api -DBUILD_SHARED_LIBS=OFF -DCMAKE_BUILD_TYPE=Release -DCMAKE_INSTALL_PREFIX=$prefix/arm64-v8a -DCMAKE_FIND_ROOT_PATH="$prefix/arm64-v8a/bin;$prefix/arm64-v8a;$prefix/arm64-v8a/share" -DBUILD_EXAMPLES=OFF -DBUILD_TOOLS=OFF -DOpenCV_DIR=$prefix/arm64-v8a/sdk/native/jni ../..
+make
+make clean
+
+
+

docker/focal/Dockerfile

@@ -54,6 +54,7 @@ ENV DEBIAN_FRONTEND=noninteractive
 # Taken from https://github.com/microsoft/Azure-Kinect-Sensor-SDK/issues/1190#issuecomment-822772494
 # K4A binaries on 20.04 not released yet, we should take those from 18.04
 RUN if [ "$TARGETPLATFORM" = "linux/amd64" ]; then echo "Installing k4a..." && \
+    apt-get update && apt-get install -y curl && \
     echo "Download libk4a1.3_1.3.0_amd64.deb..." && \
     curl -sSL https://packages.microsoft.com/ubuntu/18.04/prod/pool/main/libk/libk4a1.3/libk4a1.3_1.3.0_amd64.deb > /tmp/libk4a1.3_1.3.0_amd64.deb && \
     echo "Download libk4a1.3-dev_1.3.0_amd64.deb..." && \
@@ -77,7 +78,7 @@ RUN if [ "$TARGETPLATFORM" = "linux/amd64" ]; then echo "Installing k4a..." && \
 
 # libfreenect2 
 RUN if [ "$TARGETPLATFORM" = "linux/amd64" ]; then echo "Installing libfreenect2..." && \
-    apt-get install -y mesa-utils xserver-xorg-video-all libusb-1.0-0-dev libturbojpeg0-dev libglfw3-dev && \
+    apt-get update && apt-get install -y mesa-utils xserver-xorg-video-all libusb-1.0-0-dev libturbojpeg0-dev libglfw3-dev && \
     git clone https://github.com/OpenKinect/libfreenect2 && \
     cd libfreenect2 && \
     mkdir build && \
@@ -90,7 +91,7 @@ RUN if [ "$TARGETPLATFORM" = "linux/amd64" ]; then echo "Installing libfreenect2
 
 # zed open capture
 RUN if [ "$TARGETPLATFORM" = "linux/amd64" ]; then echo "Installing zed-open-capture..." && \
-    apt install libusb-1.0-0-dev libhidapi-libusb0 libhidapi-dev wget && \
+    apt-get update && apt install libusb-1.0-0-dev libhidapi-libusb0 libhidapi-dev wget && \
     git clone https://github.com/stereolabs/zed-open-capture.git && \
     cd zed-open-capture && \
     mkdir build && \
@@ -103,7 +104,7 @@ RUN if [ "$TARGETPLATFORM" = "linux/amd64" ]; then echo "Installing zed-open-cap
 
 # AliceVision v2.4.0 modified (Sept 13 2021)
 RUN if [ "$TARGETPLATFORM" = "linux/amd64" ]; then echo "Installing AliceVision..." && \
-apt-get update && DEBIAN_FRONTEND=noninteractive apt-get install -y \
+      apt-get update && DEBIAN_FRONTEND=noninteractive apt-get install -y \
       libsuitesparse-dev \
       libceres-dev \
       xorg-dev \

guilib/include/rtabmap/gui/ExportCloudsDialog.h

@@ -127,6 +127,7 @@ private Q_SLOTS:
 	void saveSettings();
 	void updateReconstructionFlavor();
 	void selectDistortionModel();
+	void selectCamProjMask();
 	void updateMLSGrpVisibility();
 	void cancel();
 
@@ -138,7 +139,8 @@ private:
 			const std::map<int, std::pair<pcl::PointCloud<pcl::PointXYZRGB>::Ptr, pcl::IndicesPtr> > & cachedClouds,
 			const std::map<int, LaserScan> & cachedScans,
 			const ParametersMap & parameters,
-			bool & has2dScans) const;
+			bool & has2dScans,
+			bool & scansHaveRGB) const;
 	void saveClouds(const QString & workingDirectory, const std::map<int, Transform> & poses, const std::map<int, pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr> & clouds, bool binaryMode = true, const std::vector<std::map<int, pcl::PointXY> > & pointToPixels = std::vector<std::map<int, pcl::PointXY> >());
 	void saveMeshes(const QString & workingDirectory, const std::map<int, Transform> & poses, const std::map<int, pcl::PolygonMesh::Ptr> & meshes, bool binaryMode = true);
 	void saveTextureMeshes(const QString & workingDirectory, const std::map<int, Transform> & poses, std::map<int, pcl::TextureMesh::Ptr> & textureMeshes, const QMap<int, Signature> & cachedSignatures, const std::vector<std::map<int, pcl::PointXY> > & textureVertexToPixels);
@@ -150,6 +152,7 @@ private:
 	bool _canceled;
 	GainCompensator * _compensator;
 	const DBDriver * _dbDriver;
+	bool _scansHaveRGB;
 };
 
 }

guilib/include/rtabmap/gui/MainWindow.h

@@ -194,6 +194,7 @@ protected Q_SLOTS:
 	void postGoal(const QString & goal);
 	void cancelGoal();
 	void label();
+	void removeLabel();
 	void updateCacheFromDatabase();
 	void anchorCloudsToGroundTruth();
 	void selectScreenCaptureFormat(bool checked);

guilib/src/DatabaseViewer.cpp

@@ -2702,6 +2702,16 @@ void DatabaseViewer::exportPoses(int format)
 			}
 		}
 
+		if(format != 4 && !poses.empty() && poses.begin()->first<0) // not g2o, landmark not supported
+		{
+			UWARN("Only g2o format (4) can export landmarks, they are ignored with format %d", format);
+			std::map<int, Transform>::iterator iter=poses.begin();
+			while(iter!=poses.end() && iter->first < 0)
+			{
+				poses.erase(iter++);
+			}
+		}
+
 		std::map<int, double> stamps;
 		if(format == 1 || format == 10 || format == 11)
 		{
@@ -2729,7 +2739,7 @@ void DatabaseViewer::exportPoses(int format)
 		}
 
 		QString output = pathDatabase_ + QDir::separator() + (format==3?"toro%1.graph":format==4?"poses%1.g2o":"poses%1.txt");
-		QString suffix = odometry?"_odom":"";
+		QString suffix = odometry?"_odom":groundTruth?"_gt":"";
 		output = output.arg(suffix);
 
 		QString path = QFileDialog::getSaveFileName(
@@ -5052,9 +5062,9 @@ void DatabaseViewer::update(int value,
 								float xMin=0.0f, yMin=0.0f;
 								cv::Mat map8S;
 								ParametersMap parameters = ui_->parameters_toolbox->getParameters();
-								parameters = Parameters::filterParameters(parameters, "GridGlobal", true);
 								float gridCellSize = Parameters::defaultGridCellSize();
 								Parameters::parse(parameters, Parameters::kGridCellSize(), gridCellSize);
+								parameters = Parameters::filterParameters(parameters, "GridGlobal", true);
 #ifdef RTABMAP_OCTOMAP
 								if(octomap)
 								{
@@ -7757,32 +7767,30 @@ void DatabaseViewer::refineConstraint(int from, int to, bool silent)
 						ui_->doubleSpinBox_icp_minDepth->value(),
 						0,
 						ui_->parameters_toolbox->getParameters());
-				int maxLaserScans = cloudFrom->size();
-				fromS->sensorData().setLaserScan(LaserScan(util3d::laserScanFromPointCloud(*util3d::removeNaNFromPointCloud(cloudFrom), Transform()), maxLaserScans, 0));
-				toS->sensorData().setLaserScan(LaserScan(util3d::laserScanFromPointCloud(*util3d::removeNaNFromPointCloud(cloudTo), Transform()), maxLaserScans, 0));
 
-				if(!fromS->sensorData().laserScanCompressed().isEmpty() && !toS->sensorData().laserScanCompressed().isEmpty())
+				if(cloudFrom->empty() && cloudTo->empty())
+				{
+					std::string msg = "Option to generate scan from depth is checked (GUI Parameters->Refine), but "
+									  "resulting clouds from depth are empty. Transformation estimation will likely "
+									  "fails. Uncheck the parameter to use laser scans.";
+					UWARN(msg.c_str());
+					if(!silent)
+					{
+						QMessageBox::warning(this,
+											tr("Refine link"),
+											tr("%1").arg(msg.c_str()));
+					}
+				}
+				else if(!fromS->sensorData().laserScanCompressed().isEmpty() || !toS->sensorData().laserScanCompressed().isEmpty())
 				{
 					UWARN("There are laser scans in data, but generate laser scan from "
 						  "depth image option is activated (GUI Parameters->Refine). "
 						  "Ignoring saved laser scans...");
 				}
-				else
-				{
-					QString msg = tr("Generating laser scan from depth image is checked "
-							"(GUI Parameters->Refine), but selected nodes don't contain "
-							"depth data. Empty laser scans are generated, so transform "
-							"estimation will likely fail. Uncheck to use laser scans instead "
-							"(if there are some).");
-					if(!silent)
-					{
 
-						QMessageBox::warning(this,
+				int maxLaserScans = cloudFrom->size();
-								tr("Refine a link"),
+				fromS->sensorData().setLaserScan(LaserScan(util3d::laserScanFromPointCloud(*util3d::removeNaNFromPointCloud(cloudFrom), Transform()), maxLaserScans, 0));
-								msg);
+				toS->sensorData().setLaserScan(LaserScan(util3d::laserScanFromPointCloud(*util3d::removeNaNFromPointCloud(cloudTo), Transform()), maxLaserScans, 0));
-					}
-					UWARN(msg.toStdString().c_str());
-				}
 			}
 			else
 			{
@@ -8036,25 +8044,46 @@ bool DatabaseViewer::addConstraint(int from, int to, bool silent)
 						ui_->doubleSpinBox_icp_minDepth->value(),
 						0,
 						ui_->parameters_toolbox->getParameters());
+
+				if(cloudFrom->empty() && cloudTo->empty())
+				{
+					std::string msg = "Option to generate scan from depth is checked (GUI Parameters->Refine), but "
+									  "resulting clouds from depth are empty. Transformation estimation will likely "
+									  "fails. Uncheck the parameter to use laser scans.";
+					UWARN(msg.c_str());
+					if(!silent)
+					{
+						QMessageBox::warning(this,
+											tr("Add link"),
+											tr("%1").arg(msg.c_str()));
+					}
+				}
+				else if(!fromS->sensorData().laserScanCompressed().isEmpty() || !toS->sensorData().laserScanCompressed().isEmpty())
+				{
+					UWARN("There are laser scans in data, but generate laser scan from "
+						  "depth image option is activated (GUI Parameters->Refine). Ignoring saved laser scans...");
+				}
+
 				int maxLaserScans = cloudFrom->size();
 				fromS->sensorData().setLaserScan(LaserScan(util3d::laserScanFromPointCloud(*util3d::removeNaNFromPointCloud(cloudFrom), Transform()), maxLaserScans, 0));
 				toS->sensorData().setLaserScan(LaserScan(util3d::laserScanFromPointCloud(*util3d::removeNaNFromPointCloud(cloudTo), Transform()), maxLaserScans, 0));
-
-				if(!fromS->sensorData().laserScanCompressed().isEmpty() || !toS->sensorData().laserScanCompressed().isEmpty())
-				{
-					UWARN("There are laser scans in data, but generate laser scan from "
-						  "depth image option is activated. Ignoring saved laser scans...");
-				}
 			}
 		}
 		else if(!reextractVisualFeatures && fromS->getWords().empty() && toS->getWords().empty())
 		{
-			UWARN("\"%s\" is false and signatures (%d and %d) don't have words, "
+			std::string msg = uFormat("\"%s\" is false and signatures (%d and %d) don't have words, "
 					"registration will not be possible. Set \"%s\" to true.",
 					Parameters::kRGBDLoopClosureReextractFeatures().c_str(),
 					fromS->id(),
 					toS->id(),
 					Parameters::kRGBDLoopClosureReextractFeatures().c_str());
+			UWARN(msg.c_str());
+			if(!silent)
+			{
+				QMessageBox::warning(this,
+									tr("Add link"),
+									tr("%1").arg(msg.c_str()));
+			}
 		}
 
 		Transform guess;

guilib/src/ExportCloudsDialog.cpp

@@ -86,7 +86,8 @@ ExportCloudsDialog::ExportCloudsDialog(QWidget *parent) :
 	QDialog(parent),
 	_canceled(false),
 	_compensator(0),
-	_dbDriver(0)
+	_dbDriver(0),
+	_scansHaveRGB(false)
 {
 	_ui = new Ui_ExportCloudsDialog();
 	_ui->setupUi(this);
@@ -105,6 +106,7 @@ ExportCloudsDialog::ExportCloudsDialog(QWidget *parent) :
 	connect(_ui->checkBox_binary, SIGNAL(stateChanged(int)), this, SIGNAL(configChanged()));
 	connect(_ui->spinBox_normalKSearch, SIGNAL(valueChanged(int)), this, SIGNAL(configChanged()));
 	connect(_ui->doubleSpinBox_normalRadiusSearch, SIGNAL(valueChanged(double)), this, SIGNAL(configChanged()));
+	connect(_ui->doubleSpinBox_groundNormalsUp, SIGNAL(valueChanged(double)), this, SIGNAL(configChanged()));
 	connect(_ui->comboBox_pipeline, SIGNAL(currentIndexChanged(int)), this, SIGNAL(configChanged()));
 	connect(_ui->comboBox_pipeline, SIGNAL(currentIndexChanged(int)), this, SLOT(updateReconstructionFlavor()));
 	connect(_ui->comboBox_meshingApproach, SIGNAL(currentIndexChanged(int)), this, SIGNAL(configChanged()));
@@ -151,6 +153,7 @@ ExportCloudsDialog::ExportCloudsDialog(QWidget *parent) :
 	connect(_ui->checkBox_assemble, SIGNAL(clicked(bool)), this, SIGNAL(configChanged()));
 	connect(_ui->checkBox_assemble, SIGNAL(clicked(bool)), this, SLOT(updateReconstructionFlavor()));
 	connect(_ui->doubleSpinBox_voxelSize_assembled, SIGNAL(valueChanged(double)), this, SIGNAL(configChanged()));
+	connect(_ui->spinBox_randomSamples_assembled, SIGNAL(valueChanged(int)), this, SIGNAL(configChanged()));
 	connect(_ui->comboBox_frame, SIGNAL(currentIndexChanged(int)), this, SIGNAL(configChanged()));
 	connect(_ui->comboBox_frame, SIGNAL(currentIndexChanged(int)), this, SLOT(updateReconstructionFlavor()));
 
@@ -190,6 +193,9 @@ ExportCloudsDialog::ExportCloudsDialog(QWidget *parent) :
 	connect(_ui->checkBox_cameraProjection, SIGNAL(stateChanged(int)), this, SIGNAL(configChanged()));
 	connect(_ui->checkBox_cameraProjection, SIGNAL(stateChanged(int)), this, SLOT(updateReconstructionFlavor()));
 	connect(_ui->lineEdit_camProjRoiRatios, SIGNAL(textChanged(const QString &)), this, SIGNAL(configChanged()));
+	connect(_ui->toolButton_camProjMaskFilePath, SIGNAL(clicked()), this, SLOT(selectCamProjMask()));
+	connect(_ui->lineEdit_camProjMaskFilePath, SIGNAL(textChanged(const QString &)), this, SIGNAL(configChanged()));
+	connect(_ui->spinBox_camProjDecimation, SIGNAL(valueChanged(int)), this, SIGNAL(configChanged()));
 	connect(_ui->doubleSpinBox_camProjMaxDistance, SIGNAL(valueChanged(double)), this, SIGNAL(configChanged()));
 	connect(_ui->doubleSpinBox_camProjMaxAngle, SIGNAL(valueChanged(double)), this, SIGNAL(configChanged()));
 	connect(_ui->checkBox_camProjDistanceToCamPolicy, SIGNAL(stateChanged(int)), this, SIGNAL(configChanged()));
@@ -346,6 +352,7 @@ void ExportCloudsDialog::saveSettings(QSettings & settings, const QString & grou
 	settings.setValue("binary", _ui->checkBox_binary->isChecked());
 	settings.setValue("normals_k", _ui->spinBox_normalKSearch->value());
 	settings.setValue("normals_radius", _ui->doubleSpinBox_normalRadiusSearch->value());
+	settings.setValue("normals_ground_normals_up", _ui->doubleSpinBox_groundNormalsUp->value());
 	settings.setValue("intensity_colormap", _ui->comboBox_intensityColormap->currentIndex());
 
 	settings.setValue("nodes_filtering", _ui->checkBox_nodes_filtering->isChecked());
@@ -383,6 +390,7 @@ void ExportCloudsDialog::saveSettings(QSettings & settings, const QString & grou
 
 	settings.setValue("assemble", _ui->checkBox_assemble->isChecked());
 	settings.setValue("assemble_voxel",_ui->doubleSpinBox_voxelSize_assembled->value());
+	settings.setValue("assemble_samples",_ui->spinBox_randomSamples_assembled->value());
 	settings.setValue("frame",_ui->comboBox_frame->currentIndex());
 
 	settings.setValue("subtract",_ui->checkBox_subtraction->isChecked());
@@ -410,6 +418,8 @@ void ExportCloudsDialog::saveSettings(QSettings & settings, const QString & grou
 
 	settings.setValue("cam_proj", _ui->checkBox_cameraProjection->isChecked());
 	settings.setValue("cam_proj_roi_ratios", _ui->lineEdit_camProjRoiRatios->text());
+	settings.setValue("cam_proj_mask", _ui->lineEdit_camProjMaskFilePath->text());
+	settings.setValue("cam_proj_decimation", _ui->spinBox_camProjDecimation->value());
 	settings.setValue("cam_proj_max_distance", _ui->doubleSpinBox_camProjMaxDistance->value());
 	settings.setValue("cam_proj_max_angle", _ui->doubleSpinBox_camProjMaxAngle->value());
 	settings.setValue("cam_proj_distance_policy", _ui->checkBox_camProjDistanceToCamPolicy->isChecked());
@@ -515,6 +525,7 @@ void ExportCloudsDialog::loadSettings(QSettings & settings, const QString & grou
 	_ui->checkBox_binary->setChecked(settings.value("binary", _ui->checkBox_binary->isChecked()).toBool());
 	_ui->spinBox_normalKSearch->setValue(settings.value("normals_k", _ui->spinBox_normalKSearch->value()).toInt());
 	_ui->doubleSpinBox_normalRadiusSearch->setValue(settings.value("normals_radius", _ui->doubleSpinBox_normalRadiusSearch->value()).toDouble());
+	_ui->doubleSpinBox_groundNormalsUp->setValue(settings.value("normals_ground_normals_up", _ui->doubleSpinBox_groundNormalsUp->value()).toDouble());
 	_ui->comboBox_intensityColormap->setCurrentIndex(settings.value("intensity_colormap", _ui->comboBox_intensityColormap->currentIndex()).toInt());
 
 	_ui->checkBox_nodes_filtering->setChecked(settings.value("nodes_filtering", _ui->checkBox_nodes_filtering->isChecked()).toBool());
@@ -555,6 +566,7 @@ void ExportCloudsDialog::loadSettings(QSettings & settings, const QString & grou
 		_ui->checkBox_assemble->setChecked(settings.value("assemble", _ui->checkBox_assemble->isChecked()).toBool());
 	}
 	_ui->doubleSpinBox_voxelSize_assembled->setValue(settings.value("assemble_voxel", _ui->doubleSpinBox_voxelSize_assembled->value()).toDouble());
+	_ui->spinBox_randomSamples_assembled->setValue(settings.value("assemble_samples", _ui->spinBox_randomSamples_assembled->value()).toInt());
 	_ui->comboBox_frame->setCurrentIndex(settings.value("frame", _ui->comboBox_frame->currentIndex()).toInt());
 
 	_ui->checkBox_subtraction->setChecked(settings.value("subtract",_ui->checkBox_subtraction->isChecked()).toBool());
@@ -582,6 +594,8 @@ void ExportCloudsDialog::loadSettings(QSettings & settings, const QString & grou
 
 	_ui->checkBox_cameraProjection->setChecked(settings.value("cam_proj", _ui->checkBox_cameraProjection->isChecked()).toBool());
 	_ui->lineEdit_camProjRoiRatios->setText(settings.value("cam_proj_roi_ratios", _ui->lineEdit_camProjRoiRatios->text()).toString());
+	_ui->lineEdit_camProjMaskFilePath->setText(settings.value("cam_proj_mask", _ui->lineEdit_camProjMaskFilePath->text()).toString());
+	_ui->spinBox_camProjDecimation->setValue(settings.value("cam_proj_decimation", _ui->spinBox_camProjDecimation->value()).toInt());
 	_ui->doubleSpinBox_camProjMaxDistance->setValue(settings.value("cam_proj_max_distance", _ui->doubleSpinBox_camProjMaxDistance->value()).toDouble());
 	_ui->doubleSpinBox_camProjMaxAngle->setValue(settings.value("cam_proj_max_angle", _ui->doubleSpinBox_camProjMaxAngle->value()).toDouble());
 	_ui->checkBox_camProjDistanceToCamPolicy->setChecked(settings.value("cam_proj_distance_policy", _ui->checkBox_camProjDistanceToCamPolicy->isChecked()).toBool());
@@ -687,6 +701,7 @@ void ExportCloudsDialog::restoreDefaults()
 	_ui->checkBox_binary->setChecked(true);
 	_ui->spinBox_normalKSearch->setValue(20);
 	_ui->doubleSpinBox_normalRadiusSearch->setValue(0.0);
+	_ui->doubleSpinBox_groundNormalsUp->setValue(0.0);
 	_ui->comboBox_intensityColormap->setCurrentIndex(0);
 
 	_ui->checkBox_nodes_filtering->setChecked(false);
@@ -724,6 +739,7 @@ void ExportCloudsDialog::restoreDefaults()
 
 	_ui->checkBox_assemble->setChecked(true);
 	_ui->doubleSpinBox_voxelSize_assembled->setValue(0.01);
+	_ui->spinBox_randomSamples_assembled->setValue(0);
 	_ui->comboBox_frame->setCurrentIndex(0);
 
 	_ui->checkBox_subtraction->setChecked(false);
@@ -751,6 +767,8 @@ void ExportCloudsDialog::restoreDefaults()
 
 	_ui->checkBox_cameraProjection->setChecked(false);
 	_ui->lineEdit_camProjRoiRatios->setText("0.0 0.0 0.0 0.0");
+	_ui->lineEdit_camProjMaskFilePath->setText("");
+	_ui->spinBox_camProjDecimation->setValue(1);
 	_ui->doubleSpinBox_camProjMaxDistance->setValue(0);
 	_ui->doubleSpinBox_camProjMaxAngle->setValue(0);
 	_ui->checkBox_camProjDistanceToCamPolicy->setChecked(true);
@@ -1013,6 +1031,20 @@ void ExportCloudsDialog::selectDistortionModel()
 	}
 }
 
+void ExportCloudsDialog::selectCamProjMask()
+{
+	QString dir = _ui->lineEdit_camProjMaskFilePath->text();
+	if(dir.isEmpty())
+	{
+		dir = _workingDirectory;
+	}
+	QString path = QFileDialog::getOpenFileName(this, tr("Select file"), dir, tr("Mask (grayscale) (*.png *.pgm *bmp)"));
+	if(path.size())
+	{
+		_ui->lineEdit_camProjMaskFilePath->setText(path);
+	}
+}
+
 void ExportCloudsDialog::setSaveButton()
 {
 	_ui->buttonBox->button(QDialogButtonBox::Ok)->setVisible(false);
@@ -1319,7 +1351,7 @@ void ExportCloudsDialog::viewClouds(
 							for(unsigned int j=0; j<vertices.vertices.size(); ++j)
 							{
 								UASSERT(oi < cloud->size());
-								UASSERT_MSG(vertices.vertices[j] < originalCloud->size(), uFormat("%d vs %d", vertices.vertices[j], (int)originalCloud->size()).c_str());
+								UASSERT_MSG((int)vertices.vertices[j] < (int)originalCloud->size(), uFormat("%d vs %d", vertices.vertices[j], (int)originalCloud->size()).c_str());
 								cloud->at(oi) = originalCloud->at(vertices.vertices[j]);
 								vertices.vertices[j] = oi; // new vertice index
 								++oi;
@@ -1409,7 +1441,7 @@ void ExportCloudsDialog::viewClouds(
 				_progressDialog->appendText(tr("Viewing the cloud %1 (%2 points)...").arg(iter->first).arg(iter->second->size()));
 				_progressDialog->incrementStep();
 
-				if(!_ui->checkBox_fromDepth->isChecked() &&
+				if(!_ui->checkBox_fromDepth->isChecked() && !_scansHaveRGB &&
 					!(_ui->checkBox_cameraProjection->isEnabled() &&
 					_ui->checkBox_cameraProjection->isChecked() &&
 					_ui->checkBox_camProjRecolorPoints->isChecked() &&
@@ -1637,7 +1669,8 @@ bool ExportCloudsDialog::getExportedClouds(
 					cachedClouds,
 					cachedScans,
 					parameters,
-					has2dScans);
+					has2dScans,
+					_scansHaveRGB);
 		}
 		else
 		{
@@ -1900,13 +1933,27 @@ bool ExportCloudsDialog::getExportedClouds(
 					assembledCloud->points[i].normal_y = normals->points[i].normal_y;
 					assembledCloud->points[i].normal_z = normals->points[i].normal_z;
 				}
+				_progressDialog->appendText(tr("Adjusting normals to viewpoints (%1 points)...").arg(assembledCloud->size()));
 
 				// adjust with point of views
 				util3d::adjustNormalsToViewPoints(
 											normalViewpoints,
 											rawAssembledCloud,
 											rawCameraIndices,
-											assembledCloud);
+											assembledCloud,
+											_ui->doubleSpinBox_groundNormalsUp->value());
+			}
+
+			if(_ui->spinBox_randomSamples_assembled->value()>0 &&
+			   (int)assembledCloud->size() > _ui->spinBox_randomSamples_assembled->value())
+			{
+				_progressDialog->appendText(tr("Random samples filtering (in=%1 points, samples=%2)...")
+														.arg(assembledCloud->size())
+														.arg(_ui->spinBox_randomSamples_assembled->value()));
+				assembledCloud = util3d::randomSampling(assembledCloud, _ui->spinBox_randomSamples_assembled->value());
+				_progressDialog->appendText(tr("Random samples filtering (out=%1 points, samples=%2)... done!")
+																		.arg(assembledCloud->size())
+																		.arg(_ui->spinBox_randomSamples_assembled->value()));
 			}
 
 			clouds.insert(std::make_pair(0, std::make_pair(assembledCloud, indices)));
@@ -2557,7 +2604,7 @@ bool ExportCloudsDialog::getExportedClouds(
 					{
 						TexturingState texturingState(_progressDialog, false);
 
-						if(!_ui->checkBox_fromDepth->isChecked())
+						if(!_ui->checkBox_fromDepth->isChecked() && !_scansHaveRGB)
 						{
 							// When laser scans are exported, convert Intensity to GrayScale
 							int maxIntensity = 1;
@@ -2705,17 +2752,56 @@ bool ExportCloudsDialog::getExportedClouds(
 						_progressDialog->setAutoClose(false);
 					}
 				}
+
+				std::map<int, std::vector<rtabmap::CameraModel> > cameraModelsProj;
+				if(_ui->spinBox_camProjDecimation->value()>1)
+				{
+					for(std::map<int, std::vector<rtabmap::CameraModel> >::iterator iter=cameraModels.begin();
+							iter!=cameraModels.end();
+							++iter)
+					{
+						std::vector<rtabmap::CameraModel> models;
+						for(size_t i=0; i<iter->second.size(); ++i)
+						{
+							models.push_back(iter->second[i].scaled(1.0/double(_ui->spinBox_camProjDecimation->value())));
+						}
+						cameraModelsProj.insert(std::make_pair(iter->first, models));
+					}
+				}
+				else
+				{
+					cameraModelsProj = cameraModels;
+				}
+
+				cv::Mat projMask;
+				if(!_ui->lineEdit_camProjMaskFilePath->text().isEmpty())
+				{
+					projMask = cv::imread(_ui->lineEdit_camProjMaskFilePath->text().toStdString(), cv::IMREAD_GRAYSCALE);
+					if(_ui->spinBox_camProjDecimation->value()>1)
+					{
+						cv::Mat out = projMask;
+						cv::resize(projMask, out, cv::Size(), 1.0f/float(_ui->spinBox_camProjDecimation->value()), 1.0f/float(_ui->spinBox_camProjDecimation->value()), cv::INTER_NEAREST);
+						projMask = out;
+					}
+				}
+
 				std::vector<std::pair< std::pair<int, int>, pcl::PointXY> > pointToPixel;
 				pointToPixel = util3d::projectCloudToCameras(
 						*assembledCloud,
 						cameraPoses,
-						cameraModels,
+						cameraModelsProj,
 						_ui->doubleSpinBox_camProjMaxDistance->value(),
 						_ui->doubleSpinBox_camProjMaxAngle->value()*M_PI/180.0,
 						roiRatios,
+						projMask,
 						_ui->checkBox_camProjDistanceToCamPolicy->isChecked(),
 						&texturingState);
 
+				if(texturingState.isCanceled())
+				{
+					return false;
+				}
+
 				// color the cloud
 				UASSERT(pointToPixel.empty() || pointToPixel.size() == assembledCloud->size());
 				pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr assembledCloudValidPoints;
@@ -2733,7 +2819,7 @@ bool ExportCloudsDialog::getExportedClouds(
 				if(_ui->checkBox_camProjRecolorPoints->isChecked())
 				{
 					int imagesDone = 1;
-					for(std::map<int, rtabmap::Transform>::iterator iter=cameraPoses.begin(); iter!=cameraPoses.end(); ++iter)
+					for(std::map<int, rtabmap::Transform>::iterator iter=cameraPoses.begin(); iter!=cameraPoses.end() && !_canceled; ++iter)
 					{
 						int nodeID = iter->first;
 
@@ -2750,15 +2836,19 @@ bool ExportCloudsDialog::getExportedClouds(
 						}
 						if(!image.empty())
 						{
-							UASSERT(cameraModels.find(nodeID) != cameraModels.end());
+
-							int modelsSize = cameraModels.at(nodeID).size();
+							if(_ui->spinBox_camProjDecimation->value()>1)
+							{
+								image = util2d::decimate(image, _ui->spinBox_camProjDecimation->value());
+							}
+
+							UASSERT(cameraModelsProj.find(nodeID) != cameraModelsProj.end());
+							int modelsSize = cameraModelsProj.at(nodeID).size();
 							for(size_t i=0; i<pointToPixel.size(); ++i)
 							{
 								int cameraIndex = pointToPixel[i].first.second;
 								if(nodeID == pointToPixel[i].first.first && cameraIndex>=0)
 								{
-									pcl::PointXYZRGBNormal & pt = assembledCloud->at(i);
-
 									int subImageWidth = image.cols / modelsSize;
 									cv::Mat subImage = image(cv::Range::all(), cv::Range(cameraIndex*subImageWidth, (cameraIndex+1)*subImageWidth));
 
@@ -2767,6 +2857,7 @@ bool ExportCloudsDialog::getExportedClouds(
 									UASSERT(x>=0 && x<subImage.cols);
 									UASSERT(y>=0 && y<subImage.rows);
 
+									pcl::PointXYZRGBNormal & pt = assembledCloud->at(i);
 									if(subImage.type()==CV_8UC3)
 									{
 										cv::Vec3b bgr = subImage.at<cv::Vec3b>(y, x);
@@ -2785,17 +2876,18 @@ bool ExportCloudsDialog::getExportedClouds(
 						QString msg = tr("Processed %1/%2 images").arg(imagesDone++).arg(cameraPoses.size());
 						UINFO(msg.toStdString().c_str());
 						_progressDialog->appendText(msg);
+						QApplication::processEvents();
 					}
 				}
 
 				pcl::IndicesPtr validIndices(new std::vector<int>(pointToPixel.size()));
 				size_t oi = 0;
-				for(size_t i=0; i<pointToPixel.size(); ++i)
+				for(size_t i=0; i<pointToPixel.size() && !_canceled; ++i)
 				{
 					pcl::PointXYZRGBNormal & pt = assembledCloud->at(i);
 					if(pointToPixel[i].first.first <=0)
 					{
-						if(_ui->checkBox_camProjRecolorPoints->isChecked() && !_ui->checkBox_fromDepth->isChecked())
+						if(_ui->checkBox_camProjRecolorPoints->isChecked() && !_ui->checkBox_fromDepth->isChecked() && !_scansHaveRGB)
 						{
 							pt.r = 255;
 							pt.g = 0;
@@ -3253,7 +3345,7 @@ bool ExportCloudsDialog::getExportedClouds(
 								for(int k=0; k<polygonSize; ++k)
 								{
 									//uv
-									UASSERT(vertices.vertices[k] < oter->second.size());
+									UASSERT((int)vertices.vertices[k] < (int)oter->second.size());
 									int originalVertex = oter->second[vertices.vertices[k]];
 									textureMesh->tex_coordinates[0][i*polygonSize+k] = Eigen::Vector2f(
 											float(originalVertex % w) / float(w),      // u
@@ -3438,8 +3530,10 @@ std::map<int, std::pair<pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr, pcl::Indic
 		const std::map<int, std::pair<pcl::PointCloud<pcl::PointXYZRGB>::Ptr, pcl::IndicesPtr> > & cachedClouds,
 		const std::map<int, LaserScan> & cachedScans,
 		const ParametersMap & parameters,
-		bool & has2dScans) const
+		bool & has2dScans,
+		bool & scansHaveRGB) const
 {
+	scansHaveRGB = false;
 	has2dScans = false;
 	std::map<int, std::pair<pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr, pcl::IndicesPtr> > clouds;
 	int index=1;
@@ -3568,6 +3662,10 @@ std::map<int, std::pair<pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr, pcl::Indic
 						{
 							pcl::PointCloud<pcl::Normal>::Ptr normals = util3d::computeNormals(cloudWithoutNormals, indices, _ui->spinBox_normalKSearch->value(), _ui->doubleSpinBox_normalRadiusSearch->value(), viewPoint);
 							pcl::concatenateFields(*cloudWithoutNormals, *normals, *cloud);
+							if(_ui->doubleSpinBox_groundNormalsUp->value() > 0.0)
+							{
+								util3d::adjustNormalsToViewPoint(cloud, viewPoint, (float)_ui->doubleSpinBox_groundNormalsUp->value());
+							}
 						}
 						else
 						{
@@ -3611,6 +3709,10 @@ std::map<int, std::pair<pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr, pcl::Indic
 							_ui->spinBox_normalKSearch->value(),
 							_ui->doubleSpinBox_normalRadiusSearch->value());
 
+					if(!scan.empty())
+					{
+						scansHaveRGB = scan.hasRGB();
+					}
 					localTransform = scan.localTransform();
 					cloud = util3d::laserScanToPointCloudRGBNormal(scan, localTransform); // put in base frame by default
 					indices->resize(cloud->size());
@@ -3699,6 +3801,10 @@ std::map<int, std::pair<pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr, pcl::Indic
 				{
 					pcl::PointCloud<pcl::Normal>::Ptr normals = util3d::computeNormals(cloudWithoutNormals, indices, _ui->spinBox_normalKSearch->value(), _ui->doubleSpinBox_normalRadiusSearch->value(), viewPoint);
 					pcl::concatenateFields(*cloudWithoutNormals, *normals, *cloud);
+					if(_ui->doubleSpinBox_groundNormalsUp->value() > 0.0)
+					{
+						util3d::adjustNormalsToViewPoint(cloud, viewPoint, (float)_ui->doubleSpinBox_groundNormalsUp->value());
+					}
 				}
 				else
 				{
@@ -3715,6 +3821,10 @@ std::map<int, std::pair<pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr, pcl::Indic
 							_ui->spinBox_normalKSearch->value(),
 							_ui->doubleSpinBox_normalRadiusSearch->value());
 
+				if(!scan.empty())
+				{
+					scansHaveRGB = scan.hasRGB();
+				}
 				localTransform = scan.localTransform();
 				cloud = util3d::laserScanToPointCloudRGBNormal(scan, localTransform); // put in base frame by default
 				indices->resize(cloud->size());
@@ -3879,7 +3989,7 @@ void ExportCloudsDialog::saveClouds(
 				pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloudRGBWithoutNormals;
 				pcl::PointCloud<pcl::PointXYZI>::Ptr cloudIWithoutNormals;
 				pcl::PointCloud<pcl::PointXYZINormal>::Ptr cloudIWithNormals;
-				if(!_ui->checkBox_fromDepth->isChecked() &&
+				if(!_ui->checkBox_fromDepth->isChecked() && !_scansHaveRGB &&
 					!(_ui->checkBox_cameraProjection->isEnabled() &&
 					_ui->checkBox_cameraProjection->isChecked() &&
 					_ui->checkBox_camProjRecolorPoints->isChecked() &&
@@ -4070,7 +4180,7 @@ void ExportCloudsDialog::saveClouds(
 							pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloudRGBWithoutNormals;
 							pcl::PointCloud<pcl::PointXYZI>::Ptr cloudIWithoutNormals;
 							pcl::PointCloud<pcl::PointXYZINormal>::Ptr cloudIWithNormals;
-							if(!_ui->checkBox_fromDepth->isChecked())
+							if(!_ui->checkBox_fromDepth->isChecked() && !_scansHaveRGB)
 							{
 								// When laser scans are exported, convert RGB to Intensity
 								if(_ui->spinBox_normalKSearch->value()>0 || _ui->doubleSpinBox_normalRadiusSearch->value()>0.0)

guilib/src/GuiLib.qrc

@@ -42,5 +42,6 @@
         <file>images/mynteyes.png</file>
         <file>images/l515.png</file>
         <file>images/oakd.png</file>
+		<file>images/astra.png</file>
     </qresource>
 </RCC>

guilib/src/MainWindow.cpp

@@ -354,6 +354,7 @@ MainWindow::MainWindow(PreferencesDialog * prefDialog, QWidget * parent, bool sh
 	connect(_ui->actionSend_waypoints, SIGNAL(triggered()), this, SLOT(sendWaypoints()));
 	connect(_ui->actionCancel_goal, SIGNAL(triggered()), this, SLOT(cancelGoal()));
 	connect(_ui->actionLabel_current_location, SIGNAL(triggered()), this, SLOT(label()));
+	connect(_ui->actionRemove_label, SIGNAL(triggered()), this, SLOT(removeLabel()));
 	connect(_ui->actionClear_cache, SIGNAL(triggered()), this, SLOT(clearTheCache()));
 	connect(_ui->actionAbout, SIGNAL(triggered()), _aboutDialog , SLOT(exec()));
 	connect(_ui->actionHelp, SIGNAL(triggered()), this , SLOT(openHelp()));
@@ -428,6 +429,7 @@ MainWindow::MainWindow(PreferencesDialog * prefDialog, QWidget * parent, bool sh
 	connect(_ui->actionOpenNI_CV_ASUS, SIGNAL(triggered()), this, SLOT(selectOpenniCvAsus()));
 	connect(_ui->actionOpenNI2, SIGNAL(triggered()), this, SLOT(selectOpenni2()));
 	connect(_ui->actionOpenNI2_kinect, SIGNAL(triggered()), this, SLOT(selectOpenni2()));
+	connect(_ui->actionOpenNI2_orbbec, SIGNAL(triggered()), this, SLOT(selectOpenni2()));
 	connect(_ui->actionOpenNI2_sense, SIGNAL(triggered()), this, SLOT(selectOpenni2()));
 	connect(_ui->actionFreenect2, SIGNAL(triggered()), this, SLOT(selectFreenect2()));
 	connect(_ui->actionKinect_for_Windows_SDK_v2, SIGNAL(triggered()), this, SLOT(selectK4W2()));
@@ -451,6 +453,7 @@ MainWindow::MainWindow(PreferencesDialog * prefDialog, QWidget * parent, bool sh
 	_ui->actionOpenNI_CV_ASUS->setEnabled(CameraOpenNICV::available());
 	_ui->actionOpenNI2->setEnabled(CameraOpenNI2::available());
 	_ui->actionOpenNI2_kinect->setEnabled(CameraOpenNI2::available());
+	_ui->actionOpenNI2_orbbec->setEnabled(CameraOpenNI2::available());
 	_ui->actionOpenNI2_sense->setEnabled(CameraOpenNI2::available());
 	_ui->actionFreenect2->setEnabled(CameraFreenect2::available());
 	_ui->actionKinect_for_Windows_SDK_v2->setEnabled(CameraK4W2::available());
@@ -2298,7 +2301,6 @@ void MainWindow::processStats(const rtabmap::Statistics & stat)
 			// update pose only if odometry is not received
 			std::map<int, int> mapIds = _currentMapIds;
 			std::map<int, Transform> groundTruth = _currentGTPosesMap;
-			std::map<int, std::string> labels = _currentLabels;
 
 			mapIds.insert(std::make_pair(stat.getLastSignatureData().id(), stat.getLastSignatureData().mapId()));
 			if(!stat.getLastSignatureData().getGroundTruthPose().isNull() &&
@@ -2306,10 +2308,6 @@ void MainWindow::processStats(const rtabmap::Statistics & stat)
 			{
 				groundTruth.insert(std::make_pair(stat.getLastSignatureData().id(), stat.getLastSignatureData().getGroundTruthPose()));
 			}
-			for(std::map<int, std::string>::const_iterator iter=stat.labels().begin(); iter!=stat.labels().end(); ++iter)
-			{
-				uInsert(labels, std::pair<int, std::string>(*iter)); // overwrite labels because they could have been modified
-			}
 
 			if(_preferencesDialog->isPriorIgnored() &&
 				_ui->graphicsView_graphView->getWorldMapRotation()==0.0f &&
@@ -2346,6 +2344,7 @@ void MainWindow::processStats(const rtabmap::Statistics & stat)
 			}
 #endif
 
+			UDEBUG("%d %d %d", poses.size(), poses.size()?poses.rbegin()->first:0, stat.refImageId());
 			if(!_odometryReceived && poses.size() && poses.rbegin()->first == stat.refImageId())
 			{
 				if(poses.rbegin()->first == stat.getLastSignatureData().id())
@@ -2410,7 +2409,7 @@ void MainWindow::processStats(const rtabmap::Statistics & stat)
 					poses,
 					stat.constraints(),
 					mapIds,
-					labels,
+					stat.labels(),
 					groundTruth,
 					stat.odomCachePoses(),
 					stat.odomCacheConstraints(),
@@ -2643,7 +2642,7 @@ void MainWindow::updateMapCloud(
 		std::map<int, Transform> nearestPoses;
 		if(maxNodes > 0)
 		{
-			std::map<int, float> nodes = graph::findNearestNodes(poses, currentPose, maxNodes);
+			std::map<int, float> nodes = graph::findNearestNodes(currentPose, poses, 0, 0, maxNodes);
 			for(std::map<int, float>::iterator iter=nodes.begin(); iter!=nodes.end(); ++iter)
 			{
 				if(altitudeDelta<=0.0 ||
@@ -4971,6 +4970,7 @@ void MainWindow::updateSelectSourceMenu()
 	_ui->actionOpenNI_CV_ASUS->setChecked(_preferencesDialog->getSourceDriver() == PreferencesDialog::kSrcOpenNI_CV_ASUS);
 	_ui->actionOpenNI2->setChecked(_preferencesDialog->getSourceDriver() == PreferencesDialog::kSrcOpenNI2);
 	_ui->actionOpenNI2_kinect->setChecked(_preferencesDialog->getSourceDriver() == PreferencesDialog::kSrcOpenNI2);
+	_ui->actionOpenNI2_orbbec->setChecked(_preferencesDialog->getSourceDriver() == PreferencesDialog::kSrcOpenNI2);
 	_ui->actionOpenNI2_sense->setChecked(_preferencesDialog->getSourceDriver() == PreferencesDialog::kSrcOpenNI2);
 	_ui->actionFreenect2->setChecked(_preferencesDialog->getSourceDriver() == PreferencesDialog::kSrcFreenect2);
 	_ui->actionKinect_for_Windows_SDK_v2->setChecked(_preferencesDialog->getSourceDriver() == PreferencesDialog::kSrcK4W2);
@@ -6004,6 +6004,16 @@ void MainWindow::exportPoses(int format)
 			}
 		}
 
+		if(format != 4 && !poses.empty() && poses.begin()->first<0) // not g2o, landmark not supported
+		{
+			UWARN("Only g2o format (4) can export landmarks, they are ignored with format %d", format);
+			std::map<int, Transform>::iterator iter=poses.begin();
+			while(iter!=poses.end() && iter->first < 0)
+			{
+				poses.erase(iter++);
+			}
+		}
+
 		std::map<int, double> stamps;
 		if(format == 1 || format == 10 || format == 11)
 		{
@@ -6933,6 +6943,17 @@ void MainWindow::label()
 	}
 }
 
+void MainWindow::removeLabel()
+{
+	UINFO("Removing label...");
+	bool ok = false;
+	QString label = QInputDialog::getText(this, tr("Remove label"), tr("Label: "), QLineEdit::Normal, "", &ok);
+	if(ok && !label.isEmpty())
+	{
+		this->post(new RtabmapEventCmd(RtabmapEventCmd::kCmdRemoveLabel, label.toStdString(), 0));
+	}
+}
+
 void MainWindow::updateCacheFromDatabase()
 {
 	QString dir = getWorkingDirectory();

guilib/src/PreferencesDialog.cpp

@@ -1121,6 +1121,7 @@ PreferencesDialog::PreferencesDialog(QWidget * parent) :
 	_ui->localDetection_maxPaths->setObjectName(Parameters::kRGBDProximityMaxPaths().c_str());
 	_ui->localDetection_pathFilteringRadius->setObjectName(Parameters::kRGBDProximityPathFilteringRadius().c_str());
 	_ui->localDetection_angle->setObjectName(Parameters::kRGBDProximityAngle().c_str());
+	_ui->localDetection_mergedScanCovFactor->setObjectName(Parameters::kRGBDProximityMergedScanCovFactor().c_str());
 	_ui->checkBox_localSpaceOdomGuess->setObjectName(Parameters::kRGBDProximityOdomGuess().c_str());
 	_ui->checkBox_localSpacePathOdomPosesUsed->setObjectName(Parameters::kRGBDProximityPathRawPosesUsed().c_str());
 	_ui->rgdb_localImmunizationRatio->setObjectName(Parameters::kRGBDLocalImmunizationRatio().c_str());
@@ -5873,7 +5874,7 @@ Camera * PreferencesDialog::createCamera(bool useRawImages, bool useColor)
 		_ui->lineEdit_calibrationFile->text(), 
 		(this->getSourceDriver()>=kSrcStereo && 
 		 this->getSourceDriver()<kSrcRGB && 
-		 !_ui->checkBox_stereo_rectify->isChecked()) || 
+		 _ui->checkBox_stereo_rectify->isEnabled() && !_ui->checkBox_stereo_rectify->isChecked()) || 
 		useRawImages, 
 		useColor, 
 		false,
@@ -6054,8 +6055,8 @@ Camera * PreferencesDialog::createCamera(
 		if(driver == kSrcStereoMyntEye && useRawImages)
 		{
 			QMessageBox::warning(this, tr("Calibration"),
-					tr("Using raw images for \"RealSense\" driver is not yet supported. "
+					tr("Using raw images for \"MyntEye\" driver is not yet supported. "
-						"Factory calibration loaded from RealSense2 is used."), QMessageBox::Ok);
+						"Factory calibration loaded from MyntEye is used."), QMessageBox::Ok);
 			return 0;
 		}
 		else

guilib/src/ui/calibrationDialog.ui

@@ -30,7 +30,7 @@
     <number>0</number>
    </property>
    <item>
-    <layout class="QHBoxLayout" name="horizontalLayout_4" stretch="1,0">
+    <layout class="QHBoxLayout" name="horizontalLayout_4" stretch="1,1">
      <item>
       <layout class="QVBoxLayout" name="verticalLayout_4">
        <property name="spacing">
@@ -146,6 +146,15 @@
      </item>
      <item>
       <widget class="QScrollArea" name="scrollArea">
+       <property name="sizePolicy">
+        <sizepolicy hsizetype="Fixed" vsizetype="Expanding">
+         <horstretch>0</horstretch>
+         <verstretch>0</verstretch>
+        </sizepolicy>
+       </property>
+       <property name="verticalScrollBarPolicy">
+        <enum>Qt::ScrollBarAlwaysOff</enum>
+       </property>
        <property name="widgetResizable">
         <bool>true</bool>
        </property>
@@ -154,8 +163,8 @@
          <rect>
           <x>0</x>
           <y>0</y>
-          <width>387</width>
+          <width>357</width>
-          <height>826</height>
+          <height>820</height>
          </rect>
         </property>
         <layout class="QVBoxLayout" name="verticalLayout">

guilib/src/ui/exportCloudsDialog.ui

@@ -23,26 +23,19 @@
       <property name="geometry">
        <rect>
         <x>0</x>
-        <y>-3537</y>
+        <y>-2140</y>
         <width>998</width>
-        <height>5673</height>
+        <height>5850</height>
        </rect>
       </property>
       <layout class="QVBoxLayout" name="verticalLayout_13">
        <item>
         <layout class="QGridLayout" name="gridLayout_8" columnstretch="0,1">
-         <item row="1" column="0">
+         <item row="17" column="0">
-          <widget class="QComboBox" name="comboBox_pipeline">
+          <widget class="QCheckBox" name="checkBox_cameraProjection">
-           <item>
+           <property name="text">
-            <property name="text">
+            <string/>
-             <string>Organized Point Cloud</string>
+           </property>
-            </property>
-           </item>
-           <item>
-            <property name="text">
-             <string>Dense Point Cloud</string>
-            </property>
-           </item>
           </widget>
          </item>
          <item row="1" column="1">
@@ -52,44 +45,61 @@
            </property>
           </widget>
          </item>
-         <item row="11" column="0">
+         <item row="14" column="0">
-          <widget class="QCheckBox" name="checkBox_regenerate">
-           <property name="text">
-            <string/>
-           </property>
-          </widget>
-         </item>
-         <item row="12" column="0">
           <widget class="QCheckBox" name="checkBox_filtering">
            <property name="text">
             <string/>
            </property>
           </widget>
          </item>
-         <item row="11" column="1">
+         <item row="13" column="0">
-          <widget class="QLabel" name="label_regenerate">
+          <widget class="QCheckBox" name="checkBox_regenerate">
            <property name="text">
-            <string>Regenerate clouds. This can be used to regenerate the point clouds at higher density than those used for online visualization.</string>
+            <string/>
+           </property>
+          </widget>
+         </item>
+         <item row="18" column="1">
+          <widget class="QLabel" name="label_binaryFile_12">
+           <property name="text">
+            <string>Meshing.</string>
            </property>
            <property name="wordWrap">
             <bool>true</bool>
            </property>
           </widget>
          </item>
-         <item row="8" column="1">
+         <item row="14" column="1">
-          <widget class="QLabel" name="label_voxel">
+          <widget class="QLabel" name="label_binaryFile_9">
            <property name="text">
-            <string>Voxel size. Set 0 to disable. When organized meshes are assembled, this is the radius in which the vertices of the polygons are merged.</string>
+            <string>Cloud filtering.</string>
            </property>
            <property name="wordWrap">
             <bool>true</bool>
            </property>
           </widget>
          </item>
-         <item row="4" column="1">
+         <item row="7" column="1">
-          <widget class="QLabel" name="label_binaryFile_2">
+          <widget class="QLabel" name="label_normal_2">
            <property name="text">
-            <string>Assemble clouds/meshes to a single output cloud/mesh.</string>
+            <string>Set the search radius for the normal estimation.</string>
+           </property>
+           <property name="wordWrap">
+            <bool>true</bool>
+           </property>
+          </widget>
+         </item>
+         <item row="18" column="0">
+          <widget class="QCheckBox" name="checkBox_meshing">
+           <property name="text">
+            <string/>
+           </property>
+          </widget>
+         </item>
+         <item row="16" column="1">
+          <widget class="QLabel" name="label_gainCompensation">
+           <property name="text">
+            <string>Gain compensation. Normalize brightness of images.</string>
            </property>
            <property name="wordWrap">
             <bool>true</bool>
@@ -106,54 +116,60 @@
            </property>
           </widget>
          </item>
-         <item row="6" column="0">
+         <item row="4" column="0">
-          <widget class="QSpinBox" name="spinBox_normalKSearch">
+          <widget class="QCheckBox" name="checkBox_assemble">
-           <property name="minimum">
-            <number>0</number>
-           </property>
-           <property name="value">
-            <number>20</number>
-           </property>
-          </widget>
-         </item>
-         <item row="13" column="0">
-          <widget class="QCheckBox" name="checkBox_smoothing">
            <property name="text">
             <string/>
            </property>
           </widget>
          </item>
-         <item row="12" column="1">
+         <item row="3" column="0">
-          <widget class="QLabel" name="label_binaryFile_9">
+          <widget class="QCheckBox" name="checkBox_binary">
            <property name="text">
-            <string>Cloud filtering.</string>
+            <string/>
+           </property>
+           <property name="checked">
+            <bool>true</bool>
+           </property>
+          </widget>
+         </item>
+         <item row="10" column="0">
+          <widget class="QSpinBox" name="spinBox_randomSamples_assembled">
+           <property name="minimum">
+            <number>0</number>
+           </property>
+           <property name="maximum">
+            <number>99999999</number>
+           </property>
+           <property name="singleStep">
+            <number>10000</number>
+           </property>
+           <property name="value">
+            <number>0</number>
+           </property>
+          </widget>
+         </item>
+         <item row="6" column="1">
+          <widget class="QLabel" name="label_normal">
+           <property name="text">
+            <string>Set the number of k nearest neighbors to use for the normal estimation. Set 0 to disable.</string>
            </property>
            <property name="wordWrap">
             <bool>true</bool>
            </property>
           </widget>
          </item>
-         <item row="3" column="1">
+         <item row="0" column="1">
-          <widget class="QLabel" name="label_binaryFile">
+          <widget class="QLabel" name="label_binaryFile_11">
            <property name="text">
-            <string>Binary file (for ply and pcd outputs).</string>
+            <string>From RGB-D images. If not checked, clouds will be generated from laser scans.</string>
            </property>
            <property name="wordWrap">
             <bool>true</bool>
            </property>
           </widget>
          </item>
-         <item row="13" column="1">
+         <item row="9" column="0">
-          <widget class="QLabel" name="label_smoothing">
-           <property name="text">
-            <string>Cloud smoothing using Moving Least Squares algorithm (MLS).</string>
-           </property>
-           <property name="wordWrap">
-            <bool>true</bool>
-           </property>
-          </widget>
-         </item>
-         <item row="8" column="0">
           <widget class="QDoubleSpinBox" name="doubleSpinBox_voxelSize_assembled">
            <property name="suffix">
             <string> m</string>
@@ -172,17 +188,110 @@
            </property>
           </widget>
          </item>
-         <item row="4" column="0">
+         <item row="0" column="0">
-          <widget class="QCheckBox" name="checkBox_assemble">
+          <widget class="QCheckBox" name="checkBox_fromDepth">
+           <property name="text">
+            <string/>
+           </property>
+           <property name="checked">
+            <bool>true</bool>
+           </property>
+          </widget>
+         </item>
+         <item row="6" column="0">
+          <widget class="QSpinBox" name="spinBox_normalKSearch">
+           <property name="minimum">
+            <number>0</number>
+           </property>
+           <property name="value">
+            <number>20</number>
+           </property>
+          </widget>
+         </item>
+         <item row="15" column="0">
+          <widget class="QCheckBox" name="checkBox_smoothing">
            <property name="text">
             <string/>
            </property>
           </widget>
          </item>
-         <item row="6" column="1">
+         <item row="1" column="0">
-          <widget class="QLabel" name="label_normal">
+          <widget class="QComboBox" name="comboBox_pipeline">
+           <item>
+            <property name="text">
+             <string>Organized Point Cloud</string>
+            </property>
+           </item>
+           <item>
+            <property name="text">
+             <string>Dense Point Cloud</string>
+            </property>
+           </item>
+          </widget>
+         </item>
+         <item row="15" column="1">
+          <widget class="QLabel" name="label_smoothing">
            <property name="text">
-            <string>Set the number of k nearest neighbors to use for the normal estimation. Set 0 to disable.</string>
+            <string>Cloud smoothing using Moving Least Squares algorithm (MLS).</string>
+           </property>
+           <property name="wordWrap">
+            <bool>true</bool>
+           </property>
+          </widget>
+         </item>
+         <item row="11" column="1">
+          <widget class="QLabel" name="label_intensityColormap">
+           <property name="text">
+            <string>Intensity colormap.</string>
+           </property>
+           <property name="wordWrap">
+            <bool>true</bool>
+           </property>
+          </widget>
+         </item>
+         <item row="9" column="1">
+          <widget class="QLabel" name="label_voxel">
+           <property name="text">
+            <string>Voxel size. Set 0 to disable. When organized meshes are assembled, this is the radius in which the vertices of the polygons are merged.</string>
+           </property>
+           <property name="wordWrap">
+            <bool>true</bool>
+           </property>
+          </widget>
+         </item>
+         <item row="11" column="0">
+          <widget class="QComboBox" name="comboBox_intensityColormap">
+           <item>
+            <property name="text">
+             <string>GrayScale</string>
+            </property>
+           </item>
+           <item>
+            <property name="text">
+             <string>RedYellow</string>
+            </property>
+           </item>
+           <item>
+            <property name="text">
+             <string>Rainbow</string>
+            </property>
+           </item>
+          </widget>
+         </item>
+         <item row="17" column="1">
+          <widget class="QLabel" name="label_cameraProjection">
+           <property name="text">
+            <string>Camera projection. This can be used to colorize point cloud created from scans and/or export camera IDs for each point of the cloud.</string>
+           </property>
+           <property name="wordWrap">
+            <bool>true</bool>
+           </property>
+          </widget>
+         </item>
+         <item row="12" column="1">
+          <widget class="QLabel" name="label_binaryFile_10">
+           <property name="text">
+            <string>Nodes filtering. Filter nodes to be exported in a specified region .</string>
            </property>
            <property name="wordWrap">
             <bool>true</bool>
@@ -213,46 +322,6 @@
            </item>
           </widget>
          </item>
-         <item row="3" column="0">
-          <widget class="QCheckBox" name="checkBox_binary">
-           <property name="text">
-            <string/>
-           </property>
-           <property name="checked">
-            <bool>true</bool>
-           </property>
-          </widget>
-         </item>
-         <item row="0" column="1">
-          <widget class="QLabel" name="label_binaryFile_11">
-           <property name="text">
-            <string>From RGB-D images. If not checked, clouds will be generated from laser scans.</string>
-           </property>
-           <property name="wordWrap">
-            <bool>true</bool>
-           </property>
-          </widget>
-         </item>
-         <item row="0" column="0">
-          <widget class="QCheckBox" name="checkBox_fromDepth">
-           <property name="text">
-            <string/>
-           </property>
-           <property name="checked">
-            <bool>true</bool>
-           </property>
-          </widget>
-         </item>
-         <item row="7" column="1">
-          <widget class="QLabel" name="label_normal_2">
-           <property name="text">
-            <string>Set the search radius for the normal estimation.</string>
-           </property>
-           <property name="wordWrap">
-            <bool>true</bool>
-           </property>
-          </widget>
-         </item>
          <item row="7" column="0">
           <widget class="QDoubleSpinBox" name="doubleSpinBox_normalRadiusSearch">
            <property name="singleStep">
@@ -260,103 +329,80 @@
            </property>
           </widget>
          </item>
-         <item row="9" column="1">
+         <item row="16" column="0">
-          <widget class="QLabel" name="label_intensityColormap">
-           <property name="text">
-            <string>Intensity colormap.</string>
-           </property>
-           <property name="wordWrap">
-            <bool>true</bool>
-           </property>
-          </widget>
-         </item>
-         <item row="9" column="0">
-          <widget class="QComboBox" name="comboBox_intensityColormap">
-           <item>
-            <property name="text">
-             <string>GrayScale</string>
-            </property>
-           </item>
-           <item>
-            <property name="text">
-             <string>RedYellow</string>
-            </property>
-           </item>
-           <item>
-            <property name="text">
-             <string>Rainbow</string>
-            </property>
-           </item>
-          </widget>
-         </item>
-         <item row="14" column="1">
-          <widget class="QLabel" name="label_gainCompensation">
-           <property name="text">
-            <string>Gain compensation. Normalize brightness of images.</string>
-           </property>
-           <property name="wordWrap">
-            <bool>true</bool>
-           </property>
-          </widget>
-         </item>
-         <item row="15" column="1">
-          <widget class="QLabel" name="label_cameraProjection">
-           <property name="text">
-            <string>Camera projection. This can be used to colorize point cloud created from scans and/or export camera IDs for each point of the cloud.</string>
-           </property>
-           <property name="wordWrap">
-            <bool>true</bool>
-           </property>
-          </widget>
-         </item>
-         <item row="14" column="0">
           <widget class="QCheckBox" name="checkBox_gainCompensation">
            <property name="text">
             <string/>
            </property>
           </widget>
          </item>
-         <item row="15" column="0">
-          <widget class="QCheckBox" name="checkBox_cameraProjection">
-           <property name="text">
-            <string/>
-           </property>
-          </widget>
-         </item>
-         <item row="16" column="1">
-          <widget class="QLabel" name="label_binaryFile_12">
-           <property name="text">
-            <string>Meshing.</string>
-           </property>
-           <property name="wordWrap">
-            <bool>true</bool>
-           </property>
-          </widget>
-         </item>
-         <item row="16" column="0">
-          <widget class="QCheckBox" name="checkBox_meshing">
-           <property name="text">
-            <string/>
-           </property>
-          </widget>
-         </item>
          <item row="10" column="1">
-          <widget class="QLabel" name="label_binaryFile_10">
+          <widget class="QLabel" name="label_voxel_2">
            <property name="text">
-            <string>Nodes filtering. Filter nodes to be exported in a specified region .</string>
+            <string>Number of samples to keep, done with a random sample filter. Only used when clouds are assembled. Set 0 to disable.</string>
            </property>
            <property name="wordWrap">
             <bool>true</bool>
            </property>
           </widget>
          </item>
-         <item row="10" column="0">
+         <item row="3" column="1">
+          <widget class="QLabel" name="label_binaryFile">
+           <property name="text">
+            <string>Binary file (for ply and pcd outputs).</string>
+           </property>
+           <property name="wordWrap">
+            <bool>true</bool>
+           </property>
+          </widget>
+         </item>
+         <item row="13" column="1">
+          <widget class="QLabel" name="label_regenerate">
+           <property name="text">
+            <string>Regenerate clouds. This can be used to regenerate the point clouds at higher density than those used for online visualization.</string>
+           </property>
+           <property name="wordWrap">
+            <bool>true</bool>
+           </property>
+          </widget>
+         </item>
+         <item row="12" column="0">
           <widget class="QCheckBox" name="checkBox_nodes_filtering">
            <property name="text">
             <string/>
            </property>
           </widget>
          </item>
+         <item row="4" column="1">
+          <widget class="QLabel" name="label_binaryFile_2">
+           <property name="text">
+            <string>Assemble clouds/meshes to a single output cloud/mesh.</string>
+           </property>
+           <property name="wordWrap">
+            <bool>true</bool>
+           </property>
+          </widget>
+         </item>
+         <item row="8" column="1">
+          <widget class="QLabel" name="label_normal_3">
+           <property name="text">
+            <string>Flip ground normals up if close to -z axis. Set to 0.9 to begin with, increase to limit normals very close to -z axis. Set 0 to disable.</string>
+           </property>
+           <property name="wordWrap">
+            <bool>true</bool>
+           </property>
+          </widget>
+         </item>
+         <item row="8" column="0">
+          <widget class="QDoubleSpinBox" name="doubleSpinBox_groundNormalsUp">
+           <property name="maximum">
+            <double>1.000000000000000</double>
+           </property>
+           <property name="singleStep">
+            <double>0.010000000000000</double>
+           </property>
+          </widget>
+         </item>
         </layout>
        </item>
        <item>
@@ -1824,39 +1870,7 @@ Guidelines: 4 times the voxel size, 0.025 for voxel=0.</string>
           <bool>false</bool>
          </property>
          <layout class="QGridLayout" name="gridLayout_20" columnstretch="0,0,0,1">
-          <item row="3" column="1">
+          <item row="6" column="1">
-           <widget class="QDoubleSpinBox" name="doubleSpinBox_camProjMaxAngle">
-            <property name="suffix">
-             <string> deg</string>
-            </property>
-            <property name="decimals">
-             <number>0</number>
-            </property>
-            <property name="minimum">
-             <double>0.000000000000000</double>
-            </property>
-            <property name="maximum">
-             <double>180.000000000000000</double>
-            </property>
-            <property name="singleStep">
-             <double>1.000000000000000</double>
-            </property>
-            <property name="value">
-             <double>0.000000000000000</double>
-            </property>
-           </widget>
-          </item>
-          <item row="5" column="2" colspan="2">
-           <widget class="QLabel" name="label_meshingTextureSize_15">
-            <property name="text">
-             <string>Keep points not seen by the cameras. These points will be set with a pure red color (255,0,0) if the cloud was created from laser scans.</string>
-            </property>
-            <property name="wordWrap">
-             <bool>true</bool>
-            </property>
-           </widget>
-          </item>
-          <item row="4" column="1">
            <widget class="QCheckBox" name="checkBox_camProjDistanceToCamPolicy">
             <property name="text">
              <string/>
@@ -1864,9 +1878,19 @@ Guidelines: 4 times the voxel size, 0.025 for voxel=0.</string>
            </widget>
           </item>
           <item row="4" column="2" colspan="2">
-           <widget class="QLabel" name="label_meshingTextureSize_14">
+           <widget class="QLabel" name="label_meshingTextureSize_16">
             <property name="text">
-             <string>Distance to camera policy. The closest camera from a point is used to colorize the point. If disabled, the camera for which the point projection is the closest of the image center is used to colorize the point.</string>
+             <string>Decimation of camera resolution before projection. This can help to correctly estimate points hidden by other points, in case the point cloud is sparse.</string>
+            </property>
+            <property name="wordWrap">
+             <bool>true</bool>
+            </property>
+           </widget>
+          </item>
+          <item row="9" column="2" colspan="2">
+           <widget class="QLabel" name="label_camProjExportCamera">
+            <property name="text">
+             <string>ID format of the camera selected for each point.</string>
             </property>
             <property name="wordWrap">
              <bool>true</bool>
@@ -1883,6 +1907,35 @@ Guidelines: 4 times the voxel size, 0.025 for voxel=0.</string>
             </property>
            </widget>
           </item>
+          <item row="4" column="1">
+           <widget class="QSpinBox" name="spinBox_camProjDecimation">
+            <property name="suffix">
+             <string/>
+            </property>
+            <property name="minimum">
+             <number>1</number>
+            </property>
+            <property name="maximum">
+             <number>9999</number>
+            </property>
+            <property name="singleStep">
+             <number>1</number>
+            </property>
+            <property name="value">
+             <number>1</number>
+            </property>
+           </widget>
+          </item>
+          <item row="1" column="2" colspan="2">
+           <widget class="QLabel" name="label_meshingTextureSize_17">
+            <property name="text">
+             <string>ROI ratios [left right top bottom] between 0 and 1. This can be used to ignore black borders of RGB images caused by calibration. </string>
+            </property>
+            <property name="wordWrap">
+             <bool>true</bool>
+            </property>
+           </widget>
+          </item>
           <item row="3" column="2" colspan="2">
            <widget class="QLabel" name="label_meshingTextureSize_12">
             <property name="text">
@@ -1893,46 +1946,24 @@ Guidelines: 4 times the voxel size, 0.025 for voxel=0.</string>
             </property>
            </widget>
           </item>
-          <item row="2" column="1">
+          <item row="8" column="1">
-           <widget class="QDoubleSpinBox" name="doubleSpinBox_camProjMaxDistance">
+           <widget class="QCheckBox" name="checkBox_camProjRecolorPoints">
-            <property name="suffix">
+            <property name="text">
-             <string> m</string>
+             <string/>
-            </property>
-            <property name="decimals">
-             <number>1</number>
-            </property>
-            <property name="minimum">
-             <double>0.000000000000000</double>
-            </property>
-            <property name="maximum">
-             <double>99.000000000000000</double>
-            </property>
-            <property name="singleStep">
-             <double>1.000000000000000</double>
-            </property>
-            <property name="value">
-             <double>0.000000000000000</double>
             </property>
            </widget>
           </item>
           <item row="5" column="1">
+           <widget class="QLineEdit" name="lineEdit_camProjMaskFilePath"/>
+          </item>
+          <item row="7" column="1">
            <widget class="QCheckBox" name="checkBox_camProjKeepPointsNotSeenByCameras">
             <property name="text">
              <string/>
             </property>
            </widget>
           </item>
-          <item row="7" column="2" colspan="2">
+          <item row="9" column="1">
-           <widget class="QLabel" name="label_camProjExportCamera">
-            <property name="text">
-             <string>ID format of the camera selected for each point.</string>
-            </property>
-            <property name="wordWrap">
-             <bool>true</bool>
-            </property>
-           </widget>
-          </item>
-          <item row="7" column="1">
            <widget class="QComboBox" name="comboBox_camProjExportCamera">
             <property name="toolTip">
              <string>By Node ID: cameras of same node have same ID
@@ -1961,20 +1992,74 @@ By Node ID and Camera Index: NodeID*10+CameraIndex</string>
             </item>
            </widget>
           </item>
-          <item row="1" column="1">
+          <item row="6" column="2" colspan="2">
-           <widget class="QLineEdit" name="lineEdit_camProjRoiRatios"/>
+           <widget class="QLabel" name="label_meshingTextureSize_14">
-          </item>
-          <item row="1" column="2" colspan="2">
-           <widget class="QLabel" name="label_meshingTextureSize_17">
             <property name="text">
-             <string>ROI ratios [left right top bottom] between 0 and 1. This can be used to ignore black borders of RGB images caused by calibration. </string>
+             <string>Distance to camera policy. The closest camera from a point is used to colorize the point. If disabled, the camera for which the point projection is the closest of the image center is used to colorize the point.</string>
             </property>
             <property name="wordWrap">
              <bool>true</bool>
             </property>
            </widget>
           </item>
-          <item row="6" column="2">
+          <item row="1" column="1">
+           <widget class="QLineEdit" name="lineEdit_camProjRoiRatios"/>
+          </item>
+          <item row="3" column="1">
+           <widget class="QDoubleSpinBox" name="doubleSpinBox_camProjMaxAngle">
+            <property name="suffix">
+             <string> deg</string>
+            </property>
+            <property name="decimals">
+             <number>0</number>
+            </property>
+            <property name="minimum">
+             <double>0.000000000000000</double>
+            </property>
+            <property name="maximum">
+             <double>180.000000000000000</double>
+            </property>
+            <property name="singleStep">
+             <double>1.000000000000000</double>
+            </property>
+            <property name="value">
+             <double>0.000000000000000</double>
+            </property>
+           </widget>
+          </item>
+          <item row="2" column="1">
+           <widget class="QDoubleSpinBox" name="doubleSpinBox_camProjMaxDistance">
+            <property name="suffix">
+             <string> m</string>
+            </property>
+            <property name="decimals">
+             <number>1</number>
+            </property>
+            <property name="minimum">
+             <double>0.000000000000000</double>
+            </property>
+            <property name="maximum">
+             <double>99.000000000000000</double>
+            </property>
+            <property name="singleStep">
+             <double>1.000000000000000</double>
+            </property>
+            <property name="value">
+             <double>0.000000000000000</double>
+            </property>
+           </widget>
+          </item>
+          <item row="7" column="2" colspan="2">
+           <widget class="QLabel" name="label_meshingTextureSize_15">
+            <property name="text">
+             <string>Keep points not seen by the cameras. These points will be set with a pure red color (255,0,0) if the cloud was created from laser scans.</string>
+            </property>
+            <property name="wordWrap">
+             <bool>true</bool>
+            </property>
+           </widget>
+          </item>
+          <item row="8" column="2">
            <widget class="QLabel" name="label_meshingTextureSize_18">
             <property name="text">
              <string>Recolor points from camera projection. This would be used to color laser scans with the cameras.</string>
@@ -1984,10 +2069,20 @@ By Node ID and Camera Index: NodeID*10+CameraIndex</string>
             </property>
            </widget>
           </item>
-          <item row="6" column="1">
+          <item row="5" column="2" colspan="2">
-           <widget class="QCheckBox" name="checkBox_camProjRecolorPoints">
+           <widget class="QLabel" name="label_meshingTextureSize_19">
             <property name="text">
-             <string/>
+             <string>File path for a mask. Format should be 8-bits grayscale. The mask should cover all cameras in case multi-camera is used and have the same resolution.</string>
+            </property>
+            <property name="wordWrap">
+             <bool>true</bool>
+            </property>
+           </widget>
+          </item>
+          <item row="5" column="0">
+           <widget class="QToolButton" name="toolButton_camProjMaskFilePath">
+            <property name="text">
+             <string>...</string>
             </property>
            </widget>
           </item>

guilib/src/ui/mainWindow.ui

@@ -229,8 +229,19 @@
        </property>
        <addaction name="actionRealSense2_L515"/>
       </widget>
+      <widget class="QMenu" name="menuOrbbec_Astra">
+       <property name="title">
+        <string>Orbbec Astra</string>
+       </property>
+       <property name="icon">
+        <iconset resource="../GuiLib.qrc">
+         <normaloff>:/images/astra.png</normaloff>:/images/astra.png</iconset>
+       </property>
+       <addaction name="actionOpenNI2_orbbec"/>
+      </widget>
       <addaction name="menuKinect_for_Xbox_360"/>
       <addaction name="menuXtion_PRO_LIVE"/>
+      <addaction name="menuOrbbec_Astra"/>
       <addaction name="menuSense_3D_scanner"/>
       <addaction name="menuKinect_v2"/>
       <addaction name="menuKinect_K4A"/>
@@ -340,6 +351,7 @@
     <addaction name="actionTrigger_a_new_map"/>
     <addaction name="separator"/>
     <addaction name="actionLabel_current_location"/>
+    <addaction name="actionRemove_label"/>
     <addaction name="actionSend_waypoints"/>
     <addaction name="actionSend_goal"/>
     <addaction name="actionCancel_goal"/>
@@ -1651,6 +1663,19 @@
     <string>RGBD-SLAM + ID format (*.txt)</string>
    </property>
   </action>
+  <action name="actionOpenNI2_orbbec">
+   <property name="checkable">
+    <bool>true</bool>
+   </property>
+   <property name="text">
+    <string>OpenNI2</string>
+   </property>
+  </action>
+  <action name="actionRemove_label">
+   <property name="text">
+    <string>Remove label...</string>
+   </property>
+  </action>
  </widget>
  <customwidgets>
   <customwidget>

guilib/src/ui/preferencesDialog.ui

@@ -63,7 +63,7 @@
           <property name="geometry">
            <rect>
             <x>0</x>
-            <y>-650</y>
+            <y>0</y>
             <width>756</width>
             <height>3623</height>
            </rect>
@@ -95,7 +95,7 @@
                 <enum>QFrame::Raised</enum>
                </property>
                <property name="currentIndex">
-                <number>12</number>
+                <number>13</number>
                </property>
                <widget class="QWidget" name="page_22">
                 <layout class="QVBoxLayout" name="verticalLayout_29" stretch="0,0">
@@ -11396,7 +11396,7 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag
                            </property>
                           </widget>
                          </item>
-                         <item row="4" column="1">
+                         <item row="5" column="1">
                           <widget class="QLabel" name="label_scanMatching_13">
                            <property name="text">
                             <string>Use odometry as motion guess for one-to-one proximity detection.</string>
@@ -11419,7 +11419,7 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag
                            </property>
                           </widget>
                          </item>
-                         <item row="3" column="0">
+                         <item row="4" column="0">
                           <widget class="QDoubleSpinBox" name="localDetection_angle">
                            <property name="suffix">
                             <string> deg</string>
@@ -11435,14 +11435,14 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag
                            </property>
                           </widget>
                          </item>
-                         <item row="4" column="0">
+                         <item row="5" column="0">
                           <widget class="QCheckBox" name="checkBox_localSpaceOdomGuess">
                            <property name="text">
                             <string/>
                            </property>
                           </widget>
                          </item>
-                         <item row="5" column="1">
+                         <item row="6" column="1">
                           <widget class="QLabel" name="label_space3_3">
                            <property name="text">
                             <string>Path filtering radius to reduce the number of nodes to compare in a path in one-to-many proximity detection. The nearest node in a path should be inside that radius to be considered for one-to-one proximity detection.</string>
@@ -11478,7 +11478,7 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag
                            </property>
                           </widget>
                          </item>
-                         <item row="6" column="1">
+                         <item row="7" column="1">
                           <widget class="QLabel" name="label_scanMatching_4">
                            <property name="text">
                             <string>When comparing to a local path for one-to-many proximity detection, merge the scans using the odometry poses (with neighbor link optimizations) instead of the ones in the optimized local graph.</string>
@@ -11504,7 +11504,7 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag
                            </property>
                           </widget>
                          </item>
-                         <item row="5" column="0">
+                         <item row="6" column="0">
                           <widget class="QDoubleSpinBox" name="localDetection_pathFilteringRadius">
                            <property name="suffix">
                             <string> m</string>
@@ -11530,14 +11530,14 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag
                            </property>
                           </widget>
                          </item>
-                         <item row="6" column="0">
+                         <item row="7" column="0">
                           <widget class="QCheckBox" name="checkBox_localSpacePathOdomPosesUsed">
                            <property name="text">
                             <string/>
                            </property>
                           </widget>
                          </item>
-                         <item row="8" column="1">
+                         <item row="9" column="1">
                           <widget class="QLabel" name="label_scanMatching_6">
                            <property name="text">
                             <string>Save scan matching IDs from one-to-many proximity detection in link's user data.</string>
@@ -11550,7 +11550,7 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag
                            </property>
                           </widget>
                          </item>
-                         <item row="3" column="1">
+                         <item row="4" column="1">
                           <widget class="QLabel" name="label_scanMatching_8">
                            <property name="text">
                             <string>Maximum angle (degrees) for one-to-one proximity detection.</string>
@@ -11563,14 +11563,14 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag
                            </property>
                           </widget>
                          </item>
-                         <item row="8" column="0">
+                         <item row="9" column="0">
                           <widget class="QCheckBox" name="checkBox_localSpaceScanMatchingIDsSaved">
                            <property name="text">
                             <string/>
                            </property>
                           </widget>
                          </item>
-                         <item row="9" column="1">
+                         <item row="10" column="1">
                           <widget class="QLabel" name="label_scanMatching_15">
                            <property name="text">
                             <string>Create a global assembled map from laser scans for one-to-many proximity detection, replacing the original one-to-many proximity detection (i.e., detection against local paths). Only used in localization mode, otherwise original one-to-many proximity detection is done. Note also that if graph is modified (i.e., memory management is enabled or robot jumps from one disjoint session to another in same database), the global scan map is cleared and one-to-many proximity detection is reverted to original approach.</string>
@@ -11583,13 +11583,48 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag
                            </property>
                           </widget>
                          </item>
-                         <item row="9" column="0">
+                         <item row="10" column="0">
                           <widget class="QCheckBox" name="checkBox_localSpaceCreateGlobalScanMap">
                            <property name="text">
                             <string/>
                            </property>
                           </widget>
                          </item>
+                         <item row="3" column="1">
+                          <widget class="QLabel" name="label_space3_9">
+                           <property name="text">
+                            <string>Covariance factor for one-to-many proximity detection (when Maximum neighbor nodes&gt;0 and scans are used).</string>
+                           </property>
+                           <property name="wordWrap">
+                            <bool>true</bool>
+                           </property>
+                           <property name="textInteractionFlags">
+                            <set>Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse</set>
+                           </property>
+                          </widget>
+                         </item>
+                         <item row="3" column="0">
+                          <widget class="QDoubleSpinBox" name="localDetection_mergedScanCovFactor">
+                           <property name="suffix">
+                            <string/>
+                           </property>
+                           <property name="decimals">
+                            <number>0</number>
+                           </property>
+                           <property name="minimum">
+                            <double>1.000000000000000</double>
+                           </property>
+                           <property name="maximum">
+                            <double>10000.000000000000000</double>
+                           </property>
+                           <property name="singleStep">
+                            <double>10.000000000000000</double>
+                           </property>
+                           <property name="value">
+                            <double>100.000000000000000</double>
+                           </property>
+                          </widget>
+                         </item>
                         </layout>
                        </item>
                       </layout>
@@ -21284,7 +21319,7 @@ Lower the ratio -&gt; higher the precision.</string>
                            <item row="2" column="0">
                             <widget class="QSpinBox" name="stereobm_minDisparity">
                              <property name="minimum">
-                              <number>0</number>
+                              <number>-999999</number>
                              </property>
                              <property name="maximum">
                               <number>999999</number>
@@ -21600,7 +21635,7 @@ Lower the ratio -&gt; higher the precision.</string>
                            <item row="2" column="0">
                             <widget class="QSpinBox" name="stereosgbm_minDisparity">
                              <property name="minimum">
-                              <number>0</number>
+                              <number>-999999</number>
                              </property>
                              <property name="maximum">
                               <number>999999</number>

package.xml

@@ -1,7 +1,7 @@
 <?xml version="1.0"?>
-<package>
+<package format="2">
   <name>rtabmap</name>
-  <version>0.20.16</version>
+  <version>0.20.18</version>
   <description>RTAB-Map's standalone library. RTAB-Map is a RGB-D SLAM approach with real-time constraints.</description>
   <maintainer email="matlabbe@gmail.com">Mathieu Labbe</maintainer>
   <author>Mathieu Labbe</author>
@@ -11,32 +11,22 @@
   <url type="repository">https://github.com/introlab/rtabmap</url>
 
   <buildtool_depend>cmake</buildtool_depend>
-  
+
-  <build_depend>qt_gui_cpp</build_depend> <!-- libqt4-dev or libqt5-dev -->
-  <build_depend>libpcl-all-dev</build_depend> <!--  include libvtk-qt -->
-  <build_depend>libsqlite3-dev</build_depend>
-  <build_depend>zlib</build_depend>
-  <build_depend>libfreenect-dev</build_depend>
-  <build_depend>libopenni-dev</build_depend>
-  <!--<build_depend>libopenni2-dev</build_depend> --> <!-- not available on Jessie -->
-  <build_depend>cv_bridge</build_depend>
-  <!-- libproj-dev needed due to error in vtk6 (kinetic)-->
   <build_depend>proj</build_depend>
-  <build_depend>octomap</build_depend>
+  <depend>cv_bridge</depend>
-  <build_depend>libg2o</build_depend>
+  <depend>libfreenect-dev</depend>
-  
+  <depend>libg2o</depend>
-  <run_depend>qt_gui_cpp</run_depend>
+  <depend>libopenni-dev</depend>
-  <run_depend>libpcl-all-dev</run_depend> <!--  include libvtk-qt -->
+  <!-- <depend>libopenni2-dev</depend> --> <!-- not available on Jessie -->
-  <run_depend>libsqlite3-dev</run_depend>
+  <depend>libpcl-all-dev</depend> <!--  include libvtk-qt -->
-  <run_depend>zlib</run_depend>
+  <depend>libpointmatcher</depend>
-  <run_depend>libfreenect-dev</run_depend>
+  <!-- <depend>libproj-dev</depend> needed due to error in vtk6 (kinetic)-->
-  <run_depend>libopenni-dev</run_depend>
+  <depend>libsqlite3-dev</depend>
-  <!-- <run_depend>libopenni2-dev</run_depend> -->
+  <depend>octomap</depend>
-  <run_depend>cv_bridge</run_depend>
+  <depend>qt_gui_cpp</depend> <!-- libqt4-dev or libqt5-dev -->
-  <run_depend>octomap</run_depend>
+  <depend>zlib</depend>
-  <run_depend>libg2o</run_depend>
 
   <export>
-	 <build_type>cmake</build_type>
+	  <build_type>cmake</build_type>
   </export>
 </package>

tools/Export/main.cpp

@@ -58,7 +58,7 @@ void showUsage()
 			"Options:\n"
 			"    --output \"\"           Output name (default: name of the database is used).\n"
 			"    --output_dir \"\"       Output directory (default: same directory than the database).\n"
-			"    --bin                 Export PLY in binary format.\n"
+			"    --ascii               Export PLY in ascii format.\n"
 			"    --las                 Export cloud in LAS instead of PLY (PDAL dependency required).\n"
 			"    --mesh                Create a mesh.\n"
 			"    --texture             Create a mesh with texture.\n"
@@ -67,10 +67,12 @@ void showUsage()
 			"    --texture_range #     Maximum camera range for texturing a polygon (default 0 meters: no limit).\n"
 			"    --texture_angle #     Maximum camera angle for texturing a polygon (default 0 deg: no limit).\n"
 			"    --texture_depth_error # Maximum depth error between reprojected mesh and depth image to texture a face (-1=disabled, 0=edge length is used, default=0).\n"
+			"    --texture_roi_ratios \"# # # #\" Region of interest from images to texture or to color scans. Format is \"left right top bottom\" (e.g. \"0 0 0 0.1\" means 10%% of the image bottom not used).\n"
 			"    --texture_d2c         Distance to camera policy.\n"
 			"    --cam_projection      Camera projection on assembled cloud and export node ID on each point (in PointSourceId field).\n"
 			"    --cam_projection_keep_all  Keep not colored points from cameras (node ID will be 0 and color will be red).\n"
 			"    --cam_projection_decimation  Decimate images before projecting the points.\n"
+			"    --cam_projection_mask \"\"  File path for a mask. Format should be 8-bits grayscale. The mask should cover all cameras in case multi-camera is used and have the same resolution.\n"
 			"    --poses               Export optimized poses of the robot frame (e.g., base_link).\n"
 			"    --poses_camera        Export optimized poses of the camera frame (e.g., optical frame).\n"
 			"    --poses_scan          Export optimized poses of the scan frame.\n"
@@ -107,8 +109,12 @@ void showUsage()
 			"    --max_range     #     Maximum range of the created clouds (default 4 m, 0 m with --scan).\n"
 			"    --decimation    #     Depth image decimation before creating the clouds (default 4, 1 with --scan).\n"
 			"    --voxel         #     Voxel size of the created clouds (default 0.01 m, 0 m with --scan).\n"
+			"    --ground_normals_up  #  Flip ground normals up if close to -z axis (default 0, 0=disabled, value should be >0 and <1, typical 0.9).\n"
 			"    --noise_radius  #     Noise filtering search radius (default 0, 0=disabled).\n"
 			"    --noise_k       #     Noise filtering minimum neighbors in search radius (default 5, 0=disabled).\n"
+			"    --prop_radius_factor #  Proportional radius filter factor (default 0, 0=disabled). Start tuning from 0.01.\n"
+			"    --prop_radius_scale  #  Proportional radius filter neighbor scale (default 2).\n"
+			"    --random_samples #    Number of output samples using a random filter (default 0, 0=disabled).\n"
 			"    --color_radius  #     Radius used to colorize polygons (default 0.05 m, 0 m with --scan). Set 0 for nearest color.\n"
 			"    --scan                Use laser scan for the point cloud.\n"
 			"    --save_in_db          Save resulting assembled point cloud or mesh in the database.\n"
@@ -146,7 +152,7 @@ int main(int argc, char * argv[])
 		showUsage();
 	}
 
-	bool binary = false;
+	bool binary = true;
 	bool las = false;
 	bool mesh = false;
 	bool texture = false;
@@ -162,13 +168,18 @@ int main(int argc, char * argv[])
 	int decimation = -1;
 	float maxRange = -1.0f;
 	float voxelSize = -1.0f;
+	float groundNormalsUp = 0.0f;
 	float noiseRadius = 0.0f;
 	int noiseMinNeighbors = 5;
+	float proportionalRadiusFactor = 0.0f;
+	float proportionalRadiusScale = 2.0f;
+	int randomSamples = 0;
 	int textureSize = 8192;
 	int textureCount = 1;
 	float textureRange = 0;
 	float textureAngle = 0;
 	float textureDepthError = 0;
+	std::vector<float> textureRoiRatios;
 	bool distanceToCamPolicy = false;
 	bool multiband = false;
 	int multibandDownScale = 2;
@@ -187,6 +198,7 @@ int main(int argc, char * argv[])
 	bool camProjection = false;
 	bool camProjectionKeepAll = false;
 	int cameraProjDecimation = 1;
+	std::string cameraProjMask;
 	bool exportPoses = false;
 	bool exportPosesCamera = false;
 	bool exportPosesScan = false;
@@ -230,7 +242,11 @@ int main(int argc, char * argv[])
 		}
 		else if(std::strcmp(argv[i], "--bin") == 0)
 		{
-			binary = true;
+			printf("No need to set --bin anymore, ply are now automatically exported in binary by default. Set --ascii to export as text.\n");
+		}
+		else if(std::strcmp(argv[i], "--ascii") == 0)
+		{
+			binary = false;
 		}
 		else if(std::strcmp(argv[i], "--las") == 0)
 		{
@@ -310,6 +326,46 @@ int main(int argc, char * argv[])
 				showUsage();
 			}
 		}
+		else if(std::strcmp(argv[i], "--texture_roi_ratios") == 0)
+		{
+			++i;
+			if(i<argc-1)
+			{
+				std::list<std::string> strValues = uSplit(argv[i], ' ');
+				if(strValues.size() != 4)
+				{
+					printf("The number of values must be 4 (roi=\"%s\")\n", argv[i]);
+					showUsage();
+				}
+				else
+				{
+					std::vector<float> tmpValues(4);
+					unsigned int i=0;
+					for(std::list<std::string>::iterator jter = strValues.begin(); jter!=strValues.end(); ++jter)
+					{
+						tmpValues[i] = uStr2Float(*jter);
+						++i;
+					}
+
+					if(tmpValues[0] >= 0 && tmpValues[0] < 1 && tmpValues[0] < 1.0f-tmpValues[1] &&
+						tmpValues[1] >= 0 && tmpValues[1] < 1 && tmpValues[1] < 1.0f-tmpValues[0] &&
+						tmpValues[2] >= 0 && tmpValues[2] < 1 && tmpValues[2] < 1.0f-tmpValues[3] &&
+						tmpValues[3] >= 0 && tmpValues[3] < 1 && tmpValues[3] < 1.0f-tmpValues[2])
+					{
+						textureRoiRatios = tmpValues;
+					}
+					else
+					{
+						printf("The roi ratios are not valid (roi=\"%s\")\n", argv[i]);
+						showUsage();
+					}
+				}
+			}
+			else
+			{
+				showUsage();
+			}
+		}
 		else if(std::strcmp(argv[i], "--texture_d2c") == 0)
 		{
 			distanceToCamPolicy = true;
@@ -339,6 +395,23 @@ int main(int argc, char * argv[])
 				showUsage();
 			}
 		}
+		else if(std::strcmp(argv[i], "--cam_projection_mask") == 0)
+		{
+			++i;
+			if(i<argc-1)
+			{
+				cameraProjMask = argv[i];
+				if(!UFile::exists(cameraProjMask))
+				{
+					printf("--cam_projection_mask is set with a file not existing or don't have permissions to open it. Path=\"%s\"\n", argv[i]);
+					showUsage();
+				}
+			}
+			else
+			{
+				showUsage();
+			}
+		}
 		else if(std::strcmp(argv[i], "--poses") == 0)
 		{
 			exportPoses = true;
@@ -578,6 +651,18 @@ int main(int argc, char * argv[])
 				showUsage();
 			}
 		}
+		else if(std::strcmp(argv[i], "--ground_normals_up") == 0)
+		{
+			++i;
+			if(i<argc-1)
+			{
+				groundNormalsUp = uStr2Float(argv[i]);
+			}
+			else
+			{
+				showUsage();
+			}
+		}
 		else if(std::strcmp(argv[i], "--noise_radius") == 0)
 		{
 			++i;
@@ -602,6 +687,43 @@ int main(int argc, char * argv[])
 				showUsage();
 			}
 		}
+		else if(std::strcmp(argv[i], "--prop_radius_factor") == 0)
+		{
+			++i;
+			if(i<argc-1)
+			{
+				proportionalRadiusFactor = uStr2Float(argv[i]);
+			}
+			else
+			{
+				showUsage();
+			}
+		}
+		else if(std::strcmp(argv[i], "--prop_radius_scale") == 0)
+		{
+			++i;
+			if(i<argc-1)
+			{
+				proportionalRadiusScale = uStr2Float(argv[i]);
+				UASSERT_MSG(proportionalRadiusScale>=1.0f, "--prop_radius_scale should be >= 1.0");
+			}
+			else
+			{
+				showUsage();
+			}
+		}
+		else if(std::strcmp(argv[i], "--random_samples") == 0)
+		{
+			++i;
+			if(i<argc-1)
+			{
+				randomSamples = uStr2Int(argv[i]);
+			}
+			else
+			{
+				showUsage();
+			}
+		}
 		else if(std::strcmp(argv[i], "--color_radius") == 0)
 		{
 			++i;
@@ -893,8 +1015,8 @@ int main(int argc, char * argv[])
 
 	// Construct the cloud
 	printf("Create and assemble the clouds...\n");
-	pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr mergedClouds(new pcl::PointCloud<pcl::PointXYZRGBNormal>);
+	pcl::PointCloud<pcl::PointXYZRGB>::Ptr assembledCloud(new pcl::PointCloud<pcl::PointXYZRGB>);
-	pcl::PointCloud<pcl::PointXYZINormal>::Ptr mergedCloudsI(new pcl::PointCloud<pcl::PointXYZINormal>);
+	pcl::PointCloud<pcl::PointXYZI>::Ptr assembledCloudI(new pcl::PointCloud<pcl::PointXYZI>);
 	std::map<int, rtabmap::Transform> robotPoses;
 	std::vector<std::map<int, rtabmap::Transform> > cameraPoses;
 	std::map<int, rtabmap::Transform> scanPoses;
@@ -902,6 +1024,8 @@ int main(int argc, char * argv[])
 	std::map<int, std::vector<rtabmap::CameraModel> > cameraModels;
 	std::map<int, cv::Mat> cameraDepths;
 	int imagesExported = 0;
+	std::vector<int> rawViewpointIndices;
+	std::map<int, Transform> rawViewpoints;
 	for(std::map<int, Transform>::iterator iter=optimizedPoses.lower_bound(1); iter!=optimizedPoses.end(); ++iter)
 	{
 		Signature node = nodes.find(iter->first)->second;
@@ -1043,40 +1167,61 @@ int main(int argc, char * argv[])
 		else if(cloudI.get() && !cloudI->empty())
 			cloudI = rtabmap::util3d::transformPointCloud(cloudI, iter->second);
 
+		if(filter_ceiling != 0.0 || filter_floor != 0.0f)
+		{
+			if(cloud.get() && !cloud->empty())
+			{
+				cloud = util3d::passThrough(cloud, "z", filter_floor!=0.0f?filter_floor:(float)std::numeric_limits<int>::min(), filter_ceiling!=0.0f?filter_ceiling:(float)std::numeric_limits<int>::max());
+			}
+			if(cloudI.get() && !cloudI->empty())
+			{
+				cloudI = util3d::passThrough(cloudI, "z", filter_floor!=0.0f?filter_floor:(float)std::numeric_limits<int>::min(), filter_ceiling!=0.0f?filter_ceiling:(float)std::numeric_limits<int>::max());
+			}
+		}
 
-		Eigen::Vector3f viewpoint(iter->second.x(), iter->second.y(), iter->second.z());
 		if(cloudFromScan)
 		{
 			Transform lidarViewpoint = iter->second * node.sensorData().laserScanRaw().localTransform();
-			viewpoint = Eigen::Vector3f(iter->second.x(),  iter->second.y(),  iter->second.z());
+			rawViewpoints.insert(std::make_pair(iter->first, lidarViewpoint));
 		}
+		else if(!node.sensorData().cameraModels().empty() && !node.sensorData().cameraModels()[0].localTransform().isNull())
+		{
+			Transform cameraViewpoint = iter->second * node.sensorData().cameraModels()[0].localTransform(); // take the first camera
+			rawViewpoints.insert(std::make_pair(iter->first, cameraViewpoint));
+		}
+		else if(!node.sensorData().stereoCameraModel().localTransform().isNull())
+		{
+			Transform cameraViewpoint = iter->second * node.sensorData().stereoCameraModel().localTransform();
+			rawViewpoints.insert(std::make_pair(iter->first, cameraViewpoint));
+		}
+		else
+		{
+			rawViewpoints.insert(*iter);
+		}
+
 		if(cloud.get() && !cloud->empty())
 		{
-			pcl::PointCloud<pcl::Normal>::Ptr normals = rtabmap::util3d::computeNormals(cloud, 20, 0.0f, viewpoint);
+			if(assembledCloud->empty())
-			pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr cloudWithNormals(new pcl::PointCloud<pcl::PointXYZRGBNormal>);
-			pcl::concatenateFields(*cloud, *normals, *cloudWithNormals);
-			if(mergedClouds->size() == 0)
 			{
-				*mergedClouds = *cloudWithNormals;
+				*assembledCloud = *cloud;
 			}
 			else
 			{
-				*mergedClouds += *cloudWithNormals;
+				*assembledCloud += *cloud;
 			}
+			rawViewpointIndices.resize(assembledCloud->size(), iter->first);
 		}
 		else if(cloudI.get() && !cloudI->empty())
 		{
-			pcl::PointCloud<pcl::Normal>::Ptr normals = rtabmap::util3d::computeNormals(cloudI, 20, 0.0f, viewpoint);
+			if(assembledCloudI->empty())
-			pcl::PointCloud<pcl::PointXYZINormal>::Ptr cloudIWithNormals(new pcl::PointCloud<pcl::PointXYZINormal>);
-			pcl::concatenateFields(*cloudI, *normals, *cloudIWithNormals);
-			if(mergedCloudsI->size() == 0)
 			{
-				*mergedCloudsI = *cloudIWithNormals;
+				*assembledCloudI = *cloudI;
 			}
 			else
 			{
-				*mergedCloudsI += *cloudIWithNormals;
+				*assembledCloudI += *cloudI;
 			}
+			rawViewpointIndices.resize(assembledCloudI->size(), iter->first);
 		}
 
 		if(models.empty() && node.sensorData().stereoCameraModel().isValidForProjection())
@@ -1111,14 +1256,14 @@ int main(int argc, char * argv[])
 			scanPoses.insert(std::make_pair(iter->first, iter->second*node.sensorData().laserScanCompressed().localTransform()));
 		}
 	}
-	printf("Create and assemble the clouds... done (%fs, %d points).\n", timer.ticks(), !mergedClouds->empty()?(int)mergedClouds->size():(int)mergedCloudsI->size());
+	printf("Create and assemble the clouds... done (%fs, %d points).\n", timer.ticks(), !assembledCloud->empty()?(int)assembledCloud->size():(int)assembledCloudI->size());
 
 	if(imagesExported>0)
 		printf("%d images exported!\n", imagesExported);
 
 	ConsoleProgessState progressState;
 
-	if(!mergedClouds->empty() || !mergedCloudsI->empty())
+	if(!assembledCloud->empty() || !assembledCloudI->empty())
 	{
 		if(saveInDb)
 		{
@@ -1160,35 +1305,114 @@ int main(int argc, char * argv[])
 			}
 		}
 
-		pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr cloudToExport = mergedClouds;
+		if(proportionalRadiusFactor>0.0f && proportionalRadiusScale>=1.0f)
-		pcl::PointCloud<pcl::PointXYZINormal>::Ptr cloudIToExport = mergedCloudsI;
-
-		if(filter_ceiling != 0.0 || filter_floor != 0.0f)
 		{
-			printf("Passthrough filtering of the assembled cloud along z axis... (min=%f, max=%f, %d points)\n", filter_floor, filter_ceiling, !cloudToExport->empty()?(int)cloudToExport->size():(int)cloudIToExport->size());
+			printf("Proportional radius filtering of the assembled cloud... (factor=%f scale=%f, %d points)\n", proportionalRadiusFactor, proportionalRadiusScale, !assembledCloud->empty()?(int)assembledCloud->size():(int)assembledCloudI->size());
-			if(!cloudToExport->empty())
+			pcl::IndicesPtr indices;
+			if(!assembledCloud->empty())
 			{
-				cloudToExport = util3d::passThrough(cloudToExport, "z", filter_floor!=0.0f?filter_floor:(float)std::numeric_limits<int>::min(), filter_ceiling!=0.0f?filter_ceiling:(float)std::numeric_limits<int>::max());
+				indices = util3d::proportionalRadiusFiltering(assembledCloud, rawViewpointIndices, rawViewpoints, proportionalRadiusFactor, proportionalRadiusScale);
+				pcl::PointCloud<pcl::PointXYZRGB> tmp;
+				pcl::copyPointCloud(*assembledCloud, *indices, tmp);
+				*assembledCloud = tmp;
 			}
-			if(!cloudIToExport->empty())
+			else if(!assembledCloudI->empty())
 			{
-				cloudIToExport = util3d::passThrough(cloudIToExport, "z", filter_floor!=0.0f?filter_floor:(float)std::numeric_limits<int>::min(), filter_ceiling!=0.0f?filter_ceiling:(float)std::numeric_limits<int>::max());
+				indices = util3d::proportionalRadiusFiltering(assembledCloudI, rawViewpointIndices, rawViewpoints, proportionalRadiusFactor, proportionalRadiusScale);
+				pcl::PointCloud<pcl::PointXYZI> tmp;
+				pcl::copyPointCloud(*assembledCloudI, *indices, tmp);
+				*assembledCloudI = tmp;
 			}
-			printf("Passthrough filtering of the assembled cloud alog z axis.... done! (%fs, %d points)\n", timer.ticks(), !cloudToExport->empty()?(int)cloudToExport->size():(int)cloudIToExport->size());
+			if(indices.get())
+			{
+				std::vector<int> rawCameraIndicesTmp(indices->size());
+				for (std::size_t i = 0; i < indices->size(); ++i)
+					rawCameraIndicesTmp[i] = rawViewpointIndices[indices->at(i)];
+				rawViewpointIndices = rawCameraIndicesTmp;
+			}
+			printf("Proportional radius filtering of the assembled cloud.... done! (%fs, %d points)\n", timer.ticks(), !assembledCloud->empty()?(int)assembledCloud->size():(int)assembledCloudI->size());
 		}
 
+		pcl::PointCloud<pcl::PointXYZ>::Ptr rawAssembledCloud(new pcl::PointCloud<pcl::PointXYZ>);
+		if(!assembledCloud->empty())
+			pcl::copyPointCloud(*assembledCloud, *rawAssembledCloud); // used to adjust normal orientation
+		else if(!assembledCloudI->empty())
+			pcl::copyPointCloud(*assembledCloudI, *rawAssembledCloud); // used to adjust normal orientation
+
+		pcl::PointCloud<pcl::PointXYZ>::Ptr cloudWithoutNormals = rawAssembledCloud;
+
 		if(voxelSize>0.0f)
 		{
-			printf("Voxel grid filtering of the assembled cloud... (voxel=%f, %d points)\n", voxelSize, !cloudToExport->empty()?(int)cloudToExport->size():(int)cloudIToExport->size());
+			printf("Voxel grid filtering of the assembled cloud... (voxel=%f, %d points)\n", voxelSize, !assembledCloud->empty()?(int)assembledCloud->size():(int)assembledCloudI->size());
+			if(!assembledCloud->empty())
+			{
+				assembledCloud = util3d::voxelize(assembledCloud, voxelSize);
+				cloudWithoutNormals.reset(new pcl::PointCloud<pcl::PointXYZ>);
+				pcl::copyPointCloud(*assembledCloud, *cloudWithoutNormals);
+			}
+			else if(!assembledCloudI->empty())
+			{
+				assembledCloudI = util3d::voxelize(assembledCloudI, voxelSize);
+				cloudWithoutNormals.reset(new pcl::PointCloud<pcl::PointXYZ>);
+				pcl::copyPointCloud(*assembledCloudI, *cloudWithoutNormals);
+			}
+			printf("Voxel grid filtering of the assembled cloud.... done! (%fs, %d points)\n", timer.ticks(), !assembledCloud->empty()?(int)assembledCloud->size():(int)assembledCloudI->size());
+		}
+
+		printf("Computing normals of the assembled cloud... (k=20, %d points)\n", !assembledCloud->empty()?(int)assembledCloud->size():(int)assembledCloudI->size());
+		pcl::PointCloud<pcl::Normal>::Ptr normals = util3d::computeNormals(cloudWithoutNormals, 20, 0);
+
+		pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr cloudToExport(new pcl::PointCloud<pcl::PointXYZRGBNormal>);
+		pcl::PointCloud<pcl::PointXYZINormal>::Ptr cloudIToExport(new pcl::PointCloud<pcl::PointXYZINormal>);
+		if(!assembledCloud->empty())
+		{
+			UASSERT(assembledCloud->size() == normals->size());
+			pcl::concatenateFields(*assembledCloud, *normals, *cloudToExport);
+			printf("Computing normals of the assembled cloud... done! (%fs, %d points)\n", timer.ticks(), (int)assembledCloud->size());
+			assembledCloud->clear();
+
+			// adjust with point of views
+			printf("Adjust normals to viewpoints of the assembled cloud... (%d points)\n", (int)cloudToExport->size());
+			util3d::adjustNormalsToViewPoints(
+										rawViewpoints,
+										rawAssembledCloud,
+										rawViewpointIndices,
+										cloudToExport,
+										groundNormalsUp);
+			printf("Adjust normals to viewpoints of the assembled cloud... (%fs, %d points)\n", timer.ticks(), (int)cloudToExport->size());
+		}
+		else if(!assembledCloudI->empty())
+		{
+			UASSERT(assembledCloudI->size() == normals->size());
+			pcl::concatenateFields(*assembledCloudI, *normals, *cloudIToExport);
+			printf("Computing normals of the assembled cloud... done! (%fs, %d points)\n", timer.ticks(), (int)assembledCloudI->size());
+			assembledCloudI->clear();
+
+			// adjust with point of views
+			printf("Adjust normals to viewpoints of the assembled cloud... (%d points)\n", (int)cloudIToExport->size());
+			util3d::adjustNormalsToViewPoints(
+										rawViewpoints,
+										rawAssembledCloud,
+										rawViewpointIndices,
+										cloudIToExport,
+										groundNormalsUp);
+			printf("Adjust normals to viewpoints of the assembled cloud... (%fs, %d points)\n", timer.ticks(), (int)cloudIToExport->size());
+		}
+		cloudWithoutNormals->clear();
+		rawAssembledCloud->clear();
+
+		if(randomSamples>0)
+		{
+			printf("Random samples filtering of the assembled cloud... (samples=%d, %d points)\n", randomSamples, !cloudToExport->empty()?(int)cloudToExport->size():(int)cloudIToExport->size());
 			if(!cloudToExport->empty())
 			{
-				cloudToExport = util3d::voxelize(cloudToExport, voxelSize);
+				cloudToExport = util3d::randomSampling(cloudToExport, randomSamples);
 			}
 			else if(!cloudIToExport->empty())
 			{
-				cloudIToExport = util3d::voxelize(cloudIToExport, voxelSize);
+				cloudIToExport = util3d::randomSampling(cloudIToExport, randomSamples);
 			}
-			printf("Voxel grid filtering of the assembled cloud.... done! (%fs, %d points)\n", timer.ticks(), !cloudToExport->empty()?(int)cloudToExport->size():(int)cloudIToExport->size());
+			printf("Random samples filtering of the assembled cloud.... done! (%fs, %d points)\n", timer.ticks(), !cloudToExport->empty()?(int)cloudToExport->size():(int)cloudIToExport->size());
 		}
 
 		std::vector<int> pointToCamId;
@@ -1215,6 +1439,19 @@ int main(int argc, char * argv[])
 			{
 				cameraModelsProj = cameraModels;
 			}
+
+			cv::Mat projMask;
+			if(!cameraProjMask.empty())
+			{
+				projMask = cv::imread(cameraProjMask, cv::IMREAD_GRAYSCALE);
+				if(cameraProjDecimation>1)
+				{
+					cv::Mat out = projMask;
+					cv::resize(projMask, out, cv::Size(), 1.0f/float(cameraProjDecimation), 1.0f/float(cameraProjDecimation), cv::INTER_NEAREST);
+					projMask = out;
+				}
+			}
+
 			pointToCamId.resize(!cloudToExport->empty()?cloudToExport->size():cloudIToExport->size());
 			std::vector<std::pair< std::pair<int, int>, pcl::PointXY> > pointToPixel;
 			if(!cloudToExport->empty())
@@ -1225,7 +1462,8 @@ int main(int argc, char * argv[])
 						cameraModelsProj,
 						textureRange,
 						textureAngle,
-						std::vector<float>(),
+						textureRoiRatios,
+						projMask,
 						distanceToCamPolicy,
 						&progressState);
 			}
@@ -1237,7 +1475,8 @@ int main(int argc, char * argv[])
 						cameraModelsProj,
 						textureRange,
 						textureAngle,
-						std::vector<float>(),
+						textureRoiRatios,
+						projMask,
 						distanceToCamPolicy,
 						&progressState);
 				pointToCamIntensity.resize(pointToPixel.size());
@@ -1449,14 +1688,14 @@ int main(int argc, char * argv[])
 		// Meshing...
 		if(mesh || texture)
 		{
-			if(!mergedCloudsI->empty())
+			if(!cloudIToExport->empty())
 			{
-				pcl::copyPointCloud(*mergedCloudsI, *mergedClouds);
+				pcl::copyPointCloud(*cloudIToExport, *cloudToExport);
-				mergedCloudsI->clear();
+				cloudIToExport->clear();
 			}
 
 			Eigen::Vector4f min,max;
-			pcl::getMinMax3D(*mergedClouds, min, max);
+			pcl::getMinMax3D(*cloudToExport, min, max);
 			float mapLength = uMax3(max[0]-min[0], max[1]-min[1], max[2]-min[2]);
 			int optimizedDepth = 12;
 			for(int i=6; i<12; ++i)
@@ -1477,7 +1716,7 @@ int main(int argc, char * argv[])
 			pcl::PolygonMesh::Ptr mesh(new pcl::PolygonMesh);
 			pcl::Poisson<pcl::PointXYZRGBNormal> poisson;
 			poisson.setDepth(optimizedDepth);
-			poisson.setInputCloud(mergedClouds);
+			poisson.setInputCloud(cloudToExport);
 			poisson.reconstruct(*mesh);
 			printf("Mesh reconstruction... done (%fs, %d polygons).\n", timer.ticks(), (int)mesh->polygons.size());
 
@@ -1491,7 +1730,7 @@ int main(int argc, char * argv[])
 						mesh,
 						0.0f,
 						maxPolygons,
-						mergedClouds,
+						cloudToExport,
 						colorRadius,
 						!texture,
 						doClean,
@@ -1534,7 +1773,7 @@ int main(int argc, char * argv[])
 							textureDepthError,
 							textureAngle,
 							multiband?0:50, // Min polygons in camera view to be textured by this camera
-							std::vector<float>(),
+							textureRoiRatios,
 							&progressState,
 							&vertexToPixels,
 							distanceToCamPolicy);

tools/Report/main.cpp

@@ -733,6 +733,13 @@ int main(int argc, char * argv[])
 
 						if(poses.size())
 						{
+							//remove landmarks
+							std::map<int, Transform>::iterator iter=poses.begin();
+							while(iter!=poses.end() && iter->first < 0)
+							{
+								poses.erase(iter++);
+							}
+
 							std::map<int, Transform> groundTruth;
 							for(std::map<int, Transform>::const_iterator iter=poses.begin(); iter!=poses.end(); ++iter)
 							{