OccupancyGrid: updated parameters check

.travis.yml

@@ -1,6 +1,7 @@
 sudo: true
 dist: trusty
 language: cpp
+group: deprecated-2017Q3
 
 compiler:
   - gcc

CMakeLists.txt

@@ -21,7 +21,7 @@ SET(CMAKE_MODULE_PATH "${PROJECT_SOURCE_DIR}/cmake_modules")
 #######################
 SET(RTABMAP_MAJOR_VERSION 0)
 SET(RTABMAP_MINOR_VERSION 13)
-SET(RTABMAP_PATCH_VERSION 0)
+SET(RTABMAP_PATCH_VERSION 2)
 SET(RTABMAP_VERSION
   ${RTABMAP_MAJOR_VERSION}.${RTABMAP_MINOR_VERSION}.${RTABMAP_PATCH_VERSION})
   
@@ -124,6 +124,15 @@ set(INSTALL_CMAKE_DIR ${DEF_INSTALL_CMAKE_DIR} CACHE PATH
   "Installation directory for CMake files")
   
 ####### BUILD OPTIONS #######
+
+# ANDROID_PREBUILD (early exit if true)
+OPTION( ANDROID_PREBUILD "Set to ON to build rtabmap resource build tool (required for android build)" OFF )
+IF(ANDROID_PREBUILD)
+   MESSAGE("Option ANDROID_PREBUILD is set, only rtabmap resource tool will be built. You can use android toolchain after that.")
+   ADD_SUBDIRECTORY( utilite )
+   return()
+ENDIF(ANDROID_PREBUILD)
+
 IF(APPLE)
     OPTION(BUILD_AS_BUNDLE "Set to ON to build as bundle (DragNDrop)" OFF)
 ENDIF(APPLE)
@@ -162,9 +171,11 @@ FIND_PACKAGE(OpenCV REQUIRED QUIET)
 FIND_PACKAGE(PCL 1.7 REQUIRED QUIET)
 FIND_PACKAGE(ZLIB REQUIRED QUIET)
 
-# fix libproj.so not found on Xenial
 if(NOT "${PCL_LIBRARIES}" STREQUAL "")
+   # fix libproj.so not found on Xenial
    list(REMOVE_ITEM PCL_LIBRARIES "vtkproj4")
+   # fix libmpi.so not found on Zesty
+   list(REMOVE_ITEM PCL_LIBRARIES "/usr/lib/libmpi.so")
 endif()
 
 # OpenMP ("-fopenmp" should be added for flann included in PCL)
@@ -205,12 +216,12 @@ IF(WITH_QT)
     IF("${VTK_MAJOR_VERSION}" GREATER 5)
         FIND_PACKAGE(Qt5 COMPONENTS Widgets Core Gui QUIET)
         IF(Qt5_FOUND)
-             FIND_PACKAGE(Qt5 COMPONENTS Widgets Core Gui Svg)
+             FIND_PACKAGE(Qt5 COMPONENTS Widgets Core Gui OPTIONAL_COMPONENTS Svg)
         ENDIF(Qt5_FOUND)
     ENDIF("${VTK_MAJOR_VERSION}" GREATER 5)
 
     IF(NOT Qt5_FOUND)
-        FIND_PACKAGE(Qt4 COMPONENTS QtCore QtGui QtSvg)
+        FIND_PACKAGE(Qt4 COMPONENTS QtCore QtGui OPTIONAL_COMPONENTS QtSvg)
     ENDIF(NOT Qt5_FOUND)
         
     IF(QT4_FOUND OR Qt5_FOUND)
@@ -369,7 +380,7 @@ IF(WITH_DVO)
     ENDIF(dvo_core_FOUND)
 ENDIF(WITH_DVO)
 
-IF(WITH_ORB_SLAM2 AND NOT G2O_FOUND AND NOT RealSense_FOUND)
+IF(WITH_ORB_SLAM2 AND NOT G2O_FOUND)
     FIND_PACKAGE(ORB_SLAM2 QUIET)
     IF(ORB_SLAM2_FOUND)
        MESSAGE(STATUS "Found ORB_SLAM2: ${ORB_SLAM2_INCLUDE_DIRS}")
@@ -381,12 +392,10 @@ IF(WITH_ORB_SLAM2 AND NOT G2O_FOUND AND NOT RealSense_FOUND)
           MESSAGE(STATUS "Found Pangolin: ${Pangolin_INCLUDE_DIRS}")
           SET(ORB_SLAM2_INCLUDE_DIRS ${ORB_SLAM2_INCLUDE_DIRS} ${Pangolin_INCLUDE_DIRS})
           SET(ORB_SLAM2_LIBRARIES ${ORB_SLAM2_LIBRARIES} ${Pangolin_LIBRARIES})
-          set(CMAKE_C_FLAGS "${CMAKE_C_FLAGS} -march=native")
-          set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -march=native")
-          # There could be crash issues if built with RealSense
+          MESSAGE(WARNING "Don't forget to build ORB_SLAM2 (and included g2o) without \"-march=native\" to avoid crash when ORB_SLAM2 starts.")
        ENDIF()
     ENDIF(ORB_SLAM2_FOUND)
-ENDIF(WITH_ORB_SLAM2 AND NOT G2O_FOUND AND NOT RealSense_FOUND)
+ENDIF(WITH_ORB_SLAM2 AND NOT G2O_FOUND)
 
 IF(G2O_FOUND OR GTSAM_FOUND OR ZED_FOUND OR ANDROID OR RealSense_FOUND OR ORB_SLAM2_FOUND)
    #Newest versions require std11
@@ -885,12 +894,8 @@ IF(ORB_SLAM2_FOUND)
 MESSAGE(STATUS "  With ORB_SLAM2            = YES (License: GPLv3)")
 ELSEIF(NOT WITH_ORB_SLAM2)
 MESSAGE(STATUS "  With ORB_SLAM2            = NO (WITH_ORB_SLAM2=OFF)")
-ELSEIF(G2O_FOUND AND RealSense_FOUND)
-MESSAGE(STATUS "  With ORB_SLAM2            = NO (WITH_G2O should be OFF as ORB_SLAM2 uses its own g2o version, WITH_REALSENSE should also be off to avoid eigen errors)")
 ELSEIF(G2O_FOUND)
 MESSAGE(STATUS "  With ORB_SLAM2            = NO (WITH_G2O should be OFF as ORB_SLAM2 uses its own g2o version)")
-ELSEIF(RealSense_FOUND)
-MESSAGE(STATUS "  With ORB_SLAM2            = NO (WITH_REALSENSE should be off to avoid eigen errors)")
 ELSE()
 MESSAGE(STATUS "  With ORB_SLAM2            = NO (ORB_SLAM2 not found, make sure environment variable ORB_SLAM2_ROOT_DIR is set)")
 ENDIF()

RTABMapConfig.cmake.in

@@ -47,11 +47,11 @@ ENDIF()
 set(RTABMap_LIBRARIES ${RTABMap_CORE} ${RTABMap_UTILITE})
 
 list(LENGTH RTABMap_FIND_COMPONENTS RTABMap_FIND_COMPONENTS_LENGTH)
-set(WITH_GUI OFF)
+set(WITH_GUI ON)
 if(${RTABMap_FIND_COMPONENTS_LENGTH} GREATER 0)
    list (FIND RTABMap_FIND_COMPONENTS "gui" _index)
-   if (${_index} GREATER -1)
-      set(WITH_GUI ON)
+   if (${_index} EQUAL -1)
+      set(WITH_GUI OFF)
    endif()
 endif(${RTABMap_FIND_COMPONENTS_LENGTH} GREATER 0)
 

app/android/AndroidManifest.xml.in

@@ -2,7 +2,7 @@
 <!-- BEGIN_INCLUDE(manifest) -->
 <manifest xmlns:android="http://schemas.android.com/apk/res/android"
         package="com.introlab.rtabmap"
-        android:versionCode="51"
+        android:versionCode="55"
         android:versionName="@RTABMAP_VERSION@">
 
     <uses-permission android:name="android.permission.CAMERA" />
@@ -31,7 +31,8 @@
                 android:label="@string/app_name"
                 android:launchMode="singleTask"
 		android:screenOrientation="fullSensor"
-		android:configChanges="orientation|screenSize|keyboardHidden">
+		android:configChanges="orientation|screenSize|keyboardHidden"
+		android:theme="@style/ThemeApp">
             <!-- Tell NativeActivity the name of our .so -->
             <meta-data android:name="android.app.lib_name"
                     android:value="NativeRTABMap" />

app/android/info.txt.in

@@ -1,7 +1,7 @@
 <h3>Real-Time Appearance-Based Mapping</h3>
 Version @RTABMAP_VERSION@<br>
 Author: Mathieu Labb&eacute;<br>
-Copyright 2016<br>
+Copyright 2016-2017<br>
 IntRoLab - Universit&eacute; de Sherbrooke<br>
 <b>http://introlab.github.io/rtabmap</b><br><br>
 

app/android/jni/CameraTango.cpp

@@ -189,7 +189,7 @@ bool CameraTango::init(const std::string & calibrationFolder, const std::string
 {
 	close();
 
-	TangoSupport_initializeLibrary();
+	TangoSupport_initialize(TangoService_getPoseAtTime, TangoService_getCameraIntrinsics);
 
 	// Connect to Tango
 	LOGI("NativeRTABMap: Setup tango config");

app/android/jni/RTABMapApp.cpp

@@ -298,7 +298,11 @@ int RTABMapApp::openDatabase(const std::string & databasePath, bool databaseInMe
 	optTexture_ = cv::Mat();
 	cv::Mat cloudMat;
 	std::vector<std::vector<std::vector<unsigned int> > > polygons;
+#if PCL_VERSION_COMPARE(>=, 1, 8, 0)
+	std::vector<std::vector<Eigen::Vector2f, Eigen::aligned_allocator<Eigen::Vector2f> > > texCoords;
+#else
 	std::vector<std::vector<Eigen::Vector2f> > texCoords;
+#endif
 	cv::Mat textures;
 	std::map<int, rtabmap::Transform> optPoses;
 	if(!databaseSource.empty())
@@ -1051,6 +1055,7 @@ int RTABMapApp::Render()
 					pcl::fromPCLPointCloud2(optMesh_->cloud, *mesh.cloud);
 					pcl::fromPCLPointCloud2(optMesh_->cloud, *mesh.normals);
 					mesh.polygons = optMesh_->tex_polygons[0];
+					mesh.pose.setIdentity();
 					if(optMesh_->tex_coordinates.size())
 					{
 						mesh.texCoords = optMesh_->tex_coordinates[0];
@@ -1568,7 +1573,7 @@ int RTABMapApp::Render()
 			if(rtabmapEvents.size())
 			{
 				// send statistics to GUI
-				LOGW("Posting PostRenderEvent! %fs", renderingTime_);
+				LOGI("New data added to map, rendering time: %fs", renderingTime_);
 				UEventsManager::post(new PostRenderEvent(rtabmapEvents.back()));
 				rtabmapEvents.pop_back();
 
@@ -1740,6 +1745,10 @@ void RTABMapApp::setOrthoCropFactor(float value)
 {
 	main_scene_.setOrthoCropFactor(value);
 }
+void RTABMapApp::setGridRotation(float value)
+{
+	main_scene_.setGridRotation(value);
+}
 void RTABMapApp::setLighting(bool enabled)
 {
 	main_scene_.setLighting(enabled);
@@ -2011,8 +2020,10 @@ void RTABMapApp::save(const std::string & databasePath)
 		dataRecorderMode_ = dataRecorderModeBackup;
 	}
 
+	std::map<int, rtabmap::Transform> poses = rtabmap_->getLocalOptimizedPoses();
 	rtabmap_->close(true, databasePath);
 	rtabmap_->init(getRtabmapParameters(), dataRecorderMode_?"":databasePath);
+	rtabmap_->setOptimizedPoses(poses);
 	if(dataRecorderMode_)
 	{
 		clearSceneOnNextRender_ = true;
@@ -2040,7 +2051,7 @@ bool RTABMapApp::exportMesh(
 		int optimizedMaxPolygons,
 		float optimizedColorRadius,
 		bool optimizedCleanWhitePolygons,
-		bool optimizedColorWhitePolygons, // not yet used
+		int optimizedMinClusterSize,
 		float optimizedMaxTextureDistance,
 		int optimizedMinTextureClusterSize,
 		bool blockRendering)
@@ -2048,6 +2059,24 @@ bool RTABMapApp::exportMesh(
 	// make sure createdMeshes_ is not modified while exporting! We don't
 	// lock the meshesMutex_ because we want to continue rendering.
 
+	std::map<int, rtabmap::Transform> poses = rtabmap_->getLocalOptimizedPoses();
+	if(poses.empty())
+	{
+		// look if we just triggered new map without localizing afterward (pause/resume in append Mode)
+		std::multimap<int, rtabmap::Link> links;
+		rtabmap_->getGraph(
+				poses,
+				links,
+				true,
+				false);
+		if(poses.empty())
+		{
+			UERROR("Empty optimized poses!");
+			return false;
+		}
+		rtabmap_->setOptimizedPoses(poses);
+	}
+
 	if(blockRendering)
 	{
 		renderingMutex_.lock();
@@ -2060,8 +2089,6 @@ bool RTABMapApp::exportMesh(
 
 	try
 	{
-		std::map<int, rtabmap::Transform> poses = rtabmap_->getLocalOptimizedPoses();
-
 		int totalSteps = 0;
 		totalSteps+=poses.size(); // assemble
 		if(meshing)
@@ -2314,7 +2341,7 @@ bool RTABMapApp::exportMesh(
 									optimizedColorRadius,
 									textureSize == 0,
 									optimizedCleanWhitePolygons,
-									0);
+									optimizedMinClusterSize);
 
 							if(textureSize>0)
 							{
@@ -2364,6 +2391,10 @@ bool RTABMapApp::exportMesh(
 							}
 						}
 					}
+					else
+					{
+						UERROR("Merged cloud too small (%d points) to create polygons!", (int)mergedClouds->size());
+					}
 				}
 				else // organized meshes
 				{
@@ -2727,6 +2758,10 @@ bool RTABMapApp::exportMesh(
 					success = true;
 				}
 			}
+			else
+			{
+				UERROR("Merged cloud is empty!");
+			}
 		}
 
 		progressionStatus_.finish();
@@ -2764,7 +2799,11 @@ bool RTABMapApp::postExportation(bool visualize)
 	{
 		cv::Mat cloudMat;
 		std::vector<std::vector<std::vector<unsigned int> > > polygons;
+#if PCL_VERSION_COMPARE(>=, 1, 8, 0)
+		std::vector<std::vector<Eigen::Vector2f, Eigen::aligned_allocator<Eigen::Vector2f> > > texCoords;
+#else
 		std::vector<std::vector<Eigen::Vector2f> > texCoords;
+#endif
 		cv::Mat textures;
 		std::map<int, rtabmap::Transform> optPoses;
 		if(rtabmap_ && rtabmap_->getMemory())
@@ -2801,7 +2840,11 @@ bool RTABMapApp::writeExportedMesh(const std::string & directory, const std::str
 	pcl::TextureMesh::Ptr textureMesh(new pcl::TextureMesh);
 	cv::Mat cloudMat;
 	std::vector<std::vector<std::vector<unsigned int> > > polygons;
+#if PCL_VERSION_COMPARE(>=, 1, 8, 0)
+	std::vector<std::vector<Eigen::Vector2f, Eigen::aligned_allocator<Eigen::Vector2f> > > texCoords;
+#else
 	std::vector<std::vector<Eigen::Vector2f> > texCoords;
+#endif
 	cv::Mat textures;
 	std::map<int, rtabmap::Transform> optPoses;
 	if(rtabmap_ && rtabmap_->getMemory())

app/android/jni/RTABMapApp.h

@@ -121,6 +121,7 @@ class RTABMapApp : public UEventsHandler {
   void setPointSize(float value);
   void setFOV(float angle);
   void setOrthoCropFactor(float value);
+  void setGridRotation(float value);
   void setLighting(bool enabled);
   void setBackfaceCulling(bool enabled);
   void setWireframe(bool enabled);
@@ -163,7 +164,7 @@ class RTABMapApp : public UEventsHandler {
 		  int optimizedMaxPolygons,
 		  float optimizedColorRadius,
 		  bool optimizedCleanWhitePolygons,
-		  bool optimizedColorWhitePolygons,
+		  int optimizedMinClusterSize,
 		  float optimizedMaxTextureDistance,
 		  int optimizedMinTextureClusterSize,
 		  bool blockRendering);

app/android/jni/jni_interface.cpp

@@ -180,6 +180,12 @@ Java_com_introlab_rtabmap_RTABMapLib_setOrthoCropFactor(
 	return app.setOrthoCropFactor(value);
 }
 JNIEXPORT void JNICALL
+Java_com_introlab_rtabmap_RTABMapLib_setGridRotation(
+		JNIEnv*, jobject, float value)
+{
+	return app.setGridRotation(value);
+}
+JNIEXPORT void JNICALL
 Java_com_introlab_rtabmap_RTABMapLib_setLighting(
 		JNIEnv*, jobject, bool enabled)
 {
@@ -371,7 +377,7 @@ Java_com_introlab_rtabmap_RTABMapLib_exportMesh(
 		int optimizedMaxPolygons,
 		float optimizedColorRadius,
 		bool optimizedCleanWhitePolygons,
-		bool optimizedColorWhitePolygons,
+		int optimizedMinClusterSize,
 		float optimizedMaxTextureDistance,
 		int optimizedMinTextureClusterSize,
 		bool blockRendering)
@@ -389,7 +395,7 @@ Java_com_introlab_rtabmap_RTABMapLib_exportMesh(
 			optimizedMaxPolygons,
 			optimizedColorRadius,
 			optimizedCleanWhitePolygons,
-			optimizedColorWhitePolygons,
+			optimizedMinClusterSize,
 			optimizedMaxTextureDistance,
 			optimizedMinTextureClusterSize,
 			blockRendering);

app/android/jni/point_cloud_drawable.h

@@ -70,6 +70,7 @@ private:
   bool isVisible() const {return visible_;}
   bool hasMesh() const {return polygons_.size()!=0;}
   bool hasTexture() const {return textures_ != 0;}
+  float getMinHeight() const {return minHeight_;}
   const pcl::PointXYZ & aabbMinModel() const {return aabbMinModel_;}
   const pcl::PointXYZ & aabbMaxModel() const {return aabbMaxModel_;}
   const pcl::PointXYZ & aabbMinWorld() const {return aabbMinWorld_;}
@@ -126,6 +127,7 @@ private:
   bool visible_;
   bool hasNormals_;
   std::vector<unsigned int> organizedToDenseIndices_;
+  float minHeight_; // odom frame
 
   float gainR_;
   float gainG_;

app/android/jni/scene.cpp

@@ -16,12 +16,16 @@
 
 #include <tango-gl/conversions.h>
 #include <tango-gl/gesture_camera.h>
+#include <tango-gl/util.h>
 
 #include <rtabmap/utilite/ULogger.h>
 #include <rtabmap/utilite/UStl.h>
 #include <rtabmap/utilite/UTimer.h>
 #include <rtabmap/core/util3d_filtering.h>
+#include <rtabmap/core/util3d_transforms.h>
+#include <rtabmap/core/util3d_surface.h>
 #include <pcl/common/transforms.h>
+#include <pcl/common/common.h>
 
 #include <glm/gtx/transform.hpp>
 
@@ -32,7 +36,7 @@
 // add an offset in z to our origin. We'll set this offset to 1.3 meters based
 // on the average height of a human standing with a Tango device. This allows us
 // to place a grid roughly on the ground for most users.
-const glm::vec3 kHeightOffset = glm::vec3(0.0f, 1.3f, 0.0f);
+const glm::vec3 kHeightOffset = glm::vec3(0.0f, -1.3f, 0.0f);
 
 // Color of the motion tracking trajectory.
 const tango_gl::Color kTraceColor(0.66f, 0.66f, 0.66f);
@@ -130,7 +134,7 @@ void Scene::InitGLContent()
 	trace_->ClearVertexArray();
 	trace_->SetColor(kTraceColor);
 	grid_->SetColor(kGridColor);
-	grid_->SetPosition(-kHeightOffset);
+	grid_->SetPosition(kHeightOffset);
 	box_->SetShader();
 	box_->SetColor(1,0,0);
 
@@ -194,6 +198,10 @@ void Scene::clear()
 		graph_ = 0;
 	}
 	pointClouds_.clear();
+	if(grid_)
+	{
+		grid_->SetPosition(kHeightOffset);
+	}
 }
 
 //Should only be called in OpenGL thread!
@@ -565,6 +573,15 @@ void Scene::setOrthoCropFactor(float value)
 {
 	gesture_camera_->SetOrthoCropFactor(value);
 }
+void Scene::setGridRotation(float angleDeg)
+{
+	float angleRad = angleDeg * DEGREE_2_RADIANS;
+	if(grid_)
+	{
+		glm::quat rot = glm::rotate(glm::quat(1,0,0,0), angleRad, glm::vec3(0, 1, 0));
+		grid_->SetRotation(rot);
+	}
+}
 
 rtabmap::Transform Scene::GetOpenGLCameraPose(float * fov) const
 {
@@ -669,6 +686,58 @@ void Scene::addMesh(
 	PointCloudDrawable * drawable = new PointCloudDrawable(mesh, createWireframe);
 	drawable->setPose(pose);
 	pointClouds_.insert(std::make_pair(id, drawable));
+
+	if(!mesh.pose.isNull() && mesh.cloud->size() && (!mesh.cloud->isOrganized() || mesh.indices->size()))
+	{
+		UTimer time;
+		float height = 0.0f;
+		Eigen::Affine3f affinePose = mesh.pose.toEigen3f();
+		if(mesh.polygons.size())
+		{
+			for(unsigned int i=0; i<mesh.polygons.size(); ++i)
+			{
+				for(unsigned int j=0; j<mesh.polygons[i].vertices.size(); ++j)
+				{
+					pcl::PointXYZRGB pt = pcl::transformPoint(mesh.cloud->at(mesh.polygons[i].vertices[j]), affinePose);
+					if(pt.z < height)
+					{
+						height = pt.z;
+					}
+				}
+			}
+		}
+		else
+		{
+			if(mesh.cloud->isOrganized())
+			{
+				for(unsigned int i=0; i<mesh.indices->size(); ++i)
+				{
+					pcl::PointXYZRGB pt = pcl::transformPoint(mesh.cloud->at(mesh.indices->at(i)), affinePose);
+					if(pt.z < height)
+					{
+						height = pt.z;
+					}
+				}
+			}
+			else
+			{
+				for(unsigned int i=0; i<mesh.cloud->size(); ++i)
+				{
+					pcl::PointXYZRGB pt = pcl::transformPoint(mesh.cloud->at(i), affinePose);
+					if(pt.z < height)
+					{
+						height = pt.z;
+					}
+				}
+			}
+		}
+
+		if(grid_->GetPosition().y == kHeightOffset.y || grid_->GetPosition().y > height)
+		{
+			grid_->SetPosition(glm::vec3(0,height,0));
+		}
+		LOGD("compute min height %f s", time.ticks());
+	}
 }
 
 

app/android/jni/scene.h

@@ -129,6 +129,7 @@ class Scene {
   void setPointSize(float size) {pointSize_ = size;}
   void setFOV(float angle);
   void setOrthoCropFactor(float value);
+  void setGridRotation(float angleDeg);
   void setLighting(bool enabled) {lighting_ = enabled;}
   void setBackfaceCulling(bool enabled) {backfaceCulling_ = enabled;}
   void setWireframe(bool enabled) {wireFrame_ = enabled;}

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

@@ -63,11 +63,8 @@ GestureCamera::~GestureCamera() { delete cam_parent_transform_; }
 
 void GestureCamera::OnTouchEvent(int touch_count, TouchEvent event, float x0,
 		float y0, float x1, float y1) {
-	if (camera_type_ == kFirstPerson) {
-		return;
-	}
 
-	if (touch_count == 1) {
+	if (camera_type_!=kFirstPerson && touch_count == 1) {
 		switch (event) {
 		case kTouch0Down: {
 			cam_start_angle_ = cam_cur_angle_;
@@ -100,6 +97,7 @@ void GestureCamera::OnTouchEvent(int touch_count, TouchEvent event, float x0,
 			float abs_y = y0 - y1;
 			start_touch_dist_ = std::sqrt(abs_x * abs_x + abs_y * abs_y);
 			cam_start_dist_ = GetPosition().z;
+			cam_start_fov_ = this->getFOV();
 
 			// center touch
 			touch0_start_position_.x = (x0+x1)/2.0f;
@@ -111,25 +109,31 @@ void GestureCamera::OnTouchEvent(int touch_count, TouchEvent event, float x0,
 			float abs_y = y0 - y1;
 			float dist = start_touch_dist_ - std::sqrt(abs_x * abs_x + abs_y * abs_y);
 
-			cam_cur_dist_ = tango_gl::util::Clamp(cam_start_dist_ + dist * kZoomSpeed,
-					kCamViewMinDist, kCamViewMaxDist);
-
-			this->SetOrthoMode(camera_type_ == kTopOrtho);
-			if(camera_type_ == kTopOrtho)
+			if(camera_type_ == kFirstPerson)
 			{
-				this->SetOrthoScale(cam_cur_dist_);
+				this->SetFieldOfView(tango_gl::util::Clamp(cam_start_fov_ + dist * kZoomSpeed*10.0f, 45, 90));
 			}
+			else
+			{
+				cam_cur_dist_ = tango_gl::util::Clamp(cam_start_dist_ + dist * kZoomSpeed,
+								kCamViewMinDist, kCamViewMaxDist);
 
-			glm::vec2 touch_center_position((x0+x1)/2.0f, (y0+y1)/2.0f);
-			glm::vec2 offset;
-			offset.x = (touch_center_position.x - touch0_start_position_.x) * kMoveSpeed;
-			offset.y = (touch_center_position.y - touch0_start_position_.y) * kMoveSpeed;
-			touch0_start_position_ = touch_center_position;
+				this->SetOrthoMode(camera_type_ == kTopOrtho);
+				if(camera_type_ == kTopOrtho)
+				{
+					this->SetOrthoScale(cam_cur_dist_);
+				}
 
-			StartCameraToCurrentTransform();
+				glm::vec2 touch_center_position((x0+x1)/2.0f, (y0+y1)/2.0f);
+				glm::vec2 offset;
+				offset.x = (touch_center_position.x - touch0_start_position_.x) * kMoveSpeed;
+				offset.y = (touch_center_position.y - touch0_start_position_.y) * kMoveSpeed;
+				touch0_start_position_ = touch_center_position;
 
-			anchor_offset_ += glm::rotate(cam_parent_transform_->GetRotation(), glm::vec3(-offset.x, offset.y, 0));
+				StartCameraToCurrentTransform();
 
+				anchor_offset_ += glm::rotate(cam_parent_transform_->GetRotation(), glm::vec3(-offset.x, offset.y, 0));
+			}
 			break;
 		}
 		default: { break; }

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

@@ -81,6 +81,7 @@ class GestureCamera : public Camera {
   glm::quat cam_cur_target_rot_;
 
   float cam_start_dist_;
+  float cam_start_fov_;
   float cam_cur_dist_;
   glm::vec3 anchor_offset_;
 

app/android/res/drawable/custom_seekbar.xml

@@ -0,0 +1,12 @@
+<?xml version="1.0" encoding="utf-8"?>
+<layer-list xmlns:android="http://schemas.android.com/apk/res/android" >
+    <item>
+        <shape>
+            <gradient
+                android:endColor="#00ffffff"
+                android:startColor="#ff686868"
+                android:useLevel="false" />
+        </shape>
+    </item>
+
+</layer-list>

app/android/res/layout/activity_rtabmap.xml

@@ -25,7 +25,7 @@
     
     <RelativeLayout 
         android:layout_width="wrap_content"
-        android:layout_height="wrap_content"
+        android:layout_height="match_parent"
         android:fitsSystemWindows="true">
     
     <ToggleButton
@@ -35,7 +35,7 @@
         android:layout_above="@+id/light_button"
         android:layout_alignLeft="@+id/light_button"
         android:layout_alignParentRight="true"
-        android:layout_marginBottom="15dp"
+        android:layout_marginBottom="5dp"
         android:layout_marginRight="5dp"
         android:paddingRight="5dp"
         android:textOff="@string/wireframe"
@@ -48,7 +48,7 @@
         android:layout_above="@+id/backface_button"
         android:layout_alignLeft="@+id/backface_button"
         android:layout_alignParentRight="true"
-        android:layout_marginBottom="15dp"
+        android:layout_marginBottom="5dp"
         android:layout_marginRight="5dp"
         android:paddingRight="5dp"
         android:textOff="@string/light_off"
@@ -58,60 +58,36 @@
         android:id="@+id/backface_button"
         android:layout_width="100dp"
         android:layout_height="wrap_content"
-        android:layout_above="@+id/first_person_button"
-        android:layout_alignLeft="@+id/first_person_button"
+        android:layout_above="@+id/camera_button"
+        android:layout_alignRight="@+id/camera_button"
         android:layout_alignParentRight="true"
-        android:layout_marginBottom="5dp"
+        android:layout_marginBottom="15dp"
         android:layout_marginRight="5dp"
         android:paddingRight="5dp"
         android:textOff="@string/backface_off"
         android:textOn="@string/backface_on" />
  
-    <ToggleButton
-        android:id="@+id/first_person_button"
-        android:layout_width="100dp"
-        android:layout_height="wrap_content"
-        android:layout_above="@+id/third_person_button"
-        android:layout_alignLeft="@+id/third_person_button"
-        android:layout_alignParentRight="true"
-        android:layout_marginBottom="5dp"
-        android:layout_marginRight="5dp"
-        android:paddingRight="5dp"
-        android:textOff="@string/first_person"
-        android:textOn="@string/first_person" />
-    
-    <ToggleButton
-        android:id="@+id/third_person_button"
-        android:layout_width="100dp"
-        android:layout_height="wrap_content"
-        android:layout_above="@+id/top_down_button"
-        android:layout_alignParentRight="true"
-        android:layout_marginBottom="5dp"
-        android:layout_marginRight="5dp"
-        android:paddingRight="5dp"
-        android:textOff="@string/third_person"
-        android:textOn="@string/third_person" />
-
-    <ToggleButton
-        android:id="@+id/top_down_button"
-        android:layout_width="100dp"
-        android:layout_height="wrap_content"
+    <com.introlab.rtabmap.NDSpinner
+        android:id="@+id/camera_button"
+        android:layout_width="140dp"
+        android:layout_height="40dp"
         android:layout_alignParentBottom="true"
         android:layout_alignParentRight="true"
         android:layout_marginRight="5dp"
+        android:layout_marginBottom="10dp"
         android:paddingRight="5dp"
-        android:textOff="@string/top_down"
-        android:textOn="@string/top_down" />
+        android:paddingBottom="10dp"
+        android:text="@string/camera_button"
+        android:spinnerMode="dropdown"/>
 
     <ToggleButton
         android:id="@+id/pause_button"
         android:layout_width="100dp"
         android:layout_height="wrap_content"
-        android:layout_alignLeft="@+id/first_person_button"
+		android:layout_marginRight="5dp"
         android:layout_alignParentTop="true"
-        android:layout_marginTop="61dp"
+        android:layout_marginTop="10dp"
         android:layout_alignParentRight="true"
-        android:paddingRight="5dp"
         android:textOff="@string/pause"
         android:textOn="@string/resume" />
 
@@ -119,37 +95,50 @@
         android:id="@+id/button_shareToSketchfab"
         android:layout_width="wrap_content"
         android:layout_height="wrap_content"
+        android:layout_marginRight="5dp"
         android:layout_alignParentTop="true"
-        android:layout_alignRight="@+id/pause_button"
+        android:layout_marginTop="10dp"
+        android:layout_alignParentRight="true"
         android:text="@string/share_to_sketchfab" />
 
     <Button
         android:id="@+id/button_saveOnDevice"
         android:layout_width="wrap_content"
         android:layout_height="wrap_content"
-        android:layout_alignParentTop="true"
-        android:layout_toLeftOf="@+id/button_shareToSketchfab"
+        android:layout_alignRight="@+id/button_shareToSketchfab"
+        android:layout_below="@+id/button_shareToSketchfab"
         android:text="@string/save_to_file" />
 
     <Button
         android:id="@+id/close_visualization_button"
         android:layout_width="200dp"
         android:layout_height="wrap_content"
-        android:layout_alignBaseline="@+id/top_down_button"
-        android:layout_alignBottom="@+id/top_down_button"
+        android:layout_above="@+id/camera_button"
         android:layout_centerHorizontal="true"
-        android:paddingLeft="5dp"
         android:text="@string/close_visualization" />
 
     <SeekBar
-        android:id="@+id/seekBar_fov"
+        android:id="@+id/seekBar_ortho_cut"
         android:layout_width="200dp"
         android:layout_height="wrap_content"
         android:rotation="270"
+        android:layout_above="@+id/light_button"
         android:layout_alignParentLeft="true"
-        android:layout_alignBaseline="@+id/first_person_button"
         android:layout_gravity="center"
-        android:layout_marginLeft="-50dp" />
+        android:layout_marginLeft="-50dp"
+        android:progressDrawable="@drawable/custom_seekbar" />
+    
+    <SeekBar
+        android:id="@+id/seekBar_grid"
+        android:layout_width="200dp"
+        android:layout_height="wrap_content"
+        android:layout_alignParentLeft="true"
+        android:layout_alignBottom="@+id/camera_button"
+        android:layout_marginLeft="35dp"
+        android:paddingBottom="10dp"
+        android:progressDrawable="@drawable/custom_seekbar" />
+
+
 
 </RelativeLayout>        
 </RelativeLayout>

app/android/res/layout/activity_settings.xml

@@ -292,6 +292,14 @@
 		            android:summary="@string/pref_summary_opt_clean_white"
 		            android:defaultValue="@string/pref_default_opt_clean_white"/>
 		        
+		        <ListPreference 
+		            android:key="@string/pref_key_opt_min_cluster_size"
+		    		android:title="@string/pref_title_opt_min_cluster_size"
+		    		android:summary="@string/pref_summary_opt_min_cluster_size"
+		   			android:entries="@array/pref_opt_min_cluster_size_keys"
+		   			android:entryValues="@array/pref_opt_min_cluster_size_values"
+		   			android:defaultValue="@string/pref_default_opt_min_cluster_size"/>
+		        
 		       </PreferenceCategory>
 	 	</PreferenceScreen>
 	 	<ListPreference 
@@ -308,6 +316,11 @@
    			android:entries="@array/pref_cluster_ratio_keys"
    			android:entryValues="@array/pref_cluster_ratio_values"
    			android:defaultValue="@string/pref_default_cluster_ratio"/>
+        <SwitchPreference 
+            android:key="@string/pref_key_notification_sound"
+    		android:title="@string/pref_title_notification_sound"
+    		android:summary="@string/pref_summary_notification_sound"
+   			android:defaultValue="@string/pref_default_notification_sound"/>
     </PreferenceCategory>
 	<PreferenceCategory
         android:title="@string/pref_title_presets">

app/android/res/values/arrays.xml

@@ -0,0 +1,9 @@
+
+    <resources>
+        <string-array name="camera_view_array">
+        <item>First View</item>
+        <item>Third-P. View</item>
+        <item>Top View</item>
+        <item>Ortho View</item>
+    </string-array>
+    </resources>

app/android/res/values/strings.xml

@@ -8,9 +8,7 @@
     <string name="sketchfab">Upload to Sketchfab&#8230;</string>
     <string name="status">"Status: "</string>
     <string name="words">"Words: "</string>
-    <string name="first_person">First</string>
-    <string name="third_person">Third</string>
-    <string name="top_down">Top</string>
+    <string name="camera_button">First View</string>
     <string name="pause">Pause</string>
     <string name="resume">Resume</string>
     <string name="backface_on">Backface</string>
@@ -126,10 +124,14 @@
     <string name="pref_default_opt_color_radius">0.05</string>
     <string name="pref_key_opt_clean_white">pref_key_opt_clean_white</string>
     <string name="pref_default_opt_clean_white">true</string>
+    <string name="pref_key_opt_min_cluster_size">pref_key_opt_min_cluster_size</string>
+    <string name="pref_default_opt_min_cluster_size">0</string>
     <string name="pref_key_gain_max_radius">pref_key_gain_max_radius</string>
     <string name="pref_default_gain_max_radius">0.02</string>
     <string name="pref_key_cluster_ratio">pref_key_cluster_ratio</string>
     <string name="pref_default_cluster_ratio">0.05</string>
+    <string name="pref_key_notification_sound">pref_key_notification_sound</string>
+    <string name="pref_default_notification_sound">true</string>
     <!-- Preference keys: END -->
 
     <string name="pref_title_rendering">Rendering</string>
@@ -308,7 +310,7 @@
     <string name="pref_title_smoothing">Smoothing</string>
     <string name="pref_summary_smoothing">Smooth the point clouds.</string>
     <string name="pref_title_fisheye">Fish Eye Camera</string>
-    <string name="pref_summary_fisheye">Use fish eye camera instead of the color camera. Cannot be used on Yellowstone tablet.</string>
+    <string name="pref_summary_fisheye">Use fish eye camera instead of the color camera. May not work on some devices.</string>
     <string name="pref_title_update_rate">Update Rate</string>
     <string name="pref_summary_update_rate">Rate at which a new node is added to map.</string>
     <string name="pref_title_time_thr">Time Limit</string>
@@ -659,22 +661,22 @@
         <item>"2"</item>
     </string-array>
     <string-array name="pref_min_texture_cluster_size_keys">
-        <item>"Disabled"</item>
         <item>"1000"</item>
         <item>"500"</item>
         <item>"200"</item>
         <item>"100"</item>
         <item>"50"</item>
         <item>"10"</item>
+        <item>"Disabled"</item>
     </string-array>
     <string-array name="pref_min_texture_cluster_size_values">
-        <item>"0"</item>
         <item>"1000"</item>
         <item>"500"</item>
         <item>"200"</item>
         <item>"100"</item>
         <item>"50"</item>
         <item>"10"</item>
+        <item>"0"</item>
     </string-array>
     
     <string name="pref_title_optimized">Optimized</string>
@@ -686,6 +688,8 @@
     <string name="pref_summary_opt_color_radius">Radius used to transfer nearest color from the point cloud to reconstructed mesh. When exporting with texture, if Clean Mesh is also enabled, this will limit the number of polygons textured in holes.</string>
     <string name="pref_title_opt_clean_white">Clean Mesh</string>
     <string name="pref_summary_opt_clean_white">Clean mesh from textureless or colorless reconstructed polygons.</string>
+	<string name="pref_summary_opt_min_cluster_size">This can be used to filter polygons before texturing.</string>
+    <string name="pref_title_opt_min_cluster_size">Polygon Filtering</string>
     
     <string-array name="pref_opt_depth_keys">
         <item>"Auto"</item>
@@ -735,12 +739,22 @@
         <item>"0.01"</item>
         <item>"-1"</item>
     </string-array>
+    <string-array name="pref_opt_min_cluster_size_keys">
+        <item>"Only biggest cluster kept"</item>
+        <item>"Keep all polygons"</item>
+    </string-array>
+    <string-array name="pref_opt_min_cluster_size_values">
+        <item>"-1"</item>
+        <item>"0"</item>
+    </string-array>
 
     <string name="pref_title_general">General</string>
     <string name="pref_title_gain_max_radius">Color Correction Radius</string>
     <string name="pref_summary_gain_max_radius">Radius used to find pixel correspondences for Adjust Colors optimization.</string>
     <string name="pref_title_cluster_ratio">Noise Filtering Ratio</string>
     <string name="pref_summary_cluster_ratio">Polygon clusters with size smaller than this ratio of the largest cluster are removed by the Noise Filtering optimization.</string>
+    <string name="pref_title_notification_sound">Notification Sound</string>
+    <string name="pref_summary_notification_sound">After saving database or preparing data to export, a notification sound is played.</string>
 
     <string-array name="pref_gain_max_radius_keys">
         <item>"0.3 m"</item>

app/android/res/values/styles.xml

@@ -0,0 +1,10 @@
+<resources>
+    <style name="ThemeActionBar" parent="@android:style/Widget.DeviceDefault.ActionBar.Solid">
+        <item name="android:background">#20000000</item>
+    </style>
+
+    <style name="ThemeApp" parent="@android:style/Theme.DeviceDefault">
+        <item name="android:actionBarStyle">@style/ThemeActionBar</item>
+        <item name="android:windowActionBarOverlay">true</item>
+    </style>
+</resources>

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

@@ -0,0 +1,43 @@
+package com.introlab.rtabmap;
+
+import android.content.Context;
+import android.util.AttributeSet;
+import android.widget.Spinner;
+
+
+/** Spinner extension that calls onItemSelected even when the selection is the same as its previous value 
+Author: Mattia Ruggiero 
+https://stackoverflow.com/questions/5335306/how-can-i-get-an-event-in-android-spinner-when-the-current-selected-item-is-sele
+*/
+public class NDSpinner extends Spinner {
+
+    public NDSpinner(Context context)
+    { super(context); }
+
+    public NDSpinner(Context context, AttributeSet attrs)
+    { super(context, attrs); }
+
+    public NDSpinner(Context context, AttributeSet attrs, int defStyle)
+    { super(context, attrs, defStyle); }
+
+    @Override 
+    public void setSelection(int position, boolean animate) {
+        boolean sameSelected = position == getSelectedItemPosition();
+        super.setSelection(position, animate);
+        if (sameSelected) {
+            // Spinner does not call the OnItemSelectedListener if the same item is selected, so do it manually now
+            //getOnItemSelectedListener().onItemSelected(this, getSelectedView(), position, getSelectedItemId());
+        }
+    } 
+
+    @Override
+    public void setSelection(int position) {
+        boolean sameSelected = position == getSelectedItemPosition();
+        super.setSelection(position);
+        if (sameSelected) {
+            // Spinner does not call the OnItemSelectedListener if the same item is selected, so do it manually now
+            getOnItemSelectedListener().onItemSelected(this, getSelectedView(), position, getSelectedItemId());
+        }
+    }
+
+}

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

@@ -89,6 +89,9 @@ import android.view.WindowManager;
 import android.view.inputmethod.EditorInfo;
 import android.webkit.WebView;
 import android.webkit.WebViewClient;
+import android.widget.AdapterView;
+import android.widget.AdapterView.OnItemSelectedListener;
+import android.widget.ArrayAdapter;
 import android.widget.Button;
 import android.widget.EditText;
 import android.widget.LinearLayout;
@@ -96,6 +99,7 @@ import android.widget.NumberPicker;
 import android.widget.RelativeLayout;
 import android.widget.SeekBar;
 import android.widget.SeekBar.OnSeekBarChangeListener;
+import android.widget.Spinner;
 import android.widget.TextView;
 import android.widget.Toast;
 import android.widget.ToggleButton;
@@ -104,7 +108,7 @@ import com.google.atap.tangoservice.Tango;
 
 // The main activity of the application. This activity shows debug information
 // and a glSurfaceView that renders graphic content.
-public class RTABMapActivity extends Activity implements OnClickListener {
+public class RTABMapActivity extends Activity implements OnClickListener, OnItemSelectedListener {
 
 	// Tag for debug logging.
 	public static final String TAG = RTABMapActivity.class.getSimpleName();
@@ -134,7 +138,6 @@ public class RTABMapActivity extends Activity implements OnClickListener {
 	
 	public static final long NOTOUCH_TIMEOUT = 5000; // 5 sec
 	private boolean mHudVisible = true;
-	private boolean mTipOrthoShown_ = false;
 	private int mSavedRenderingType = 0;
 	private boolean mMenuOpened = false;
 
@@ -182,9 +185,7 @@ public class RTABMapActivity extends Activity implements OnClickListener {
 	private MenuItem mItemStatusVisibility;
 	private MenuItem mItemDebugVisibility;
 
-	private ToggleButton mButtonFirst;
-	private ToggleButton mButtonThird;
-	private ToggleButton mButtonTop;
+	private NDSpinner mButtonCameraView;
 	private ToggleButton mButtonPause;
 	private ToggleButton mButtonLighting;
 	private ToggleButton mButtonWireframe;
@@ -192,7 +193,8 @@ public class RTABMapActivity extends Activity implements OnClickListener {
 	private Button mButtonCloseVisualization;
 	private Button mButtonSaveOnDevice;
 	private Button mButtonShareOnSketchfab;
-	private SeekBar mSeekBarFov;
+	private SeekBar mSeekBarOrthoCut;
+	private SeekBar mSeekBarGrid;
 
 	private String mOpenedDatabasePath = "";
 	private String mWorkingDirectory = "";
@@ -255,6 +257,7 @@ public class RTABMapActivity extends Activity implements OnClickListener {
 		display.getSize(mScreenSize);
 
 		getWindow().addFlags(WindowManager.LayoutParams.FLAG_KEEP_SCREEN_ON);
+		getWindow().addFlags(WindowManager.LayoutParams.FLAG_TRANSLUCENT_STATUS);
 		
 		// Setting content view of this activity.
 		setContentView(R.layout.activity_rtabmap);
@@ -267,9 +270,7 @@ public class RTABMapActivity extends Activity implements OnClickListener {
 		mActionBarHeight = getActionBarHeight();
 	
 		// Buttons for selecting camera view and Set up button click listeners.
-		mButtonFirst = (ToggleButton)findViewById(R.id.first_person_button);
-		mButtonThird = (ToggleButton)findViewById(R.id.third_person_button);
-		mButtonTop = (ToggleButton)findViewById(R.id.top_down_button);
+		mButtonCameraView = (NDSpinner)findViewById(R.id.camera_button);
 		mButtonPause = (ToggleButton)findViewById(R.id.pause_button);
 		mButtonLighting = (ToggleButton)findViewById(R.id.light_button);
 		mButtonWireframe = (ToggleButton)findViewById(R.id.wireframe_button);
@@ -277,9 +278,7 @@ public class RTABMapActivity extends Activity implements OnClickListener {
 		mButtonCloseVisualization = (Button)findViewById(R.id.close_visualization_button);
 		mButtonSaveOnDevice = (Button)findViewById(R.id.button_saveOnDevice);
 		mButtonShareOnSketchfab = (Button)findViewById(R.id.button_shareToSketchfab);
-		mButtonFirst.setOnClickListener(this);
-		mButtonThird.setOnClickListener(this);
-		mButtonTop.setOnClickListener(this);
+		mButtonCameraView.setOnItemSelectedListener(this);
 		mButtonPause.setOnClickListener(this);
 		mButtonLighting.setOnClickListener(this);
 		mButtonWireframe.setOnClickListener(this);
@@ -287,7 +286,6 @@ public class RTABMapActivity extends Activity implements OnClickListener {
 		mButtonCloseVisualization.setOnClickListener(this);
 		mButtonSaveOnDevice.setOnClickListener(this);
 		mButtonShareOnSketchfab.setOnClickListener(this);
-		mButtonFirst.setChecked(true);
 		mButtonLighting.setChecked(false);
 		mButtonLighting.setVisibility(View.INVISIBLE);
 		mButtonWireframe.setChecked(false);
@@ -300,20 +298,43 @@ public class RTABMapActivity extends Activity implements OnClickListener {
 			mButtonBackfaceShown.setVisibility(mItemRenderingMesh.isChecked() || mItemRenderingTextureMesh.isChecked()?View.VISIBLE:View.INVISIBLE);
 		}
 		
-		mSeekBarFov = (SeekBar)findViewById(R.id.seekBar_fov);
-		mSeekBarFov.setMax(45);
-		mSeekBarFov.setProgress(20);
-		mSeekBarFov.setOnSeekBarChangeListener(new OnSeekBarChangeListener() {
+		ArrayAdapter<CharSequence> adapter = ArrayAdapter.createFromResource(this, R.array.camera_view_array, android.R.layout.simple_spinner_item);
+		adapter.setDropDownViewResource(android.R.layout.simple_spinner_dropdown_item);
+		mButtonCameraView.setAdapter(adapter);
+		mButtonCameraView.setOnTouchListener(new OnTouchListener() {
+		    @Override
+		    public boolean onTouch(View v, MotionEvent event) {
+		        resetNoTouchTimer();
+		        return false;
+		    }
+		});
+		
+		mSeekBarOrthoCut = (SeekBar)findViewById(R.id.seekBar_ortho_cut);
+		mSeekBarOrthoCut.setMax(120);
+		mSeekBarOrthoCut.setProgress(80);
+		mSeekBarOrthoCut.setOnSeekBarChangeListener(new OnSeekBarChangeListener() {
 			  @Override
 			  public void onProgressChanged(SeekBar seekBar, int progressValue, boolean fromUser) {
-				  if(mButtonFirst.isChecked())
-				  {
-					  RTABMapLib.setFOV((float)progressValue+45.0f);
-				  }
-				  else if(mButtonTop.isChecked())
-				  {
-					  RTABMapLib.setOrthoCropFactor((float)(120-progressValue)/20.0f - 3.0f);
-				  }
+				  RTABMapLib.setOrthoCropFactor((float)(120-progressValue)/20.0f - 3.0f);
+				  resetNoTouchTimer();
+			  }
+			
+			  @Override
+			  public void onStartTrackingTouch(SeekBar seekBar) {
+			  }
+			
+			  @Override
+			  public void onStopTrackingTouch(SeekBar seekBar) {
+			  }
+		   });
+		
+		mSeekBarGrid = (SeekBar)findViewById(R.id.seekBar_grid);
+		mSeekBarGrid.setMax(180);
+		mSeekBarGrid.setProgress(90);
+		mSeekBarGrid.setOnSeekBarChangeListener(new OnSeekBarChangeListener() {
+			  @Override
+			  public void onProgressChanged(SeekBar seekBar, int progressValue, boolean fromUser) {
+				  RTABMapLib.setGridRotation(((float)progressValue-90.0f)/2.0f);
 				  resetNoTouchTimer();
 			  }
 			
@@ -340,7 +361,7 @@ public class RTABMapActivity extends Activity implements OnClickListener {
 	        @Override
 	        public boolean onTouch(View v, MotionEvent event) {
 	        	
-	        	resetNoTouchTimer();
+	        	resetNoTouchTimer(getActionBar().isShowing() && mHudVisible == false);
 	        	
 	            mGesDetect.onTouchEvent(event);
 	            
@@ -701,7 +722,7 @@ public class RTABMapActivity extends Activity implements OnClickListener {
 		}
 		
 		TangoInitializationHelper.bindTangoService(getActivity(), mTangoServiceConnection);
-		resetNoTouchTimer();
+		resetNoTouchTimer(true);
 	}
 	
 	private void setCamera(int type)
@@ -714,28 +735,13 @@ public class RTABMapActivity extends Activity implements OnClickListener {
 		updateStatusTexts();
 				
 		RTABMapLib.setCamera(type);
-		mButtonFirst.setChecked(type==0);
-		mButtonThird.setChecked(type==1);
-		mButtonTop.setChecked(type==2 || type==3);
-		mButtonTop.setText(type==3?"Ortho":"Top");
-		mButtonTop.setTextOn(type==3?"Ortho":"Top");
-		mButtonTop.setTextOff(type==3?"Ortho":"Top");
-		mSeekBarFov.setVisibility(type!=0 && type!=3?View.INVISIBLE:View.VISIBLE);
-		if(type==0)
-		{
-			mSeekBarFov.setMax(45);
-			mSeekBarFov.setProgress(20);
-		}
+		mButtonCameraView.setSelection(type, true);
+		mSeekBarOrthoCut.setVisibility(type!=3?View.INVISIBLE:View.VISIBLE);
+		mSeekBarGrid.setVisibility(mSeekBarGrid.isEnabled() && type==3?View.VISIBLE:View.INVISIBLE);
 		if(type==3)
 		{
-			mSeekBarFov.setMax(120);
-			mSeekBarFov.setProgress(80);
-		}
-		
-		if(type==2 && !mTipOrthoShown_)
-		{
-			mToast.makeText(this, "Tip: Click again on Top to set Ortho mode.", mToast.LENGTH_LONG).show();
-			mTipOrthoShown_ = true;
+			mSeekBarOrthoCut.setMax(120);
+			mSeekBarOrthoCut.setProgress(80);
 		}
 	}
 
@@ -743,15 +749,8 @@ public class RTABMapActivity extends Activity implements OnClickListener {
 	public void onClick(View v) {
 		// Handle button clicks.
 		switch (v.getId()) {
-		case R.id.first_person_button:
-			setCamera(0);
+		case R.id.gl_surface_view:
 
-			break;
-		case R.id.third_person_button:
-			setCamera(1);
-			break;
-		case R.id.top_down_button:
-			setCamera(mButtonFirst.isChecked() || mButtonThird.isChecked() || mButtonTop.getTextOn().equals("Ortho")?2:3);
 			break;
 		case R.id.pause_button:
 			pauseMapping();
@@ -793,6 +792,15 @@ public class RTABMapActivity extends Activity implements OnClickListener {
 		resetNoTouchTimer();
 	}
 	
+	public void onItemSelected(AdapterView<?> parent, View view, int pos, long id) {
+		setCamera(pos);
+		resetNoTouchTimer();
+    }
+
+    public void onNothingSelected(AdapterView<?> parent) {
+    	resetNoTouchTimer();
+    }
+		
 	private void setAndroidOrientation() {
         Display display = getWindowManager().getDefaultDisplay();
         Camera.CameraInfo colorCameraInfo = new Camera.CameraInfo();
@@ -806,11 +814,26 @@ public class RTABMapActivity extends Activity implements OnClickListener {
 
         @Override
         public boolean onDoubleTap(MotionEvent event) {
+        	if(!DISABLE_LOG) Log.i(TAG, "onDoubleTap");
         	float normalizedX = event.getX(0) / mScreenSize.x;
 			float normalizedY = event.getY(0) / mScreenSize.y;
         	RTABMapLib.onTouchEvent(3, event.getActionMasked(), normalizedX, normalizedY, 0.0f, 0.0f);
             return true;
         }
+        @Override
+        public boolean onSingleTapConfirmed(MotionEvent event) {
+        	if(!DISABLE_LOG) Log.i(TAG, "onSingleTapConfirmed");
+        	if(mHudVisible)
+			{
+				notouchHandler.removeCallbacks(notouchCallback);
+				notouchHandler.postDelayed(notouchCallback, 0);
+			}
+			else
+			{
+				resetNoTouchTimer(true);
+			}
+            return true;
+        }
     }
 
 	@Override
@@ -1146,9 +1169,9 @@ public class RTABMapActivity extends Activity implements OnClickListener {
 			}
 			
 			updateState(State.STATE_VISUALIZING_WHILE_LOADING);
-			if(mButtonFirst.isChecked())
+			if(mButtonCameraView.getSelectedItemPosition() == 0)
 			{
-				setCamera(1);
+				setCamera(2);
 			}
 			mToast.makeText(getActivity(), String.format("Optimized mesh detected in the database, it is shown while the database is loading..."), mToast.LENGTH_LONG).show();
 			mProgressDialog.dismiss();
@@ -1348,13 +1371,19 @@ public class RTABMapActivity extends Activity implements OnClickListener {
     };
 
     public void resetNoTouchTimer(){
-    	if(!mHudVisible)
+    	resetNoTouchTimer(false);
+    }
+    
+    public void resetNoTouchTimer(boolean showHud){
+    	if(showHud)
     	{
-    		setNavVisibility(true);
     		mHudVisible = true;
-    		updateState(mState);
+			setNavVisibility(true);
+			if(mItemSave != null)
+			{
+				updateState(mState);
+			}
     	}
-    	mHudVisible = true;
     		
         notouchHandler.removeCallbacks(notouchCallback);
         notouchHandler.postDelayed(notouchCallback, NOTOUCH_TIMEOUT);
@@ -1371,6 +1400,7 @@ public class RTABMapActivity extends Activity implements OnClickListener {
 			mToast.makeText(getActivity(), String.format("Re-adding %d online clouds, this may take some time...", mMapNodes), mToast.LENGTH_LONG).show();
 		}
 		mState = state;
+		if(!DISABLE_LOG) Log.i(TAG, String.format("updateState() state=%s hud=%d", state.toString(), mHudVisible?1:0));
 		switch(state)
 		{
 		case STATE_PROCESSING:
@@ -1438,11 +1468,10 @@ public class RTABMapActivity extends Activity implements OnClickListener {
 			mItemDataRecorderMode.setEnabled(mButtonPause.isChecked());
 			break;
 		}
-		mButtonFirst.setVisibility(mHudVisible?View.VISIBLE:View.INVISIBLE);
-		mButtonThird.setVisibility(mHudVisible?View.VISIBLE:View.INVISIBLE);
-		mButtonTop.setVisibility(mHudVisible?View.VISIBLE:View.INVISIBLE);
+		mButtonCameraView.setVisibility(mHudVisible?View.VISIBLE:View.INVISIBLE);
 		mButtonBackfaceShown.setVisibility(mHudVisible && (mItemRenderingMesh.isChecked() || mItemRenderingTextureMesh.isChecked())?View.VISIBLE:View.INVISIBLE);
-		mSeekBarFov.setVisibility(mHudVisible && (mButtonFirst.isChecked() || (mButtonTop.isChecked() && mButtonTop.getTextOn().equals("Ortho")))?View.VISIBLE:View.INVISIBLE);
+		mSeekBarOrthoCut.setVisibility(mHudVisible && mButtonCameraView.getSelectedItemPosition() == 3?View.VISIBLE:View.INVISIBLE);
+		mSeekBarGrid.setVisibility(mHudVisible && mSeekBarGrid.isEnabled() && mButtonCameraView.getSelectedItemPosition() == 3?View.VISIBLE:View.INVISIBLE);
 	}
 
 	private void pauseMapping() {
@@ -1682,6 +1711,8 @@ public class RTABMapActivity extends Activity implements OnClickListener {
 		else if(itemId == R.id.grid_visible)
 		{
 			item.setChecked(!item.isChecked());
+			mSeekBarGrid.setEnabled(item.isChecked());
+			mSeekBarGrid.setVisibility(mHudVisible && mSeekBarGrid.isEnabled()&&mButtonCameraView.getSelectedItemPosition() == 3?View.VISIBLE:View.INVISIBLE);
 			RTABMapLib.setGridVisible(item.isChecked());
 		}
 		else if (itemId == R.id.save)
@@ -1727,6 +1758,7 @@ public class RTABMapActivity extends Activity implements OnClickListener {
 							.setNegativeButton("No", new DialogInterface.OnClickListener() {
 								public void onClick(DialogInterface dialog, int which) {
 									dialog.dismiss();
+									resetNoTouchTimer(true);
 								}
 							})
 							.show();
@@ -1993,7 +2025,7 @@ public class RTABMapActivity extends Activity implements OnClickListener {
 		final int optimizedDepth = Integer.parseInt(sharedPref.getString(getString(R.string.pref_key_opt_depth), getString(R.string.pref_default_opt_depth)));
 		final float optimizedColorRadius = Float.parseFloat(sharedPref.getString(getString(R.string.pref_key_opt_color_radius), getString(R.string.pref_default_opt_color_radius)));
 		final boolean optimizedCleanWhitePolygons = sharedPref.getBoolean(getString(R.string.pref_key_opt_clean_white), Boolean.parseBoolean(getString(R.string.pref_default_opt_clean_white)));
-		final boolean optimizedColorWhitePolygons = false;//sharedPref.getBoolean("pref_key_opt_color_white", false); // not used
+		final int optimizedMinClusterSize = Integer.parseInt(sharedPref.getString(getString(R.string.pref_key_opt_min_cluster_size), getString(R.string.pref_default_opt_min_cluster_size)));
 		final boolean blockRendering = sharedPref.getBoolean(getString(R.string.pref_key_block_render), Boolean.parseBoolean(getString(R.string.pref_default_block_render)));
 
 		
@@ -2024,7 +2056,7 @@ public class RTABMapActivity extends Activity implements OnClickListener {
 						optimizedMaxPolygons,
 						optimizedColorRadius,
 						optimizedCleanWhitePolygons,
-						optimizedColorWhitePolygons,
+						optimizedMinClusterSize,
 						maxTextureDistance,
 						minTextureClusterSize,
 						blockRendering);
@@ -2041,6 +2073,25 @@ public class RTABMapActivity extends Activity implements OnClickListener {
 								
 								final long endTime = System.currentTimeMillis()/1000;
 								
+								if(endTime-startTime > 10)
+								{
+									// build notification
+									// the addAction re-use the same intent to keep the example short
+									SharedPreferences sharedPref = PreferenceManager.getDefaultSharedPreferences(getActivity());
+									boolean notifySound = sharedPref.getBoolean(getString(R.string.pref_key_notification_sound), Boolean.parseBoolean(getString(R.string.pref_default_notification_sound)));
+									Notification n  = new Notification.Builder(getActivity())
+									.setContentTitle(getString(R.string.app_name))
+									.setContentText("Data generated and ready to be exported!")
+									.setSmallIcon(R.drawable.ic_launcher)
+									.setDefaults(notifySound?Notification.DEFAULT_SOUND:0)
+									.setAutoCancel(true).build();
+						
+									NotificationManager notificationManager = 
+											(NotificationManager) getSystemService(NOTIFICATION_SERVICE);
+						
+									notificationManager.notify(0, n); 
+								}
+
 								// Visualize the result?
 								AlertDialog d = new AlertDialog.Builder(getActivity())
 								.setCancelable(false)
@@ -2048,7 +2099,7 @@ public class RTABMapActivity extends Activity implements OnClickListener {
 								.setMessage(Html.fromHtml("Do you want visualize the result before saving to file or sharing to <a href=\"https://sketchfab.com/about\">Sketchfab</a>?"))
 								.setPositiveButton("Yes", new DialogInterface.OnClickListener() {
 									public void onClick(DialogInterface dialog, int which) {
-										resetNoTouchTimer();
+										resetNoTouchTimer(true);
 										mSavedRenderingType = mItemRenderingPointCloud.isChecked()?0:mItemRenderingMesh.isChecked()?1:2;
 										if(!meshing)
 										{
@@ -2069,7 +2120,7 @@ public class RTABMapActivity extends Activity implements OnClickListener {
 										}
 										updateState(State.STATE_VISUALIZING);
 										RTABMapLib.postExportation(true);
-										if(mButtonFirst.isChecked())
+										if(mButtonCameraView.getSelectedItemPosition() == 0)
 										{
 											setCamera(2);
 										}
@@ -2096,6 +2147,7 @@ public class RTABMapActivity extends Activity implements OnClickListener {
 										})
 										.setNeutralButton("Cancel", new DialogInterface.OnClickListener() {
 											public void onClick(DialogInterface dialog, int which) {
+												resetNoTouchTimer(true);
 											}
 										})
 										.create();
@@ -2111,7 +2163,7 @@ public class RTABMapActivity extends Activity implements OnClickListener {
 							}
 							else
 							{
-								updateState(State.STATE_IDLE);
+								updateState(previousState);
 								mToast.makeText(getActivity(), String.format("Exporting map failed!"), mToast.LENGTH_LONG).show();
 							}
 							mExportProgressDialog.dismiss();
@@ -2142,45 +2194,51 @@ public class RTABMapActivity extends Activity implements OnClickListener {
 			mProgressDialog.setMessage(String.format("Please wait while saving \"%s\"...", newDatabasePathHuman));
 		}
 		mProgressDialog.show();
+		final State previousState = mState;
 		updateState(State.STATE_PROCESSING);
 		Thread saveThread = new Thread(new Runnable() {
 			public void run() {
 				RTABMapLib.save(newDatabasePath); // save
 				runOnUiThread(new Runnable() {
 					public void run() {
+						String msg;
 						if(mOpenedDatabasePath.equals(newDatabasePath))
 						{
-							mToast.makeText(getActivity(), String.format("Database \"%s\" updated.", newDatabasePathHuman), mToast.LENGTH_LONG).show();
+							msg = String.format("Database \"%s\" updated.", newDatabasePathHuman);	
 						}
 						else
 						{
-							mToast.makeText(getActivity(), String.format("Database saved to \"%s\".", newDatabasePathHuman), mToast.LENGTH_LONG).show();
-
-							Intent intent = new Intent(getActivity(), RTABMapActivity.class);
-							// use System.currentTimeMillis() to have a unique ID for the pending intent
-							PendingIntent pIntent = PendingIntent.getActivity(getActivity(), (int) System.currentTimeMillis(), intent, 0);
-
-							// build notification
-							// the addAction re-use the same intent to keep the example short
-							Notification n  = new Notification.Builder(getActivity())
-							.setContentTitle(getString(R.string.app_name))
-							.setContentText(newDatabasePathHuman + " saved!")
-							.setSmallIcon(R.drawable.ic_launcher)
-							.setContentIntent(pIntent)
-							.setAutoCancel(true).build();
-
-
-							NotificationManager notificationManager = 
-									(NotificationManager) getSystemService(NOTIFICATION_SERVICE);
-
-							notificationManager.notify(0, n); 
+							msg = String.format("Database saved to \"%s\".", newDatabasePathHuman);	
 						}
+						
+						mToast.makeText(getActivity(), msg, mToast.LENGTH_LONG).show();
+						
+						// build notification
+						Intent intent = new Intent(getActivity(), RTABMapActivity.class);
+						// use System.currentTimeMillis() to have a unique ID for the pending intent
+						PendingIntent pIntent = PendingIntent.getActivity(getActivity(), (int) System.currentTimeMillis(), intent, 0);
+						SharedPreferences sharedPref = PreferenceManager.getDefaultSharedPreferences(getActivity());
+						boolean notifySound = sharedPref.getBoolean(getString(R.string.pref_key_notification_sound), Boolean.parseBoolean(getString(R.string.pref_default_notification_sound)));
+						Notification n  = new Notification.Builder(getActivity())
+						.setContentTitle(getString(R.string.app_name))
+						.setContentText(msg)
+						.setSmallIcon(R.drawable.ic_launcher)
+						.setContentIntent(pIntent)
+						.setDefaults(notifySound?Notification.DEFAULT_SOUND:0)
+						.setAutoCancel(true).build();
+						
+						NotificationManager notificationManager = 
+								(NotificationManager) getSystemService(NOTIFICATION_SERVICE);
+
+						notificationManager.notify(0, n);
+						
+						resetNoTouchTimer(true);
 						if(!mItemDataRecorderMode.isChecked())
 						{
 							mOpenedDatabasePath = newDatabasePath;
 						}
 						mProgressDialog.dismiss();
-						updateState(State.STATE_IDLE);
+						updateState(previousState);
 					}
 				});
 			} 
@@ -2215,6 +2273,7 @@ public class RTABMapActivity extends Activity implements OnClickListener {
 			public void onClick(DialogInterface dialog, int which)
 			{
 				dialog.dismiss();
+				resetNoTouchTimer(true);
 			}
 		});
 		builder.setPositiveButton("Ok", new DialogInterface.OnClickListener() {
@@ -2339,11 +2398,12 @@ public class RTABMapActivity extends Activity implements OnClickListener {
 							.setContentIntent(pIntent)
 							.setAutoCancel(true).build();
 				
-				
 							NotificationManager notificationManager = 
 									(NotificationManager) getSystemService(NOTIFICATION_SERVICE);
 				
 							notificationManager.notify(0, n); 
+							
+							resetNoTouchTimer(true);
 						}
 					});
 				}
@@ -2353,6 +2413,7 @@ public class RTABMapActivity extends Activity implements OnClickListener {
 						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();
+							resetNoTouchTimer(true);
 						}
 					});
 				}
@@ -2435,15 +2496,15 @@ public class RTABMapActivity extends Activity implements OnClickListener {
 							if(status >= 1 && status<=3)
 							{
 								mProgressDialog.dismiss();
-								resetNoTouchTimer();
+								resetNoTouchTimer(true);
 								updateState(State.STATE_VISUALIZING);
 								mToast.makeText(getActivity(), String.format("Database loaded!"), mToast.LENGTH_LONG).show();
 							}
 							else if(!mItemTrajectoryMode.isChecked())
 							{
-								if(mButtonFirst.isChecked())
+								if(mButtonCameraView.getSelectedItemPosition() == 0)
 								{
-									setCamera(1);
+									setCamera(2);
 								}
 								// creating meshes...
 								updateState(State.STATE_IDLE);

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

@@ -80,6 +80,7 @@ public class RTABMapLib
     public static native void setPointSize(float value);
     public static native void setFOV(float value);
     public static native void setOrthoCropFactor(float value);
+    public static native void setGridRotation(float value);
     public static native void setLighting(boolean enabled);
     public static native void setBackfaceCulling(boolean enabled);
     public static native void setWireframe(boolean enabled);
@@ -108,7 +109,7 @@ public class RTABMapLib
     		int optimizedMaxPolygons,
     		float optimizedColorRadius,
     		boolean optimizedCleanWhitePolygons,
-    		boolean optimizedColorWhitePolygons,
+    		int optimizedMinClusterSize,
     		float optimizedMaxTextureDistance,
     		int optimizedMinTextureClusterSize,
     		boolean blockRendering);

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

@@ -215,6 +215,7 @@ public class SettingsActivity extends PreferenceActivity implements OnSharedPref
 
         ((Preference)findPreference(getString(R.string.pref_key_opt_depth))).setSummary("("+((ListPreference)findPreference(getString(R.string.pref_key_opt_depth))).getEntry() + ") "+getString(R.string.pref_summary_opt_depth));
         ((Preference)findPreference(getString(R.string.pref_key_opt_color_radius))).setSummary("("+((ListPreference)findPreference(getString(R.string.pref_key_opt_color_radius))).getEntry() + ") "+getString(R.string.pref_summary_opt_color_radius));
+        ((Preference)findPreference(getString(R.string.pref_key_opt_min_cluster_size))).setSummary("("+((ListPreference)findPreference(getString(R.string.pref_key_opt_min_cluster_size))).getEntry() + ") "+getString(R.string.pref_summary_opt_min_cluster_size));
 
         ((Preference)findPreference(getString(R.string.pref_key_cluster_ratio))).setSummary("("+((ListPreference)findPreference(getString(R.string.pref_key_cluster_ratio))).getEntry() + ") "+getString(R.string.pref_summary_cluster_ratio));
         ((Preference)findPreference(getString(R.string.pref_key_gain_max_radius))).setSummary("("+((ListPreference)findPreference(getString(R.string.pref_key_gain_max_radius))).getEntry() + ") "+getString(R.string.pref_summary_gain_max_radius));
@@ -272,6 +273,7 @@ public class SettingsActivity extends PreferenceActivity implements OnSharedPref
 
         	if(key.compareTo(getString(R.string.pref_key_opt_depth))==0) pref.setSummary("("+((ListPreference)pref).getEntry() + ") "+getString(R.string.pref_summary_opt_depth));
         	if(key.compareTo(getString(R.string.pref_key_opt_color_radius))==0) pref.setSummary("("+((ListPreference)pref).getEntry() + ") "+getString(R.string.pref_summary_opt_color_radius));
+        	if(key.compareTo(getString(R.string.pref_key_opt_min_cluster_size))==0) pref.setSummary("("+((ListPreference)pref).getEntry() + ") "+getString(R.string.pref_summary_opt_min_cluster_size));
         	
         	if(key.compareTo(getString(R.string.pref_key_cluster_ratio))==0) pref.setSummary("("+((ListPreference)pref).getEntry() + ") "+getString(R.string.pref_summary_cluster_ratio));
         	if(key.compareTo(getString(R.string.pref_key_gain_max_radius))==0) pref.setSummary("("+((ListPreference)pref).getEntry() + ") "+getString(R.string.pref_summary_gain_max_radius));

app/src/CMakeLists.txt

@@ -74,7 +74,11 @@ ELSE()
 ENDIF()
 TARGET_LINK_LIBRARIES(rtabmap rtabmap_core rtabmap_gui rtabmap_utilite ${LIBRARIES})
 IF(Qt5_FOUND)
-    QT5_USE_MODULES(rtabmap Widgets Core Gui Svg PrintSupport)
+    IF(Qt5Svg_FOUND)
+        QT5_USE_MODULES(rtabmap Widgets Core Gui Svg PrintSupport)
+    ELSE()
+        QT5_USE_MODULES(rtabmap Widgets Core Gui PrintSupport)
+    ENDIF()
 ENDIF(Qt5_FOUND)
 
 IF(APPLE AND BUILD_AS_BUNDLE)

cmake_modules/FindORB_SLAM2.cmake

@@ -9,12 +9,13 @@
 
 find_path(ORB_SLAM2_INCLUDE_DIR NAMES System.h PATHS $ENV{ORB_SLAM2_ROOT_DIR}/include)
 find_library(ORB_SLAM2_LIBRARY NAMES ORB_SLAM2 PATHS $ENV{ORB_SLAM2_ROOT_DIR}/lib)
+find_library(g2o_LIBRARY NAMES g2o PATHS $ENV{ORB_SLAM2_ROOT_DIR}/Thirdparty/g2o/lib)
 
-IF (ORB_SLAM2_INCLUDE_DIR AND ORB_SLAM2_LIBRARY)
+IF (ORB_SLAM2_INCLUDE_DIR AND ORB_SLAM2_LIBRARY AND g2o_LIBRARY)
    SET(ORB_SLAM2_FOUND TRUE)
    SET(ORB_SLAM2_INCLUDE_DIRS ${ORB_SLAM2_INCLUDE_DIR} $ENV{ORB_SLAM2_ROOT_DIR})
-   SET(ORB_SLAM2_LIBRARIES ${ORB_SLAM2_LIBRARY})
-ENDIF (ORB_SLAM2_INCLUDE_DIR AND ORB_SLAM2_LIBRARY)
+   SET(ORB_SLAM2_LIBRARIES ${g2o_LIBRARY} ${ORB_SLAM2_LIBRARY})
+ENDIF (ORB_SLAM2_INCLUDE_DIR AND ORB_SLAM2_LIBRARY AND g2o_LIBRARY)
 
 IF (ORB_SLAM2_FOUND)
    # show which ORB_SLAM2 was found only if not quiet

corelib/include/rtabmap/core/DBDriver.h

@@ -104,12 +104,20 @@ public:
 			const cv::Mat & cloud,
 			const std::map<int, Transform> & poses = std::map<int, Transform>(), // if we want to do localization afterward using optimized mesh
 			const std::vector<std::vector<std::vector<unsigned int> > > & polygons = std::vector<std::vector<std::vector<unsigned int> > >(),      // Textures -> polygons -> vertices
+#if PCL_VERSION_COMPARE(>=, 1, 8, 0)
+			const std::vector<std::vector<Eigen::Vector2f, Eigen::aligned_allocator<Eigen::Vector2f> > > & texCoords = std::vector<std::vector<Eigen::Vector2f, Eigen::aligned_allocator<Eigen::Vector2f> > >(), // Textures -> uv coords for each vertex of the polygons
+#else
 			const std::vector<std::vector<Eigen::Vector2f> > & texCoords = std::vector<std::vector<Eigen::Vector2f> >(), // Textures -> uv coords for each vertex of the polygons
+#endif
 			const cv::Mat & textures = cv::Mat()) const; // concatenated textures (assuming square textures with all same size);
 	cv::Mat loadOptimizedMesh(
 			std::map<int, Transform> * poses = 0,
 			std::vector<std::vector<std::vector<unsigned int> > > * polygons = 0,
+#if PCL_VERSION_COMPARE(>=, 1, 8, 0)
+			std::vector<std::vector<Eigen::Vector2f, Eigen::aligned_allocator<Eigen::Vector2f> > > * texCoords = 0,
+#else
 			std::vector<std::vector<Eigen::Vector2f> > * texCoords = 0,
+#endif
 			cv::Mat * textures = 0) const;
 
 public:
@@ -219,12 +227,20 @@ private:
 				const cv::Mat & cloud,
 				const std::map<int, Transform> & poses,
 				const std::vector<std::vector<std::vector<unsigned int> > > & polygons,
+#if PCL_VERSION_COMPARE(>=, 1, 8, 0)
+				const std::vector<std::vector<Eigen::Vector2f, Eigen::aligned_allocator<Eigen::Vector2f> > > & texCoords,
+#else
 				const std::vector<std::vector<Eigen::Vector2f> > & texCoords,
+#endif
 				const cv::Mat & textures) const = 0;
 	virtual cv::Mat loadOptimizedMeshQuery(
 				std::map<int, Transform> * poses,
 				std::vector<std::vector<std::vector<unsigned int> > > * polygons,
+#if PCL_VERSION_COMPARE(>=, 1, 8, 0)
+				std::vector<std::vector<Eigen::Vector2f, Eigen::aligned_allocator<Eigen::Vector2f> > > * texCoords,
+#else
 				std::vector<std::vector<Eigen::Vector2f> > * texCoords,
+#endif
 				cv::Mat * textures) const = 0;
 
 	// Load objects

corelib/include/rtabmap/core/Features2d.h

@@ -105,7 +105,7 @@ public:
 		kFeatureGfttBrief=6,
 		kFeatureBrisk=7,
 		kFeatureGfttOrb=8,  //new 0.10.11
-		kFeatureFreak=9};   //new 0.11.14
+		kFeatureKaze=9};    //new 0.13.2
 
 	static Feature2D * create(const ParametersMap & parameters = ParametersMap());
 	static Feature2D * create(Feature2D::Type type, const ParametersMap & parameters = ParametersMap()); // for convenience
@@ -433,27 +433,31 @@ private:
 	cv::Ptr<CV_BRISK> brisk_;
 };
 
-//FREAK
-class RTABMAP_EXP FREAK : public Feature2D
+//KAZE
+class RTABMAP_EXP KAZE : public Feature2D
 {
 public:
-	FREAK(const ParametersMap & parameters = ParametersMap());
-	virtual ~FREAK();
+	KAZE(const ParametersMap & parameters = ParametersMap());
+	virtual ~KAZE();
 
 	virtual void parseParameters(const ParametersMap & parameters);
-	virtual Feature2D::Type getType() const { return kFeatureFreak; }
+	virtual Feature2D::Type getType() const { return kFeatureKaze; }
 
 private:
 	virtual std::vector<cv::KeyPoint> generateKeypointsImpl(const cv::Mat & image, const cv::Rect & roi, const cv::Mat & mask = cv::Mat()) const;
 	virtual cv::Mat generateDescriptorsImpl(const cv::Mat & image, std::vector<cv::KeyPoint> & keypoints) const;
 
 private:
-	bool orientationNormalized_;
-	bool scaleNormalized_;
-	float patternScale_;
+	bool extended_;
+	bool upright_;
+	float threshold_;
 	int nOctaves_;
+	int nOctaveLayers_;
+	int diffusivity_;
 
-	cv::Ptr<CV_FREAK> _freak;
+#if CV_MAJOR_VERSION > 2
+	cv::Ptr<cv::KAZE> kaze_;
+#endif
 };
 
 

corelib/include/rtabmap/core/Memory.h

@@ -42,6 +42,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #include "rtabmap/utilite/UStl.h"
 #include <opencv2/core/core.hpp>
 #include <opencv2/features2d/features2d.hpp>
+#include <pcl/pcl_config.h>
 
 namespace rtabmap {
 
@@ -98,12 +99,20 @@ public:
 			const cv::Mat & cloud,
 			const std::map<int, Transform> & poses = std::map<int, Transform>(), // if we want to do localization afterward using optimized mesh
 			const std::vector<std::vector<std::vector<unsigned int> > > & polygons = std::vector<std::vector<std::vector<unsigned int> > >(),      // Textures -> polygons -> vertices
+#if PCL_VERSION_COMPARE(>=, 1, 8, 0)
+			const std::vector<std::vector<Eigen::Vector2f, Eigen::aligned_allocator<Eigen::Vector2f> > > & texCoords = std::vector<std::vector<Eigen::Vector2f, Eigen::aligned_allocator<Eigen::Vector2f> > >(), // Textures -> uv coords for each vertex of the polygons
+#else
 			const std::vector<std::vector<Eigen::Vector2f> > & texCoords = std::vector<std::vector<Eigen::Vector2f> >(), // Textures -> uv coords for each vertex of the polygons
+#endif
 			const cv::Mat & textures = cv::Mat()) const; // concatenated textures (assuming square textures with all same size)
 	cv::Mat loadOptimizedMesh(
 			std::map<int, Transform> * poses = 0,
 			std::vector<std::vector<std::vector<unsigned int> > > * polygons = 0,
+#if PCL_VERSION_COMPARE(>=, 1, 8, 0)
+			std::vector<std::vector<Eigen::Vector2f, Eigen::aligned_allocator<Eigen::Vector2f> > > * texCoords = 0,
+#else
 			std::vector<std::vector<Eigen::Vector2f> > * texCoords = 0,
+#endif
 			cv::Mat * textures = 0) const;
 	void emptyTrash();
 	void joinTrashThread();

corelib/include/rtabmap/core/OccupancyGrid.h

@@ -47,6 +47,7 @@ public:
 	float getMinMapSize() const {return minMapSize_;}
 	bool isGridFromDepth() const {return occupancyFromCloud_;}
 	bool isFullUpdate() const {return fullUpdate_;}
+	bool isMapFrameProjection() const {return projMapFrame_;}
 	const std::map<int, Transform> & addedNodes() const {return addedNodes_;}
 	int cacheSize() const {return (int)cache_.size();}
 

corelib/include/rtabmap/core/OdometryDVO.h

@@ -58,6 +58,7 @@ private:
 	dvo::core::RgbdCameraPyramid * camera_;
 	bool lost_;
 	Transform motionFromKeyFrame_;
+	Transform previousLocalTransform_;
 
 };
 

corelib/include/rtabmap/core/OdometryEvent.h

@@ -86,8 +86,6 @@ public:
 		return std::vector<float>();
 	}
 	const OdometryInfo & info() const {return _info;}
-	double rotVariance() const {return uMax3(_info.covariance.at<double>(3,3), _info.covariance.at<double>(4,4), _info.covariance.at<double>(5,5));}
-	double transVariance() const {return uMax3(_info.covariance.at<double>(0,0), _info.covariance.at<double>(1,1), _info.covariance.at<double>(2,2));}
 
 private:
 	SensorData _data;

corelib/include/rtabmap/core/OdometryFovis.h

@@ -60,6 +60,7 @@ private:
 	fovis::StereoDepth * stereoDepth_;
 	ParametersMap fovisParameters_;
 	bool lost_;
+	Transform previousLocalTransform_;
 };
 
 }

corelib/include/rtabmap/core/OdometryORBSLAM2.h

@@ -54,6 +54,7 @@ private:
 	ORBSLAM2System * orbslam2_;
 	ORB_SLAM2::System * system_;
 	bool firstFrame_;
+	Transform originLocalTransform_;
 
 };
 

corelib/include/rtabmap/core/OdometryViso2.h

@@ -54,6 +54,7 @@ private:
 	bool lost_;
 	bool keep_reference_frame_;
 	Transform reference_motion_;
+	Transform previousLocalTransform_;
 	ParametersMap viso2Parameters_;
 };
 

corelib/include/rtabmap/core/Parameters.h

@@ -168,7 +168,6 @@ typedef std::pair<std::string, std::string> ParametersPair;
 class RTABMAP_EXP Parameters
 {
     // Rtabmap parameters
-    RTABMAP_PARAM(Rtabmap, VhStrategy,                   int, 0,      "None 0, Similarity 1, Epipolar 2.");
     RTABMAP_PARAM(Rtabmap, PublishStats,                 bool, true,  "Publishing statistics.");
     RTABMAP_PARAM(Rtabmap, PublishLastSignature,         bool, true,  "Publishing last signature.");
     RTABMAP_PARAM(Rtabmap, PublishPdf,                   bool, true,  "Publishing pdf.");
@@ -225,9 +224,9 @@ class RTABMAP_EXP Parameters
     RTABMAP_PARAM(Kp, BadSignRatio,             float, 0.5,   "Bad signature ratio (less than Ratio x AverageWordsPerImage = bad).");
     RTABMAP_PARAM(Kp, NndrRatio,                float, 0.8,   "NNDR ratio (A matching pair is detected, if its distance is closer than X times the distance of the second nearest neighbor.)");
 #ifdef RTABMAP_NONFREE
-    RTABMAP_PARAM(Kp, DetectorStrategy,         int, 0,       "0=SURF 1=SIFT 2=ORB 3=FAST/FREAK 4=FAST/BRIEF 5=GFTT/FREAK 6=GFTT/BRIEF 7=BRISK 8=GFTT/ORB 9=FREAK.");
+    RTABMAP_PARAM(Kp, DetectorStrategy,         int, 0,       "0=SURF 1=SIFT 2=ORB 3=FAST/FREAK 4=FAST/BRIEF 5=GFTT/FREAK 6=GFTT/BRIEF 7=BRISK 8=GFTT/ORB 9=KAZE.");
 #else
-    RTABMAP_PARAM(Kp, DetectorStrategy,         int, 2,       "0=SURF 1=SIFT 2=ORB 3=FAST/FREAK 4=FAST/BRIEF 5=GFTT/FREAK 6=GFTT/BRIEF 7=BRISK 8=GFTT/ORB 9=FREAK.");
+    RTABMAP_PARAM(Kp, DetectorStrategy,         int, 2,       "0=SURF 1=SIFT 2=ORB 3=FAST/FREAK 4=FAST/BRIEF 5=GFTT/FREAK 6=GFTT/BRIEF 7=BRISK 8=GFTT/ORB 9=KAZE.");
 #endif
     RTABMAP_PARAM(Kp, TfIdfLikelihoodUsed,      bool, true,   "Use of the td-idf strategy to compute the likelihood.");
     RTABMAP_PARAM(Kp, Parallelized,             bool, true,   "If the dictionary update and signature creation were parallelized.");
@@ -295,22 +294,30 @@ class RTABMAP_EXP Parameters
     RTABMAP_PARAM(BRISK, Octaves,      int, 3,  "Detection octaves. Use 0 to do single scale.");
     RTABMAP_PARAM(BRISK, PatternScale, float, 1,"Apply this scale to the pattern used for sampling the neighbourhood of a keypoint.");
 
+    RTABMAP_PARAM(KAZE, Extended,       bool, false,   "Set to enable extraction of extended (128-byte) descriptor.");
+    RTABMAP_PARAM(KAZE, Upright,        bool, false,   "Set to enable use of upright descriptors (non rotation-invariant).");
+    RTABMAP_PARAM(KAZE, Threshold,      float, 0.001,  "Detector response threshold to accept point.");
+    RTABMAP_PARAM(KAZE, NOctaves,       int, 4,        "Maximum octave evolution of the image.");
+    RTABMAP_PARAM(KAZE, NOctaveLayers,  int, 4,        "Default number of sublevels per scale level.");
+    RTABMAP_PARAM(KAZE, Diffusivity,    int, 1,        "Diffusivity type: 0=DIFF_PM_G1, 1=DIFF_PM_G2, 2=DIFF_WEICKERT or 3=DIFF_CHARBONNIER.");
+
     // BayesFilter
     RTABMAP_PARAM(Bayes, VirtualPlacePriorThr, float, 0.9,  "Virtual place prior");
     RTABMAP_PARAM_STR(Bayes, PredictionLC, "0.1 0.36 0.30 0.16 0.062 0.0151 0.00255 0.000324 2.5e-05 1.3e-06 4.8e-08 1.2e-09 1.9e-11 2.2e-13 1.7e-15 8.5e-18 2.9e-20 6.9e-23", "Prediction of loop closures (Gaussian-like, here with sigma=1.6) - Format: {VirtualPlaceProb, LoopClosureProb, NeighborLvl1, NeighborLvl2, ...}.");
     RTABMAP_PARAM(Bayes, FullPredictionUpdate, bool, false, "Regenerate all the prediction matrix on each iteration (otherwise only removed/added ids are updated).");
 
     // Verify hypotheses
+    RTABMAP_PARAM(VhEp, Enabled, bool, false,       uFormat("Verify visual loop closure hypothesis by computing a fundamental matrix. This is done prior to transformation computation when %s is enabled.", kRGBDEnabled().c_str()));
     RTABMAP_PARAM(VhEp, MatchCountMin, int, 8,      "Minimum of matching visual words pairs to accept the loop hypothesis.");
     RTABMAP_PARAM(VhEp, RansacParam1,  float, 3,    "Fundamental matrix (see cvFindFundamentalMat()): Max distance (in pixels) from the epipolar line for a point to be inlier.");
-    RTABMAP_PARAM(VhEp, RansacParam2,  float, 0.99, "Fundamental matrix (see cvFindFundamentalMat()): Performance of the RANSAC.");
+    RTABMAP_PARAM(VhEp, RansacParam2,  float, 0.99, "Fundamental matrix (see cvFindFundamentalMat()): Performance of RANSAC.");
 
     // RGB-D SLAM
     RTABMAP_PARAM(RGBD, Enabled,                  bool, true,  "");
     RTABMAP_PARAM(RGBD, LinearUpdate,             float, 0.1,  "Minimum linear displacement to update the map. Rehearsal is done prior to this, so weights are still updated.");
     RTABMAP_PARAM(RGBD, AngularUpdate,            float, 0.1,  "Minimum angular displacement to update the map. Rehearsal is done prior to this, so weights are still updated.");
     RTABMAP_PARAM(RGBD, NewMapOdomChangeDistance, float, 0,    "A new map is created if a change of odometry translation greater than X m is detected (0 m = disabled).");
-    RTABMAP_PARAM(RGBD, OptimizeFromGraphEnd,     bool, false, "Optimize graph from the newest node. If false, the graph is optimized from the oldest node of the current graph (this adds an overhead computation to detect to oldest mode of the current graph, but it can be useful to preserve the map referential from the oldest node). Warning when set to false: when some nodes are transferred, the first referential of the local map may change, resulting in momentary changes in robot/map position (which are annoying in teleoperation).");
+    RTABMAP_PARAM(RGBD, OptimizeFromGraphEnd,     bool, false, "Optimize graph from the newest node. If false, the graph is optimized from the oldest node of the current graph (this adds an overhead computation to detect to oldest node of the current graph, but it can be useful to preserve the map referential from the oldest node). Warning when set to false: when some nodes are transferred, the first referential of the local map may change, resulting in momentary changes in robot/map position (which are annoying in teleoperation).");
     RTABMAP_PARAM(RGBD, OptimizeMaxError,         float, 1,    uFormat("Reject loop closures if optimization error is greater than this value (0=disabled). This will help to detect when a wrong loop closure is added to the graph. Not compatible with \"%s\" if enabled.", kOptimizerRobust().c_str()));
     RTABMAP_PARAM(RGBD, GoalReachedRadius,        float, 0.5,  "Goal reached radius (m).");
     RTABMAP_PARAM(RGBD, PlanStuckIterations,      int, 0,      "Mark the current goal node on the path as unreachable if it is not updated after X iterations (0=disabled). If all upcoming nodes on the path are unreachabled, the plan fails.");
@@ -469,12 +476,12 @@ class RTABMAP_EXP Parameters
 #ifndef RTABMAP_NONFREE
 #ifdef RTABMAP_OPENCV3
     // OpenCV 3 without xFeatures2D module doesn't have BRIEF
-    RTABMAP_PARAM(Vis, FeatureType, int, 8, "0=SURF 1=SIFT 2=ORB 3=FAST/FREAK 4=FAST/BRIEF 5=GFTT/FREAK 6=GFTT/BRIEF 7=BRISK 8=GFTT/ORB 9=FREAK.");
+    RTABMAP_PARAM(Vis, FeatureType, int, 8, "0=SURF 1=SIFT 2=ORB 3=FAST/FREAK 4=FAST/BRIEF 5=GFTT/FREAK 6=GFTT/BRIEF 7=BRISK 8=GFTT/ORB 9=KAZE.");
 #else
-    RTABMAP_PARAM(Vis, FeatureType, int, 6, "0=SURF 1=SIFT 2=ORB 3=FAST/FREAK 4=FAST/BRIEF 5=GFTT/FREAK 6=GFTT/BRIEF 7=BRISK 8=GFTT/ORB 9=FREAK.");
+    RTABMAP_PARAM(Vis, FeatureType, int, 6, "0=SURF 1=SIFT 2=ORB 3=FAST/FREAK 4=FAST/BRIEF 5=GFTT/FREAK 6=GFTT/BRIEF 7=BRISK 8=GFTT/ORB 9=KAZE.");
 #endif
 #else
-    RTABMAP_PARAM(Vis, FeatureType, int, 6, "0=SURF 1=SIFT 2=ORB 3=FAST/FREAK 4=FAST/BRIEF 5=GFTT/FREAK 6=GFTT/BRIEF 7=BRISK 8=GFTT/ORB 9=FREAK.");
+    RTABMAP_PARAM(Vis, FeatureType, int, 6, "0=SURF 1=SIFT 2=ORB 3=FAST/FREAK 4=FAST/BRIEF 5=GFTT/FREAK 6=GFTT/BRIEF 7=BRISK 8=GFTT/ORB 9=KAZE.");
 #endif
     RTABMAP_PARAM(Vis, MaxFeatures,              int, 1000,   "0 no limits.");
     RTABMAP_PARAM(Vis, MaxDepth,                  float, 0,   "Max depth of the features (0 means no limit).");

corelib/include/rtabmap/core/Registration.h

@@ -25,8 +25,8 @@ ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
 SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 */
 
-#ifndef REGISTRATION_H_
-#define REGISTRATION_H_
+#ifndef RTABMAP_REGISTRATION_H_
+#define RTABMAP_REGISTRATION_H_
 
 #include "rtabmap/core/RtabmapExp.h" // DLL export/import defines
 
@@ -76,7 +76,7 @@ public:
 	Transform computeTransformation(
 			const SensorData & from,
 			const SensorData & to,
-			Transform SensorData = Transform::getIdentity(),
+			Transform guess = Transform::getIdentity(),
 			RegistrationInfo * info = 0) const;
 
 	Transform computeTransformationMod(
@@ -115,4 +115,4 @@ private:
 
 }
 
-#endif /* REGISTRATION_H_ */
+#endif /* RTABMAP_REGISTRATION_H_ */

corelib/include/rtabmap/core/Rtabmap.h

@@ -215,6 +215,7 @@ private:
 	unsigned int _maxMemoryAllowed; // signatures count in WM
 	float _loopThr;
 	float _loopRatio;
+	bool _verifyLoopClosureHypothesis;
 	unsigned int _maxRetrieved;
 	unsigned int _maxLocalRetrieved;
 	bool _rawDataKept;

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

@@ -121,7 +121,9 @@ typename pcl::PointCloud<PointT>::Ptr OccupancyGrid::segmentCloud(
 		{
 			UDEBUG("");
 			// passthrough filter
-			groundIndices = rtabmap::util3d::passThrough(cloud, indices, "z", minGroundHeight_!=0.0f?minGroundHeight_:std::numeric_limits<int>::min(), maxGroundHeight_);
+			groundIndices = rtabmap::util3d::passThrough(cloud, indices, "z",
+					minGroundHeight_!=0.0f?minGroundHeight_:std::numeric_limits<int>::min(),
+					maxGroundHeight_!=0.0f?maxGroundHeight_:std::numeric_limits<int>::max());
 
 			pcl::IndicesPtr notObstacles = groundIndices;
 			if(indices->size())

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

@@ -97,8 +97,7 @@ void denseMeshPostProcessing(
 
 	if(cloud.get()!=0 &&
 		!hasColors &&
-		transferColorRadius >= 0.0 &&
-		coloredOutput)
+		transferColorRadius >= 0.0)
 	{
 		if(progressState) progressState->callback(uFormat("Transferring color from point cloud to mesh..."));
 
@@ -179,63 +178,8 @@ void denseMeshPostProcessing(
 		}
 		hasColors = true;
 	}
-	else if(cloud.get()!=0 &&
-			!hasColors &&
-			transferColorRadius > 0.0 &&
-			cleanMesh &&
-			!coloredOutput)
-	{
-		if(progressState) progressState->callback(uFormat("Removing polygons too far from the cloud..."));
 
-		// transfer color from point cloud to mesh
-		typename pcl::search::KdTree<pointRGBT>::Ptr tree (new pcl::search::KdTree<pointRGBT>(true));
-		tree->setInputCloud(cloud);
-		pcl::PointCloud<pcl::PointNormal>::Ptr optimizedCloud(new pcl::PointCloud<pcl::PointNormal>);
-		pcl::fromPCLPointCloud2(mesh->cloud, *optimizedCloud);
-		std::vector<bool> closePts(optimizedCloud->size());
-		for(unsigned int i=0; i<optimizedCloud->size(); ++i)
-		{
-			std::vector<int> kIndices;
-			std::vector<float> kDistances;
-			pointRGBT pt;
-			pt.x = optimizedCloud->at(i).x;
-			pt.y = optimizedCloud->at(i).y;
-			pt.z = optimizedCloud->at(i).z;
-			tree->radiusSearch(pt, transferColorRadius, kIndices, kDistances);
-			if(kIndices.size())
-			{
-				closePts.at(i) = true;
-			}
-			else
-			{
-				closePts.at(i) = false;
-			}
-		}
-
-		// remove far polygons
-		std::vector<pcl::Vertices> filteredPolygons(mesh->polygons.size());
-		int oi=0;
-		for(unsigned int i=0; i<mesh->polygons.size(); ++i)
-		{
-			bool keepPolygon = true;
-			for(unsigned int j=0; j<mesh->polygons[i].vertices.size(); ++j)
-			{
-				if(!closePts.at(mesh->polygons[i].vertices[j]))
-				{
-					keepPolygon = false;
-					break;
-				}
-			}
-			if(keepPolygon)
-			{
-				filteredPolygons[oi++] = mesh->polygons[i];
-			}
-		}
-		filteredPolygons.resize(oi);
-		mesh->polygons = filteredPolygons;
-	}
-
-	if(minClusterSize && coloredOutput && !cleanMesh)
+	if(minClusterSize)
 	{
 		if(progressState) progressState->callback(uFormat("Filter small polygon clusters..."));
 
@@ -290,7 +234,7 @@ void denseMeshPostProcessing(
 		int before = (int)mesh->polygons.size();
 		mesh->polygons = filteredPolygons;
 
-		if(progressState) progressState->callback(uFormat("Filtered %1 polygons.", before-(int)mesh->polygons.size()));
+		if(progressState) progressState->callback(uFormat("Filtered %d polygons.", before-(int)mesh->polygons.size()));
 	}
 
 	// compute normals for the mesh if not already here, add also white color if colored output is required

corelib/include/rtabmap/core/util3d_surface.h

@@ -292,7 +292,7 @@ void denseMeshPostProcessing(
 		int maximumPolygons = 0,           // 0=disabled
 		const typename pcl::PointCloud<pointRGBT>::Ptr & cloud = pcl::PointCloud<pointRGBT>::Ptr(), // A RGB point cloud used to transfer colors back to mesh (needed for parameters below)
 		float transferColorRadius = 0.05f, // <0=disabled, 0=nearest color
-		bool coloredOutput = true,         // If output should be colored
+		bool coloredOutput = true,         // Not used anymore, output is colored if transferColorRadius>=0
 		bool cleanMesh = true,             // Remove polygons not colored (if coloredOutput is disabled, transferColorRadius is still used to clean the mesh)
 		int minClusterSize = 50,           // Remove small polygon clusters after the mesh has been cleaned (0=disabled)
 		ProgressState * progressState = 0);

corelib/src/CMakeLists.txt

@@ -344,7 +344,7 @@ SET(RESOURCES_HEADERS
 IF(ANDROID)
 
    IF(NOT RTABMAP_RES_TOOL)
-      find_host_program(RTABMAP_RES_TOOL rtabmap-res_tool)
+      find_host_program(RTABMAP_RES_TOOL rtabmap-res_tool PATHS ${CMAKE_RUNTIME_OUTPUT_DIRECTORY})
       IF(NOT RTABMAP_RES_TOOL)
          MESSAGE( FATAL_ERROR "RTABMAP_RES_TOOL is not defined (it is the path to \"rtabmap-res_tool\" application created by a non-Android build)." )
       ENDIF(NOT RTABMAP_RES_TOOL)

corelib/src/CameraStereo.cpp

@@ -860,18 +860,27 @@ bool CameraStereoZed::init(const std::string & calibrationFolder, const std::str
 	param.depth_minimum_distance=-1;
 	param.camera_disable_self_calib=!selfCalibration_;
 
+	sl::ERROR_CODE r = sl::ERROR_CODE::SUCCESS;
 	if(src_ == CameraVideo::kVideoFile)
 	{
 		UINFO("svo file = %s", svoFilePath_.c_str());
 		zed_ = new sl::Camera(); // Use in SVO playback mode
 		param.svo_input_filename=svoFilePath_.c_str();
-		zed_->open(param);
+		r = zed_->open(param);
 	}
 	else
 	{
 		UINFO("Resolution=%d imagerate=%f device=%d", resolution_, getImageRate(), usbDevice_);
 		zed_ = new sl::Camera(); // Use in Live Mode
-		zed_->open(param);
+		r = zed_->open(param);
+	}
+
+	if(r!=sl::ERROR_CODE::SUCCESS)
+	{
+		UERROR("Camera initialization failed: \"%s\"", errorCode2str(r).c_str());
+		delete zed_;
+		zed_ = 0;
+		return false;
 	}
 
 
@@ -979,11 +988,7 @@ SensorData CameraStereoZed::captureImage(CameraInfo * info)
 {
 	SensorData data;
 #ifdef RTABMAP_ZED
-	sl::RuntimeParameters rparam;
-	rparam.sensing_mode=(sl::SENSING_MODE)sensingMode_;
-	rparam.enable_depth=quality_ > 0;
-	rparam.enable_point_cloud=quality_ > 0;
-	rparam.move_point_cloud_to_world_frame=false;
+	sl::RuntimeParameters rparam((sl::SENSING_MODE)sensingMode_, quality_ > 0, quality_ > 0, sl::REFERENCE_FRAME_CAMERA);
 	if(zed_)
 	{
 		UTimer timer;

corelib/src/DBDriver.cpp

@@ -1043,7 +1043,11 @@ void DBDriver::saveOptimizedMesh(
 			const cv::Mat & cloud,
 			const std::map<int, Transform> & poses,
 			const std::vector<std::vector<std::vector<unsigned int> > > & polygons,
+#if PCL_VERSION_COMPARE(>=, 1, 8, 0)
+			const std::vector<std::vector<Eigen::Vector2f, Eigen::aligned_allocator<Eigen::Vector2f> > > & texCoords,
+#else
 			const std::vector<std::vector<Eigen::Vector2f> > & texCoords,
+#endif
 			const cv::Mat & textures) const
 {
 	_dbSafeAccessMutex.lock();
@@ -1054,7 +1058,11 @@ void DBDriver::saveOptimizedMesh(
 cv::Mat DBDriver::loadOptimizedMesh(
 				std::map<int, Transform> * poses,
 				std::vector<std::vector<std::vector<unsigned int> > > * polygons,
+#if PCL_VERSION_COMPARE(>=, 1, 8, 0)
+				std::vector<std::vector<Eigen::Vector2f, Eigen::aligned_allocator<Eigen::Vector2f>> > * texCoords,
+#else
 				std::vector<std::vector<Eigen::Vector2f> > * texCoords,
+#endif
 				cv::Mat * textures) const
 {
 	_dbSafeAccessMutex.lock();

corelib/src/DBDriverSqlite3.cpp

@@ -428,7 +428,7 @@ void DBDriverSqlite3::disconnectDatabaseQuery(bool save, const std::string & out
 			}
 			if(outputFile.empty())
 			{
-				UERROR("Database was initialized with an empty url (in memory). To save it "
+				UERROR("Database was initialized with an empty url (in memory). To save it, "
 						"the output url should not be empty. The database is thus closed without being saved!");
 			}
 			else
@@ -3833,7 +3833,11 @@ void DBDriverSqlite3::saveOptimizedMeshQuery(
 			const cv::Mat & cloud,
 			const std::map<int, Transform> & poses,
 			const std::vector<std::vector<std::vector<unsigned int> > > & polygons,
+#if PCL_VERSION_COMPARE(>=, 1, 8, 0)
+			const std::vector<std::vector<Eigen::Vector2f, Eigen::aligned_allocator<Eigen::Vector2f> > > & texCoords,
+#else
 			const std::vector<std::vector<Eigen::Vector2f> > & texCoords,
+#endif
 			const cv::Mat & textures) const
 {
 	UDEBUG("");
@@ -4049,7 +4053,11 @@ void DBDriverSqlite3::saveOptimizedMeshQuery(
 cv::Mat DBDriverSqlite3::loadOptimizedMeshQuery(
 			std::map<int, Transform> * poses,
 			std::vector<std::vector<std::vector<unsigned int> > > * polygons,
+#if PCL_VERSION_COMPARE(>=, 1, 8, 0)
+			std::vector<std::vector<Eigen::Vector2f, Eigen::aligned_allocator<Eigen::Vector2f> > > * texCoords,
+#else
 			std::vector<std::vector<Eigen::Vector2f> > * texCoords,
+#endif
 			cv::Mat * textures) const
 {
 	UDEBUG("");
@@ -4158,7 +4166,11 @@ cv::Mat DBDriverSqlite3::loadOptimizedMeshQuery(
 						for(int t=0; t<serializedTexCoords.cols; ++t)
 						{
 							UASSERT(int(serializedTexCoords.at<float>(t)) > 0);
+#if PCL_VERSION_COMPARE(>=, 1, 8, 0)
+							std::vector<Eigen::Vector2f, Eigen::aligned_allocator<Eigen::Vector2f> > materialtexCoords(int(serializedTexCoords.at<float>(t)));
+#else
 							std::vector<Eigen::Vector2f> materialtexCoords(int(serializedTexCoords.at<float>(t)));
+#endif
 							++t;
 							UASSERT(t < serializedTexCoords.cols);
 							UDEBUG("materialtexCoords=%d", (int)materialtexCoords.size());

corelib/src/DBDriverSqlite3.h

@@ -91,8 +91,8 @@ private:
 			const cv::Point3f & viewpoint) const;
 
 	virtual void updateDepthImageQuery(
-				int nodeId,
-				const cv::Mat & image) const;
+			int nodeId,
+			const cv::Mat & image) const;
 
 	virtual void addStatisticsQuery(const Statistics & statistics) const;
 	virtual void savePreviewImageQuery(const cv::Mat & image) const;
@@ -101,12 +101,20 @@ private:
 			const cv::Mat & cloud,
 			const std::map<int, Transform> & poses,
 			const std::vector<std::vector<std::vector<unsigned int> > > & polygons,
+#if PCL_VERSION_COMPARE(>=, 1, 8, 0)
+			const std::vector<std::vector<Eigen::Vector2f, Eigen::aligned_allocator<Eigen::Vector2f> > > & texCoords,
+#else
 			const std::vector<std::vector<Eigen::Vector2f> > & texCoords,
+#endif
 			const cv::Mat & textures) const;
 	virtual cv::Mat loadOptimizedMeshQuery(
 			std::map<int, Transform> * poses,
 			std::vector<std::vector<std::vector<unsigned int> > > * polygons,
+#if PCL_VERSION_COMPARE(>=, 1, 8, 0)
+			std::vector<std::vector<Eigen::Vector2f, Eigen::aligned_allocator<Eigen::Vector2f> > > * texCoords,
+#else
 			std::vector<std::vector<Eigen::Vector2f> > * texCoords,
+#endif
 			cv::Mat * textures) const;
 
 	// Load objects

corelib/src/Features2d.cpp

@@ -414,8 +414,7 @@ Feature2D * Feature2D::create(Feature2D::Type type, const ParametersMap & parame
 	if(type == Feature2D::kFeatureFastBrief ||
 	   type == Feature2D::kFeatureFastFreak ||
 	   type == Feature2D::kFeatureGfttBrief ||
-	   type == Feature2D::kFeatureGfttFreak ||
-	   type == Feature2D::kFeatureFreak)
+	   type == Feature2D::kFeatureGfttFreak)
 	{
 		UWARN("BRIEF and FREAK features cannot be used because OpenCV was not built with xfeatures2d module. ORB is used instead.");
 		type = Feature2D::kFeatureOrb;
@@ -424,10 +423,15 @@ Feature2D * Feature2D::create(Feature2D::Type type, const ParametersMap & parame
 #endif
 
 #if CV_MAJOR_VERSION < 3
-	if(type == Feature2D::kFeatureFreak)
+	if(type == Feature2D::kFeatureKaze)
 	{
-		UWARN("FREAK detector/descriptor can be used only with OpenCV3. GFTT/FREAK is used instead.");
-		type = Feature2D::kFeatureGfttFreak;
+#ifdef RTABMAP_NONFREE
+		UWARN("KAZE detector/descriptor can be used only with OpenCV3. SURF is used instead.");
+		type = Feature2D::kFeatureSurf;
+#else
+		UWARN("KAZE detector/descriptor can be used only with OpenCV3. ORB is used instead.");
+		type = Feature2D::kFeatureOrb;
+#endif
 	}
 #endif
 
@@ -461,6 +465,9 @@ Feature2D * Feature2D::create(Feature2D::Type type, const ParametersMap & parame
 	case Feature2D::kFeatureBrisk:
 		feature2D = new BRISK(parameters);
 		break;
+	case Feature2D::kFeatureKaze:
+		feature2D = new KAZE(parameters);
+		break;
 #ifdef RTABMAP_NONFREE
 	default:
 		feature2D = new SURF(parameters);
@@ -1469,71 +1476,66 @@ cv::Mat BRISK::generateDescriptorsImpl(const cv::Mat & image, std::vector<cv::Ke
 	return descriptors;
 }
 
-
 //////////////////////////
-//FREAK
+//KAZE
 //////////////////////////
-FREAK::FREAK(const ParametersMap & parameters) :
-	orientationNormalized_(Parameters::defaultFREAKOrientationNormalized()),
-	scaleNormalized_(Parameters::defaultFREAKScaleNormalized()),
-	patternScale_(Parameters::defaultFREAKPatternScale()),
-	nOctaves_(Parameters::defaultFREAKNOctaves())
+KAZE::KAZE(const ParametersMap & parameters) :
+		extended_(Parameters::defaultKAZEExtended()),
+		upright_(Parameters::defaultKAZEUpright()),
+		threshold_(Parameters::defaultKAZEThreshold()),
+		nOctaves_(Parameters::defaultKAZENOctaves()),
+		nOctaveLayers_(Parameters::defaultKAZENOctaveLayers()),
+		diffusivity_(Parameters::defaultKAZEDiffusivity())
 {
 	parseParameters(parameters);
 }
 
-FREAK::~FREAK()
+KAZE::~KAZE()
 {
 }
 
-void FREAK::parseParameters(const ParametersMap & parameters)
+void KAZE::parseParameters(const ParametersMap & parameters)
 {
-	Parameters::parse(parameters, Parameters::kFREAKOrientationNormalized(), orientationNormalized_);
-	Parameters::parse(parameters, Parameters::kFREAKScaleNormalized(), scaleNormalized_);
-	Parameters::parse(parameters, Parameters::kFREAKPatternScale(), patternScale_);
-	Parameters::parse(parameters, Parameters::kFREAKNOctaves(), nOctaves_);
+	Parameters::parse(parameters, Parameters::kKAZEExtended(), extended_);
+	Parameters::parse(parameters, Parameters::kKAZEUpright(), upright_);
+	Parameters::parse(parameters, Parameters::kKAZEThreshold(), threshold_);
+	Parameters::parse(parameters, Parameters::kKAZENOctaves(), nOctaves_);
+	Parameters::parse(parameters, Parameters::kKAZENOctaveLayers(), nOctaveLayers_);
+	Parameters::parse(parameters, Parameters::kKAZEDiffusivity(), diffusivity_);
 
-#if CV_MAJOR_VERSION < 3
-	_freak = cv::Ptr<CV_FREAK>(new CV_FREAK(orientationNormalized_, scaleNormalized_, patternScale_, nOctaves_));
+#if CV_MAJOR_VERSION > 2
+	kaze_ = cv::KAZE::create(extended_, upright_, threshold_, nOctaves_, nOctaveLayers_, diffusivity_);
 #else
-#ifdef HAVE_OPENCV_XFEATURES2D
-	_freak = CV_FREAK::create(orientationNormalized_, scaleNormalized_, patternScale_, nOctaves_);
-#else
-	UWARN("RTAB-Map is not built with OpenCV xfeatures2d module so Freak cannot be used!");
-#endif
+	UWARN("RTAB-Map is not built with OpenCV3 so Kaze feature cannot be used!");
 #endif
 }
 
-std::vector<cv::KeyPoint> FREAK::generateKeypointsImpl(const cv::Mat & image, const cv::Rect & roi, const cv::Mat & mask) const
+std::vector<cv::KeyPoint> KAZE::generateKeypointsImpl(const cv::Mat & image, const cv::Rect & roi, const cv::Mat & mask) const
 {
 	UASSERT(!image.empty() && image.channels() == 1 && image.depth() == CV_8U);
 	std::vector<cv::KeyPoint> keypoints;
-#ifdef HAVE_OPENCV_XFEATURES2D
+#if CV_MAJOR_VERSION > 2
 	cv::Mat imgRoi(image, roi);
 	cv::Mat maskRoi;
 	if (!mask.empty())
 	{
 		maskRoi = cv::Mat(mask, roi);
 	}
-	_freak->detect(imgRoi, keypoints, maskRoi); // Opencv keypoints
+	kaze_->detect(imgRoi, keypoints, maskRoi); // Opencv keypoints
 #else
-	UWARN("RTAB-Map is not built with OpenCV3 xfeatures2d module so Freak (for keypoint detection) cannot be used!");
+	UWARN("RTAB-Map is not built with OpenCV3 so Kaze feature cannot be used!");
 #endif
 	return keypoints;
 }
 
-cv::Mat FREAK::generateDescriptorsImpl(const cv::Mat & image, std::vector<cv::KeyPoint> & keypoints) const
+cv::Mat KAZE::generateDescriptorsImpl(const cv::Mat & image, std::vector<cv::KeyPoint> & keypoints) const
 {
 	UASSERT(!image.empty() && image.channels() == 1 && image.depth() == CV_8U);
 	cv::Mat descriptors;
-#if CV_MAJOR_VERSION < 3
-	_freak->compute(image, keypoints, descriptors);
+#if CV_MAJOR_VERSION > 2
+	kaze_->compute(image, keypoints, descriptors);
 #else
-#ifdef HAVE_OPENCV_XFEATURES2D
-	_freak->compute(image, keypoints, descriptors);
-#else
-	UWARN("RTAB-Map is not built with OpenCV xfeatures2d module so Freak cannot be used!");
-#endif
+	UWARN("RTAB-Map is not built with OpenCV3 so Kaze feature cannot be used!");
 #endif
 	return descriptors;
 }

corelib/src/Memory.cpp

@@ -718,7 +718,15 @@ void Memory::addSignatureToStm(Signature * signature, const cv::Mat & covariance
 				if(!signature->getPose().isNull() &&
 				   !_signatures.at(*_stMem.rbegin())->getPose().isNull())
 				{
+					UASSERT(covariance.cols == 6 && covariance.rows == 6 && covariance.type() == CV_64FC1);
 					cv::Mat infMatrix = covariance.inv();
+					if((uIsFinite(covariance.at<double>(0,0)) && covariance.at<double>(0,0)>0.0) &&
+						!(uIsFinite(infMatrix.at<double>(0,0)) && infMatrix.at<double>(0,0)>0.0))
+					{
+						UERROR("Failed to invert the covariance matrix! Covariance matrix should be invertible!");
+						std::cout << "Covariance: " << covariance << std::endl;
+						infMatrix = cv::Mat::eye(6,6,CV_64FC1);
+					}
 					motionEstimate = _signatures.at(*_stMem.rbegin())->getPose().inverse() * signature->getPose();
 					_signatures.at(*_stMem.rbegin())->addLink(Link(*_stMem.rbegin(), signature->id(), Link::kNeighbor, motionEstimate, infMatrix));
 					signature->addLink(Link(signature->id(), *_stMem.rbegin(), Link::kNeighbor, motionEstimate.inverse(), infMatrix));
@@ -1671,7 +1679,11 @@ void Memory::saveOptimizedMesh(
 		const cv::Mat & cloud,
 		const std::map<int, Transform> & poses,
 		const std::vector<std::vector<std::vector<unsigned int> > > & polygons,
+#if PCL_VERSION_COMPARE(>=, 1, 8, 0)
+		const std::vector<std::vector<Eigen::Vector2f, Eigen::aligned_allocator<Eigen::Vector2f> > > & texCoords,
+#else
 		const std::vector<std::vector<Eigen::Vector2f> > & texCoords,
+#endif
 		const cv::Mat & textures) const
 {
 	if(_dbDriver)
@@ -1683,7 +1695,11 @@ void Memory::saveOptimizedMesh(
 cv::Mat Memory::loadOptimizedMesh(
 			std::map<int, Transform> * poses,
 			std::vector<std::vector<std::vector<unsigned int> > > * polygons,
+#if PCL_VERSION_COMPARE(>=, 1, 8, 0)
+			std::vector<std::vector<Eigen::Vector2f, Eigen::aligned_allocator<Eigen::Vector2f>> > * texCoords,
+#else
 			std::vector<std::vector<Eigen::Vector2f> > * texCoords,
+#endif
 			cv::Mat * textures) const
 {
 	if(_dbDriver)

corelib/src/OccupancyGrid.cpp

@@ -98,26 +98,6 @@ void OccupancyGrid::parseParameters(const ParametersMap & parameters)
 	Parameters::parse(parameters, Parameters::kGridMaxObstacleHeight(), maxObstacleHeight_);
 	Parameters::parse(parameters, Parameters::kGridMinGroundHeight(), minGroundHeight_);
 	Parameters::parse(parameters, Parameters::kGridMaxGroundHeight(), maxGroundHeight_);
-	if(maxGroundHeight_ != 0.0f &&
-		maxObstacleHeight_ != 0.0f &&
-		maxObstacleHeight_ < maxGroundHeight_)
-	{
-		UWARN("\"%s\" should be lower than \"%s\", setting \"%s\" to 0 (disabled).",
-				Parameters::kGridMaxGroundHeight().c_str(),
-				Parameters::kGridMaxObstacleHeight().c_str(),
-				Parameters::kGridMaxObstacleHeight().c_str());
-		maxObstacleHeight_ = 0;
-	}
-	if(maxGroundHeight_ != 0.0f &&
-		minGroundHeight_ != 0.0f &&
-		maxGroundHeight_ < minGroundHeight_)
-	{
-		UWARN("\"%s\" should be lower than \"%s\", setting \"%s\" to 0 (disabled).",
-				Parameters::kGridMinGroundHeight().c_str(),
-				Parameters::kGridMaxGroundHeight().c_str(),
-				Parameters::kGridMinGroundHeight().c_str());
-		minGroundHeight_ = 0;
-	}
 	Parameters::parse(parameters, Parameters::kGridNormalK(), normalKSearch_);
 	if(Parameters::parse(parameters, Parameters::kGridMaxGroundAngle(), maxGroundAngle_))
 	{
@@ -182,6 +162,26 @@ void OccupancyGrid::parseParameters(const ParametersMap & parameters)
 				Parameters::kGridMaxGroundHeight().c_str(), cellSize_);
 		maxGroundHeight_ = cellSize_;
 	}
+	if(maxGroundHeight_ != 0.0f &&
+		maxObstacleHeight_ != 0.0f &&
+		maxObstacleHeight_ < maxGroundHeight_)
+	{
+		UWARN("\"%s\" should be lower than \"%s\", setting \"%s\" to 0 (disabled).",
+				Parameters::kGridMaxGroundHeight().c_str(),
+				Parameters::kGridMaxObstacleHeight().c_str(),
+				Parameters::kGridMaxObstacleHeight().c_str());
+		maxObstacleHeight_ = 0;
+	}
+	if(maxGroundHeight_ != 0.0f &&
+		minGroundHeight_ != 0.0f &&
+		maxGroundHeight_ < minGroundHeight_)
+	{
+		UWARN("\"%s\" should be lower than \"%s\", setting \"%s\" to 0 (disabled).",
+				Parameters::kGridMinGroundHeight().c_str(),
+				Parameters::kGridMaxGroundHeight().c_str(),
+				Parameters::kGridMinGroundHeight().c_str());
+		minGroundHeight_ = 0;
+	}
 }
 
 void OccupancyGrid::setCellSize(float cellSize)
@@ -299,7 +299,8 @@ void OccupancyGrid::createLocalMap(
 			viewPoint.z = viewpointRotated.z();
 		}
 
-		if(cloud->size())
+		if((cloud->is_dense && cloud->size()) ||
+			(!cloud->is_dense && indices->size()))
 		{
 			pcl::IndicesPtr groundIndices(new std::vector<int>);
 			pcl::IndicesPtr obstaclesIndices(new std::vector<int>);

corelib/src/OdometryDVO.cpp

@@ -90,6 +90,7 @@ void OdometryDVO::reset(const Transform & initialPose)
 	}
 	lost_ = false;
 	motionFromKeyFrame_.setIdentity();
+	previousLocalTransform_.setNull();
 #endif
 }
 
@@ -192,6 +193,7 @@ Transform OdometryDVO::computeTransform(
 
 	dvo::core::RgbdImagePyramid * current = new dvo::core::RgbdImagePyramid(*camera_, grey_s16, depth_float);
 
+	const Transform & localTransform = data.cameraModels()[0].localTransform();
 	cv::Mat covariance;
 	if(reference_ == 0)
 	{
@@ -245,16 +247,24 @@ Transform OdometryDVO::computeTransform(
 			reference_ = 0; // this will make restart from the next frame
 			motionFromKeyFrame_.setIdentity();
 			t.setNull();
+			previousLocalTransform_.setNull();
 			covariance = cv::Mat::eye(6,6,CV_64FC1) * 9999.0;
 			UWARN("dvo failed to estimate motion, tracking will be reinitialized on next frame.");
 		}
-	}
 
-	const Transform & localTransform = data.cameraModels()[0].localTransform();
-	if(!t.isNull() && !t.isIdentity() && !localTransform.isIdentity() && !localTransform.isNull())
-	{
-		// from camera frame to base frame
-		t = localTransform * t * localTransform.inverse();
+		if(!t.isNull() && !t.isIdentity() && !localTransform.isIdentity() && !localTransform.isNull())
+		{
+			// from camera frame to base frame
+			if(!previousLocalTransform_.isNull())
+			{
+				t = previousLocalTransform_ * t * localTransform.inverse();
+			}
+			else
+			{
+				t = localTransform * t * localTransform.inverse();
+			}
+			previousLocalTransform_ = localTransform;
+		}
 	}
 
 	if(info)

corelib/src/OdometryFovis.cpp

@@ -111,6 +111,7 @@ void OdometryFovis::reset(const Transform & initialPose)
 		stereoDepth_ = 0;
 	}
 	lost_ = false;
+	previousLocalTransform_.setNull();
 #endif
 }
 
@@ -341,12 +342,14 @@ Transform OdometryFovis::computeTransform(
 		t.setNull();
 		lost_ = true;
 		covariance = cv::Mat::eye(6,6, CV_64FC1)*9999.0;
+		previousLocalTransform_.setNull();
 	}
 	else if(lost_)
 	{
 		lost_ = false;
 		// we are not lost anymore but we don't know where we are now according to last valid pose
 		covariance = cv::Mat::eye(6,6, CV_64FC1)*9999.0;
+		previousLocalTransform_.setNull();
 	}
 	else
 	{
@@ -362,7 +365,15 @@ Transform OdometryFovis::computeTransform(
 	if(!t.isNull() && !t.isIdentity() && !localTransform.isIdentity() && !localTransform.isNull())
 	{
 		// from camera frame to base frame
-		t = localTransform * t * localTransform.inverse();
+		if(!previousLocalTransform_.isNull())
+		{
+			t = previousLocalTransform_ * t * localTransform.inverse();
+		}
+		else
+		{
+			t = localTransform * t * localTransform.inverse();
+		}
+		previousLocalTransform_ = localTransform;
 	}
 
 	if(info)

corelib/src/OdometryORBSLAM2.cpp

@@ -33,6 +33,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #include "rtabmap/utilite/ULogger.h"
 #include "rtabmap/utilite/UTimer.h"
 #include "rtabmap/utilite/UStl.h"
+#include "rtabmap/utilite/UDirectory.h"
 
 #ifdef RTABMAP_ORB_SLAM2
 #include <System.h>
@@ -495,6 +496,7 @@ public:
 		if(!vocabularyPath.empty())
 		{
 			//Load ORB Vocabulary
+			vocabularyPath = uReplaceChar(vocabularyPath, '~', UDirectory::homeDir());
 			UWARN("Loading ORB Vocabulary: \"%s\". This could take a while...", vocabularyPath.c_str());
 			mpVocabulary = new ORB_SLAM2::ORBVocabulary();
 			bool bVocLoad = mpVocabulary->loadFromTextFile(vocabularyPath);
@@ -773,6 +775,7 @@ void OdometryORBSLAM2::reset(const Transform & initialPose)
 		orbslam2_->shutdown();
 	}
 	firstFrame_ = true;
+	originLocalTransform_.setNull();
 #endif
 }
 
@@ -845,14 +848,17 @@ Transform OdometryORBSLAM2::computeTransform(
 	}
 	else if(Tcw.cols == 4 && Tcw.rows == 4)
 	{
-		t = Transform(cv::Mat(Tcw, cv::Range(0,3), cv::Range(0,4)).clone());
+		t = Transform(cv::Mat(Tcw, cv::Range(0,3), cv::Range(0,4)));
 
 		if(!t.isNull() && !t.isIdentity() && !localTransform.isIdentity() && !localTransform.isNull())
 		{
-			// from camera frame to base frame
-			t = localTransform * t.inverse() * localTransform.inverse();
+			if(originLocalTransform_.isNull())
+			{
+				originLocalTransform_ = localTransform;
+			}
+			t = originLocalTransform_ * t.inverse() * localTransform.inverse();
+			t = this->getPose().inverse() * t;
 		}
-		t = this->getPose().inverse() * t;
 
 		if(firstFrame_)
 		{
@@ -916,12 +922,13 @@ Transform OdometryORBSLAM2::computeTransform(
 			}
 			info->wordMatches.resize(oi);
 			info->wordInliers.resize(oi);
+			info->inliers = oi;
 
 			std::vector<ORB_SLAM2::MapPoint*> mapPoints = orbslam2_->mpMap->GetAllMapPoints();
 			for (unsigned int i = 0; i < mapPoints.size(); ++i)
 			{
 				cv::Mat pt = mapPoints[i]->GetWorldPos();
-				info->localMap.insert(std::make_pair(mapPoints[i]->mnId, util3d::transformPoint(cv::Point3f(pt), localTransform)));
+				info->localMap.insert(std::make_pair(mapPoints[i]->mnId, util3d::transformPoint(cv::Point3f(pt), originLocalTransform_)));
 			}
 		}
 	}

corelib/src/OdometryViso2.cpp

@@ -86,6 +86,7 @@ void OdometryViso2::reset(const Transform & initialPose)
 	}
 	lost_ = false;
 	reference_motion_.setIdentity();
+	previousLocalTransform_.setNull();
 #endif
 }
 
@@ -253,10 +254,19 @@ Transform OdometryViso2::computeTransform(
 		}
 	}
 
-	if(!t.isNull() && !t.isIdentity() && !data.stereoCameraModel().localTransform().isIdentity() && !data.stereoCameraModel().localTransform().isNull())
+	const Transform & localTransform = data.stereoCameraModel().localTransform();
+	if(!t.isNull() && !t.isIdentity() && !localTransform.isIdentity() && !localTransform.isNull())
 	{
 		// from camera frame to base frame
-		t = data.stereoCameraModel().localTransform() * t * data.stereoCameraModel().localTransform().inverse();
+		if(!previousLocalTransform_.isNull())
+		{
+			t = previousLocalTransform_ * t * localTransform.inverse();
+		}
+		else
+		{
+			t = localTransform * t * localTransform.inverse();
+		}
+		previousLocalTransform_ = localTransform;
 	}
 
 	if(info)

corelib/src/Parameters.cpp

@@ -156,7 +156,8 @@ bool Parameters::isFeatureParameter(const std::string & parameter)
 			group.compare("FREAK") == 0 ||
 			group.compare("BRIEF") == 0 ||
 			group.compare("GFTT") == 0 ||
-			group.compare("BRISK") == 0;
+			group.compare("BRISK") == 0 ||
+			group.compare("KAZE") == 0;
 }
 
 rtabmap::ParametersMap Parameters::getDefaultOdometryParameters(bool stereo, bool vis, bool icp)
@@ -224,6 +225,9 @@ const std::map<std::string, std::pair<bool, std::string> > & Parameters::getRemo
 	{
 		// removed parameters
 
+		// 0.13.1
+		removedParameters_.insert(std::make_pair("Rtabmap/VhStrategy",            std::make_pair(true,  Parameters::kVhEpEnabled())));
+
 		// 0.12.5
 		removedParameters_.insert(std::make_pair("Grid/FullUpdate",               std::make_pair(true,  Parameters::kGridGlobalFullUpdate())));
 
@@ -807,7 +811,7 @@ void Parameters::readINI(const std::string & configFile, ParametersMap & paramet
 	{
 		ULOGGER_WARN("Section \"Core\" in %s doesn't exist... "
 				    "Ignore this warning if the ini file does not exist yet. "
-				    "The ini file will be automatically created when this node will close.", configFile.c_str());
+				    "The ini file will be automatically created when rtabmap will close.", configFile.c_str());
 	}
 }
 

corelib/src/Registration.cpp

@@ -257,17 +257,17 @@ void Registration::normalizeCovariance(cv::Mat & covariance, const Transform & t
 	}
 
 	double epsilon = 0.000001;
-	if(covariance.at<double>(0,0)<=0.0)
+	if(covariance.at<double>(0,0)<=epsilon)
 		covariance.at<double>(0,0) = epsilon; // epsilon if exact transform
-	if(covariance.at<double>(1,1)<=0.0)
+	if(covariance.at<double>(1,1)<=epsilon)
 		covariance.at<double>(1,1) = epsilon; // epsilon if exact transform
-	if(covariance.at<double>(2,2)<=0.0)
+	if(covariance.at<double>(2,2)<=epsilon)
 		covariance.at<double>(2,2) = epsilon; // epsilon if exact transform
-	if(covariance.at<double>(3,3)<=0.0)
+	if(covariance.at<double>(3,3)<=epsilon)
 		covariance.at<double>(3,3) = epsilon; // epsilon if exact transform
-	if(covariance.at<double>(4,4)<=0.0)
+	if(covariance.at<double>(4,4)<=epsilon)
 		covariance.at<double>(4,4) = epsilon; // epsilon if exact transform
-	if(covariance.at<double>(5,5)<=0.0)
+	if(covariance.at<double>(5,5)<=epsilon)
 		covariance.at<double>(5,5) = epsilon; // epsilon if exact transform
 }
 

corelib/src/RegistrationVis.cpp

@@ -1234,6 +1234,20 @@ Transform RegistrationVis::computeTransformationImpl(
 					UINFO(msg.c_str());
 				}
 			}
+
+			double epsilon = 0.000001;
+			if(covariances[dir].at<double>(0,0)<=epsilon)
+				covariances[dir].at<double>(0,0) = epsilon; // epsilon if exact transform
+			if(covariances[dir].at<double>(1,1)<=epsilon)
+				covariances[dir].at<double>(1,1) = epsilon; // epsilon if exact transform
+			if(covariances[dir].at<double>(2,2)<=epsilon)
+				covariances[dir].at<double>(2,2) = epsilon; // epsilon if exact transform
+			if(covariances[dir].at<double>(3,3)<=epsilon)
+				covariances[dir].at<double>(3,3) = epsilon; // epsilon if exact transform
+			if(covariances[dir].at<double>(4,4)<=epsilon)
+				covariances[dir].at<double>(4,4) = epsilon; // epsilon if exact transform
+			if(covariances[dir].at<double>(5,5)<=epsilon)
+				covariances[dir].at<double>(5,5) = epsilon; // epsilon if exact transform
 		}
 
 		if(!_forwardEstimateOnly)
@@ -1292,13 +1306,13 @@ Transform RegistrationVis::computeTransformationImpl(
 			poses.insert(std::make_pair(2, transforms[0]));
 
 			cv::Mat cov = covariances[0].clone();
-			normalizeCovariance(cov, transform);
+			normalizeCovariance(cov, transforms[0]);
 
 			links.insert(std::make_pair(1, Link(1, 2, Link::kNeighbor, transforms[0], cov.inv())));
 			if(!transforms[1].isNull() && inliers[1].size())
 			{
 				cov = covariances[1].clone();
-				normalizeCovariance(cov, transform);
+				normalizeCovariance(cov, transforms[1]);
 				links.insert(std::make_pair(2, Link(2, 1, Link::kNeighbor, transforms[1], cov.inv())));
 			}
 

corelib/src/Rtabmap.cpp

@@ -83,6 +83,7 @@ Rtabmap::Rtabmap() :
 	_maxMemoryAllowed(Parameters::defaultRtabmapMemoryThr()), // 0=inf
 	_loopThr(Parameters::defaultRtabmapLoopThr()),
 	_loopRatio(Parameters::defaultRtabmapLoopRatio()),
+	_verifyLoopClosureHypothesis(Parameters::defaultVhEpEnabled()),
 	_maxRetrieved(Parameters::defaultRtabmapMaxRetrieved()),
 	_maxLocalRetrieved(Parameters::defaultRGBDMaxLocalRetrieved()),
 	_rawDataKept(Parameters::defaultMemImageKept()),
@@ -401,6 +402,7 @@ void Rtabmap::parseParameters(const ParametersMap & parameters)
 	Parameters::parse(parameters, Parameters::kRtabmapMemoryThr(), _maxMemoryAllowed);
 	Parameters::parse(parameters, Parameters::kRtabmapLoopThr(), _loopThr);
 	Parameters::parse(parameters, Parameters::kRtabmapLoopRatio(), _loopRatio);
+	Parameters::parse(parameters, Parameters::kVhEpEnabled(), _verifyLoopClosureHypothesis);
 	Parameters::parse(parameters, Parameters::kRtabmapMaxRetrieved(), _maxRetrieved);
 	Parameters::parse(parameters, Parameters::kRGBDMaxLocalRetrieved(), _maxLocalRetrieved);
 	Parameters::parse(parameters, Parameters::kMemImageKept(), _rawDataKept);
@@ -470,22 +472,11 @@ void Rtabmap::parseParameters(const ParametersMap & parameters)
 		_memory->parseParameters(parameters);
 	}
 
-	VhStrategy vhStrategy = kVhUndef;
-	// Verifying hypotheses strategy
-	if((iter=parameters.find(Parameters::kRtabmapVhStrategy())) != parameters.end())
-	{
-		vhStrategy = (VhStrategy)std::atoi((*iter).second.c_str());
-	}
-	if(!_epipolarGeometry && vhStrategy == kVhEpipolar)
+	if(!_epipolarGeometry)
 	{
 		_epipolarGeometry = new EpipolarGeometry(_parameters);
 	}
-	else if(_epipolarGeometry && vhStrategy == kVhNone)
-	{
-		delete _epipolarGeometry;
-		_epipolarGeometry = 0;
-	}
-	else if(_epipolarGeometry)
+	else
 	{
 		_epipolarGeometry->parseParameters(parameters);
 	}
@@ -1407,7 +1398,7 @@ bool Rtabmap::process(
 						// Ignore loop closure if there is only one loop closure hypothesis
 						UDEBUG("rejected hypothesis: single hypothesis");
 					}
-					else if(_epipolarGeometry && !_epipolarGeometry->check(signature, _memory->getSignature(_highestHypothesis.first)))
+					else if(_verifyLoopClosureHypothesis && !_epipolarGeometry->check(signature, _memory->getSignature(_highestHypothesis.first)))
 					{
 						UWARN("rejected hypothesis: by epipolar geometry");
 					}
@@ -2550,6 +2541,7 @@ bool Rtabmap::process(
 			id = _memory->getLastWorkingSignature()->id();
 			UDEBUG("Refresh local map from %d", id);
 		}
+		UDEBUG("id=%d _optimizedPoses=%d", id, (int)_optimizedPoses.size());
 		if(id > 0)
 		{
 			if(_lastLocalizationNodeId != 0)

corelib/src/RtabmapThread.cpp

@@ -570,8 +570,7 @@ void RtabmapThread::addData(const OdometryEvent & odomEvent)
 		}
 		if(!lastPose_.isIdentity() &&
 						(odomEvent.pose().isIdentity() ||
-						odomEvent.rotVariance()>=9999 ||
-						odomEvent.transVariance()>=9999))
+						odomEvent.info().covariance.at<double>(0,0)>=9999))
 		{
 			if(odomEvent.pose().isIdentity())
 			{
@@ -579,21 +578,21 @@ void RtabmapThread::addData(const OdometryEvent & odomEvent)
 			}
 			else
 			{
-				UWARN("Odometry is reset (high variance (%f/%f >=9999 detected). Increment map id!", odomEvent.transVariance(), odomEvent.rotVariance());
+				UWARN("Odometry is reset (high variance (%f >=9999 detected). Increment map id!", odomEvent.info().covariance.at<double>(0,0));
 			}
 			pushNewState(kStateTriggeringMap);
 			covariance_ = cv::Mat();
 		}
 
-		double maxRotVar = odomEvent.rotVariance();
-		double maxTransVar = odomEvent.transVariance();
-		if(maxRotVar != 1.0f && maxTransVar != 1.0f && !covariance_.empty())
+		if(uIsFinite(odomEvent.info().covariance.at<double>(0,0)) &&
+			odomEvent.info().covariance.at<double>(0,0) != 1.0 &&
+			odomEvent.info().covariance.at<double>(0,0)>0.0)
 		{
-			covariance_ += odomEvent.covariance();
-		}
-		else
-		{
-			covariance_ = odomEvent.covariance();
+			// Use largest covariance error (to be independent of the odometry frame rate)
+			if(covariance_.empty() || odomEvent.info().covariance.at<double>(0,0) > covariance_.at<double>(0,0))
+			{
+				covariance_ = odomEvent.info().covariance;
+			}
 		}
 
 		if(ignoreFrame && !_createIntermediateNodes)

corelib/src/util3d.cpp

@@ -957,9 +957,40 @@ pcl::PointCloud<pcl::PointXYZ>::Ptr RTABMAP_EXP cloudFromSensorData(
 			leftMono = sensorData.imageRaw();
 		}
 
+		cv::Mat right(sensorData.rightRaw());
+		StereoCameraModel model = sensorData.stereoCameraModel();
+		if( roiRatios.size() == 4 &&
+			((roiRatios[0] > 0.0f && roiRatios[0] <= 1.0f) ||
+			 (roiRatios[1] > 0.0f && roiRatios[1] <= 1.0f) ||
+			 (roiRatios[2] > 0.0f && roiRatios[2] <= 1.0f) ||
+			 (roiRatios[3] > 0.0f && roiRatios[3] <= 1.0f)))
+		{
+			cv::Rect roi = util2d::computeRoi(leftMono, roiRatios);
+			if(	roi.width%decimation==0 &&
+				roi.height%decimation==0)
+			{
+				leftMono = cv::Mat(leftMono, roi);
+				right = cv::Mat(right, roi);
+				model.roi(roi);
+			}
+			else
+			{
+				UERROR("Cannot apply ROI ratios [%f,%f,%f,%f] because resulting "
+					  "dimension (left=%dx%d) cannot be divided exactly "
+					  "by decimation parameter (%d). Ignoring ROI ratios...",
+					  roiRatios[0],
+					  roiRatios[1],
+					  roiRatios[2],
+					  roiRatios[3],
+					  roi.width,
+					  roi.height,
+					  decimation);
+			}
+		}
+
 		cloud = cloudFromDisparity(
-				util2d::disparityFromStereoImages(leftMono, sensorData.rightRaw(), stereoParameters),
-				sensorData.stereoCameraModel(),
+				util2d::disparityFromStereoImages(leftMono, right, stereoParameters),
+				model,
 				decimation,
 				maxDepth,
 				minDepth,
@@ -1091,10 +1122,43 @@ pcl::PointCloud<pcl::PointXYZRGB>::Ptr RTABMAP_EXP cloudRGBFromSensorData(
 	{
 		//stereo
 		UDEBUG("");
+
+		cv::Mat left(sensorData.imageRaw());
+		cv::Mat right(sensorData.rightRaw());
+		StereoCameraModel model = sensorData.stereoCameraModel();
+		if( roiRatios.size() == 4 &&
+			((roiRatios[0] > 0.0f && roiRatios[0] <= 1.0f) ||
+			 (roiRatios[1] > 0.0f && roiRatios[1] <= 1.0f) ||
+			 (roiRatios[2] > 0.0f && roiRatios[2] <= 1.0f) ||
+			 (roiRatios[3] > 0.0f && roiRatios[3] <= 1.0f)))
+		{
+			cv::Rect roi = util2d::computeRoi(left, roiRatios);
+			if(	roi.width%decimation==0 &&
+				roi.height%decimation==0)
+			{
+				left = cv::Mat(left, roi);
+				right = cv::Mat(right, roi);
+				model.roi(roi);
+			}
+			else
+			{
+				UERROR("Cannot apply ROI ratios [%f,%f,%f,%f] because resulting "
+					  "dimension (left=%dx%d) cannot be divided exactly "
+					  "by decimation parameter (%d). Ignoring ROI ratios...",
+					  roiRatios[0],
+					  roiRatios[1],
+					  roiRatios[2],
+					  roiRatios[3],
+					  roi.width,
+					  roi.height,
+					  decimation);
+			}
+		}
+
 		cloud = cloudFromStereoImages(
-				sensorData.imageRaw(),
-				sensorData.rightRaw(),
-				sensorData.stereoCameraModel(),
+				left,
+				right,
+				model,
 				decimation,
 				maxDepth,
 				minDepth,

corelib/src/util3d_filtering.cpp

@@ -142,6 +142,8 @@ pcl::PointCloud<pcl::PointXYZ>::Ptr voxelize(
 		float voxelSize)
 {
 	UASSERT(voxelSize > 0.0f);
+	UASSERT_MSG((cloud->is_dense && cloud->size()) || (!cloud->is_dense && indices->size()),
+			uFormat("Cloud size=%d indices=%d is_dense=%s", (int)cloud->size(), (int)indices->size(), cloud->is_dense?"true":"false").c_str());
 	pcl::PointCloud<pcl::PointXYZ>::Ptr output(new pcl::PointCloud<pcl::PointXYZ>);
 	pcl::VoxelGrid<pcl::PointXYZ> filter;
 	filter.setLeafSize(voxelSize, voxelSize, voxelSize);
@@ -159,6 +161,8 @@ pcl::PointCloud<pcl::PointNormal>::Ptr voxelize(
 		float voxelSize)
 {
 	UASSERT(voxelSize > 0.0f);
+	UASSERT_MSG((cloud->is_dense && cloud->size()) || (!cloud->is_dense && indices->size()),
+			uFormat("Cloud size=%d indices=%d is_dense=%s", (int)cloud->size(), (int)indices->size(), cloud->is_dense?"true":"false").c_str());
 	pcl::PointCloud<pcl::PointNormal>::Ptr output(new pcl::PointCloud<pcl::PointNormal>);
 	pcl::VoxelGrid<pcl::PointNormal> filter;
 	filter.setLeafSize(voxelSize, voxelSize, voxelSize);
@@ -176,6 +180,8 @@ pcl::PointCloud<pcl::PointXYZRGB>::Ptr voxelize(
 		float voxelSize)
 {
 	UASSERT(voxelSize > 0.0f);
+	UASSERT_MSG((cloud->is_dense && cloud->size()) || (!cloud->is_dense && indices->size()),
+			uFormat("Cloud size=%d indices=%d is_dense=%s", (int)cloud->size(), (int)indices->size(), cloud->is_dense?"true":"false").c_str());
 	pcl::PointCloud<pcl::PointXYZRGB>::Ptr output(new pcl::PointCloud<pcl::PointXYZRGB>);
 	pcl::VoxelGrid<pcl::PointXYZRGB> filter;
 	filter.setLeafSize(voxelSize, voxelSize, voxelSize);
@@ -193,6 +199,8 @@ pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr voxelize(
 		float voxelSize)
 {
 	UASSERT(voxelSize > 0.0f);
+	UASSERT_MSG((cloud->is_dense && cloud->size()) || (!cloud->is_dense && indices->size()),
+			uFormat("Cloud size=%d indices=%d is_dense=%s", (int)cloud->size(), (int)indices->size(), cloud->is_dense?"true":"false").c_str());
 	pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr output(new pcl::PointCloud<pcl::PointXYZRGBNormal>);
 	pcl::VoxelGrid<pcl::PointXYZRGBNormal> filter;
 	filter.setLeafSize(voxelSize, voxelSize, voxelSize);
@@ -266,7 +274,7 @@ pcl::IndicesPtr passThrough(
 		float max,
 		bool negative)
 {
-	UASSERT(max > min);
+	UASSERT_MSG(max > min, uFormat("cloud=%d, max=%f min=%f axis=%s", (int)cloud->size(), max, min, axis.c_str()).c_str());
 	UASSERT(axis.compare("x") == 0 || axis.compare("y") == 0 || axis.compare("z") == 0);
 
 	pcl::IndicesPtr output(new std::vector<int>);
@@ -287,7 +295,7 @@ pcl::IndicesPtr passThrough(
 		float max,
 		bool negative)
 {
-	UASSERT(max > min);
+	UASSERT_MSG(max > min, uFormat("cloud=%d, max=%f min=%f axis=%s", (int)cloud->size(), max, min, axis.c_str()).c_str());
 	UASSERT(axis.compare("x") == 0 || axis.compare("y") == 0 || axis.compare("z") == 0);
 
 	pcl::IndicesPtr output(new std::vector<int>);
@@ -308,7 +316,7 @@ pcl::PointCloud<pcl::PointXYZ>::Ptr passThrough(
 		float max,
 		bool negative)
 {
-	UASSERT(max > min);
+	UASSERT_MSG(max > min, uFormat("cloud=%d, max=%f min=%f axis=%s", (int)cloud->size(), max, min, axis.c_str()).c_str());
 	UASSERT(axis.compare("x") == 0 || axis.compare("y") == 0 || axis.compare("z") == 0);
 
 	pcl::PointCloud<pcl::PointXYZ>::Ptr output(new pcl::PointCloud<pcl::PointXYZ>);
@@ -328,7 +336,7 @@ pcl::PointCloud<pcl::PointXYZRGB>::Ptr passThrough(
 		float max,
 		bool negative)
 {
-	UASSERT(max > min);
+	UASSERT_MSG(max > min, uFormat("cloud=%d, max=%f min=%f axis=%s", (int)cloud->size(), max, min, axis.c_str()).c_str());
 	UASSERT(axis.compare("x") == 0 || axis.compare("y") == 0 || axis.compare("z") == 0);
 
 	pcl::PointCloud<pcl::PointXYZRGB>::Ptr output(new pcl::PointCloud<pcl::PointXYZRGB>);
@@ -348,7 +356,7 @@ pcl::PointCloud<pcl::PointNormal>::Ptr passThrough(
 		float max,
 		bool negative)
 {
-	UASSERT(max > min);
+	UASSERT_MSG(max > min, uFormat("cloud=%d, max=%f min=%f axis=%s", (int)cloud->size(), max, min, axis.c_str()).c_str());
 	UASSERT(axis.compare("x") == 0 || axis.compare("y") == 0 || axis.compare("z") == 0);
 
 	pcl::PointCloud<pcl::PointNormal>::Ptr output(new pcl::PointCloud<pcl::PointNormal>);

docker/xenial/android/Dockerfile

@@ -1,6 +1,13 @@
 # Image: introlab3it/rtabmap:android-deps
 
-FROM introlab3it/rtabmap
+FROM ubuntu:16.04
+
+# Install build dependencies
+RUN apt-get update && apt-get install -y \
+      git \
+      cmake \
+      g++ \
+      software-properties-common
 
 ENV ANDROID_HOME=/opt/android-sdk
 ENV ANDROID_NATIVE_API_LEVEL=android-19
@@ -15,4 +22,4 @@ RUN /bin/bash -c "./setup_android_env.bash"
 
 ADD install_deps.bash /root/install_deps.bash
 RUN chmod +x install_deps.bash
-RUN /bin/bash -c "./install_deps.bash"
+RUN /bin/bash -c "./install_deps.bash /opt/android"

docker/xenial/android/install_deps.bash

@@ -1,5 +1,18 @@
 #!/bin/bash
 
+if [ $# -ne 1 ]; then
+    echo "install_deps.bash android_install_prefix   # Example: install_deps.bash /opt/android"
+    exit 1
+fi
+
+prefix=$1
+pwd=$(pwd)
+wget -nv https://github.com/introlab/rtabmap/raw/master/cmake_modules/android.toolchain.cmake
+
+# Install directory for all dependencies
+mkdir -p $prefix/armeabi-v7a
+mkdir -p $prefix/arm64-v8a
+
 # Boost
 echo "wget boost..."
 wget -nv https://downloads.sourceforge.net/project/boost/boost/1.59.0/boost_1_59_0.tar.gz
@@ -9,11 +22,11 @@ wget -nv https://gist.github.com/matlabbe/0bce8feeb73a499a76afbbcc5c687221/raw/4
 wget -nv https://gist.github.com/matlabbe/0bce8feeb73a499a76afbbcc5c687221/raw/e7fbf0e301cfea417a7aa69989a761a4de08b8c3/CMakeLists.txt
 mkdir build
 cd build
-cmake -DCMAKE_TOOLCHAIN_FILE=/root/android.toolchain.cmake -DANDROID_ABI=armeabi-v7a -DBUILD_SHARED_LIBS=OFF -DCMAKE_BUILD_TYPE=Release -DCMAKE_INSTALL_PREFIX=/opt/android/armeabi-v7a ..
+cmake -DCMAKE_TOOLCHAIN_FILE=$pwd/android.toolchain.cmake -DANDROID_ABI=armeabi-v7a -DBUILD_SHARED_LIBS=OFF -DCMAKE_BUILD_TYPE=Release -DCMAKE_INSTALL_PREFIX=$prefix/armeabi-v7a ..
 make
 make install
 rm -r *
-cmake -DCMAKE_TOOLCHAIN_FILE=/root/android.toolchain.cmake -DANDROID_ABI=arm64-v8a -DBUILD_SHARED_LIBS=OFF -DCMAKE_BUILD_TYPE=Release -DCMAKE_INSTALL_PREFIX=/opt/android/arm64-v8a ..
+cmake -DCMAKE_TOOLCHAIN_FILE=$pwd/android.toolchain.cmake -DANDROID_ABI=arm64-v8a -DBUILD_SHARED_LIBS=OFF -DCMAKE_BUILD_TYPE=Release -DCMAKE_INSTALL_PREFIX=$prefix/arm64-v8a ..
 make
 make install
 cd
@@ -26,11 +39,11 @@ tar -xzf 3.2.7.tar.gz
 cd eigen-eigen-b30b87236a1b
 mkdir build
 cd build
-cmake -DCMAKE_TOOLCHAIN_FILE=/root/android.toolchain.cmake -DANDROID_ABI=armeabi-v7a -DBUILD_SHARED_LIBS=OFF -DCMAKE_BUILD_TYPE=Release -DCMAKE_INSTALL_PREFIX=/opt/android/armeabi-v7a ..
+cmake -DCMAKE_TOOLCHAIN_FILE=$pwd/android.toolchain.cmake -DANDROID_ABI=armeabi-v7a -DBUILD_SHARED_LIBS=OFF -DCMAKE_BUILD_TYPE=Release -DCMAKE_INSTALL_PREFIX=$prefix/armeabi-v7a ..
 make
 make install
 rm -r *
-cmake -DCMAKE_TOOLCHAIN_FILE=/root/android.toolchain.cmake -DANDROID_ABI=arm64-v8a -DBUILD_SHARED_LIBS=OFF -DCMAKE_BUILD_TYPE=Release -DCMAKE_INSTALL_PREFIX=/opt/android/arm64-v8a ..
+cmake -DCMAKE_TOOLCHAIN_FILE=$pwd/android.toolchain.cmake -DANDROID_ABI=arm64-v8a -DBUILD_SHARED_LIBS=OFF -DCMAKE_BUILD_TYPE=Release -DCMAKE_INSTALL_PREFIX=$prefix/arm64-v8a ..
 make
 make install
 cd
@@ -43,11 +56,11 @@ unzip -qq flann-1.8.4-src.zip
 cd flann-1.8.4-src
 mkdir build
 cd build
-cmake -DCMAKE_TOOLCHAIN_FILE=/root/android.toolchain.cmake -DANDROID_ABI=armeabi-v7a -DBUILD_SHARED_LIBS=OFF -DCMAKE_BUILD_TYPE=Release -DCMAKE_INSTALL_PREFIX=/opt/android/armeabi-v7a ..
+cmake -DCMAKE_TOOLCHAIN_FILE=$pwd/android.toolchain.cmake -DANDROID_ABI=armeabi-v7a -DBUILD_SHARED_LIBS=OFF -DCMAKE_BUILD_TYPE=Release -DCMAKE_INSTALL_PREFIX=$prefix/armeabi-v7a ..
 make 
 make install
 rm -r *
-cmake -DCMAKE_TOOLCHAIN_FILE=/root/android.toolchain.cmake -DANDROID_ABI=arm64-v8a -DBUILD_SHARED_LIBS=OFF -DCMAKE_BUILD_TYPE=Release -DCMAKE_INSTALL_PREFIX=/opt/android/arm64-v8a ..
+cmake -DCMAKE_TOOLCHAIN_FILE=$pwd/android.toolchain.cmake -DANDROID_ABI=arm64-v8a -DBUILD_SHARED_LIBS=OFF -DCMAKE_BUILD_TYPE=Release -DCMAKE_INSTALL_PREFIX=$prefix/arm64-v8a ..
 make
 make install
 cd
@@ -62,11 +75,11 @@ wget -nv https://gist.github.com/matlabbe/726b490c658afd3293f4b3f2f501b863/raw/d
 git apply gtsam_GKlib_android_fix.patch
 mkdir build
 cd build
-cmake -DCMAKE_TOOLCHAIN_FILE=/root/android.toolchain.cmake -DANDROID_ABI=armeabi-v7a -DMETIS_SHARED=OFF -DGTSAM_BUILD_STATIC_LIBRARY=ON -DGTSAM_BUILD_TESTS=OFF -DGTSAM_BUILD_EXAMPLES_ALWAYS=OFF -DCMAKE_BUILD_TYPE=Release -DCMAKE_INSTALL_PREFIX=/opt/android/armeabi-v7a ..
+cmake -DCMAKE_TOOLCHAIN_FILE=$pwd/android.toolchain.cmake -DANDROID_ABI=armeabi-v7a -DMETIS_SHARED=OFF -DGTSAM_BUILD_STATIC_LIBRARY=ON -DGTSAM_BUILD_TESTS=OFF -DGTSAM_BUILD_EXAMPLES_ALWAYS=OFF -DCMAKE_BUILD_TYPE=Release -DCMAKE_INSTALL_PREFIX=$prefix/armeabi-v7a ..
 make 
 make install
 rm -r *
-cmake -DCMAKE_TOOLCHAIN_FILE=/root/android.toolchain.cmake -DANDROID_ABI=arm64-v8a -DMETIS_SHARED=OFF -DGTSAM_BUILD_STATIC_LIBRARY=ON -DGTSAM_BUILD_TESTS=OFF -DGTSAM_BUILD_EXAMPLES_ALWAYS=OFF -DCMAKE_BUILD_TYPE=Release -DCMAKE_INSTALL_PREFIX=/opt/android/arm64-v8a ..
+cmake -DCMAKE_TOOLCHAIN_FILE=$pwd/android.toolchain.cmake -DANDROID_ABI=arm64-v8a -DMETIS_SHARED=OFF -DGTSAM_BUILD_STATIC_LIBRARY=ON -DGTSAM_BUILD_TESTS=OFF -DGTSAM_BUILD_EXAMPLES_ALWAYS=OFF -DCMAKE_BUILD_TYPE=Release -DCMAKE_INSTALL_PREFIX=$prefix/arm64-v8a ..
 make
 make install
 cd
@@ -77,11 +90,11 @@ git clone https://github.com/RainerKuemmerle/g2o.git
 cd g2o
 mkdir build
 cd build
-cmake -DCMAKE_TOOLCHAIN_FILE=/root/android.toolchain.cmake -DANDROID_ABI=armeabi-v7a -DBUILD_LGPL_SHARED_LIBS=OFF -DG2O_BUILD_APPS=OFF -DG2O_BUILD_EXAMPLES=OFF -DBUILD_SHARED_LIBS=OFF -DCMAKE_BUILD_TYPE=Release -DCMAKE_INSTALL_PREFIX=/opt/android/armeabi-v7a ..
+cmake -DCMAKE_TOOLCHAIN_FILE=$pwd/android.toolchain.cmake -DANDROID_ABI=armeabi-v7a -DBUILD_LGPL_SHARED_LIBS=OFF -DG2O_BUILD_APPS=OFF -DG2O_BUILD_EXAMPLES=OFF -DBUILD_SHARED_LIBS=OFF -DCMAKE_BUILD_TYPE=Release -DCMAKE_INSTALL_PREFIX=$prefix/armeabi-v7a ..
 make
 make install
 rm -r *
-cmake -DCMAKE_TOOLCHAIN_FILE=/root/android.toolchain.cmake -DANDROID_ABI=arm64-v8a -DBUILD_LGPL_SHARED_LIBS=OFF -DG2O_BUILD_APPS=OFF -DG2O_BUILD_EXAMPLES=OFF -DBUILD_SHARED_LIBS=OFF -DCMAKE_BUILD_TYPE=Release -DCMAKE_INSTALL_PREFIX=/opt/android/arm64-v8a ..
+cmake -DCMAKE_TOOLCHAIN_FILE=$pwd/android.toolchain.cmake -DANDROID_ABI=arm64-v8a -DBUILD_LGPL_SHARED_LIBS=OFF -DG2O_BUILD_APPS=OFF -DG2O_BUILD_EXAMPLES=OFF -DBUILD_SHARED_LIBS=OFF -DCMAKE_BUILD_TYPE=Release -DCMAKE_INSTALL_PREFIX=$prefix/arm64-v8a ..
 make
 make install
 cd
@@ -100,13 +113,13 @@ cd VTK
 git checkout tags/v7.1.1
 mkdir build
 cd build
-~/cmake-3.7.2-Linux-x86_64/bin/cmake -DBUILD_EXAMPLES=OFF -DBUILD_TESTING=OFF -DVTK_ANDROID_BUILD=ON -DANDROID_ARCH_ABI=armeabi-v7a -DBUILD_SHARED_LIBS=OFF -DCMAKE_BUILD_TYPE=Release -DCMAKE_INSTALL_PREFIX=/opt/android/armeabi-v7a ..
+~/cmake-3.7.2-Linux-x86_64/bin/cmake -DBUILD_EXAMPLES=OFF -DBUILD_TESTING=OFF -DVTK_ANDROID_BUILD=ON -DANDROID_ARCH_ABI=armeabi-v7a -DBUILD_SHARED_LIBS=OFF -DCMAKE_BUILD_TYPE=Release -DCMAKE_INSTALL_PREFIX=$prefix/armeabi-v7a ..
 make
-cp -r CMakeExternals/Install/vtk-android/* /opt/android/armeabi-v7a/.
+cp -r CMakeExternals/Install/vtk-android/* $prefix/armeabi-v7a/.
 rm -r *
-~/cmake-3.7.2-Linux-x86_64/bin/cmake -DBUILD_EXAMPLES=OFF -DBUILD_TESTING=OFF -DVTK_ANDROID_BUILD=ON -DANDROID_ARCH_ABI=arm64-v8a -DBUILD_SHARED_LIBS=OFF -DCMAKE_BUILD_TYPE=Release -DCMAKE_INSTALL_PREFIX=/opt/android/arm64-v8a ..
+~/cmake-3.7.2-Linux-x86_64/bin/cmake -DBUILD_EXAMPLES=OFF -DBUILD_TESTING=OFF -DVTK_ANDROID_BUILD=ON -DANDROID_ARCH_ABI=arm64-v8a -DBUILD_SHARED_LIBS=OFF -DCMAKE_BUILD_TYPE=Release -DCMAKE_INSTALL_PREFIX=$prefix/arm64-v8a ..
 make
-cp -r CMakeExternals/Install/vtk-android/* /opt/android/arm64-v8a/.
+cp -r CMakeExternals/Install/vtk-android/* $prefix/arm64-v8a/.
 cd
 rm -r VTK cmake-3.7.2-Linux-x86_64
 
@@ -120,14 +133,14 @@ 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!?
-cmake -DCMAKE_TOOLCHAIN_FILE=/root/android.toolchain.cmake -DANDROID_ABI=armeabi-v7a -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 -DCMAKE_BUILD_TYPE=Release -DCMAKE_INSTALL_PREFIX=/opt/android/armeabi-v7a ..
-cmake -DCMAKE_TOOLCHAIN_FILE=/root/android.toolchain.cmake -DANDROID_ABI=armeabi-v7a -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 -DCMAKE_BUILD_TYPE=Release -DCMAKE_INSTALL_PREFIX=/opt/android/armeabi-v7a ..
+cmake -DCMAKE_TOOLCHAIN_FILE=$pwd/android.toolchain.cmake -DANDROID_ABI=armeabi-v7a -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 -DCMAKE_BUILD_TYPE=Release -DCMAKE_INSTALL_PREFIX=$prefix/armeabi-v7a ..
+cmake -DCMAKE_TOOLCHAIN_FILE=$pwd/android.toolchain.cmake -DANDROID_ABI=armeabi-v7a -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 -DCMAKE_BUILD_TYPE=Release -DCMAKE_INSTALL_PREFIX=$prefix/armeabi-v7a ..
 make
 make install
 rm -r *
 # do it 2 times because there is a cmake error on the first time and not the second time!?
-cmake -DCMAKE_TOOLCHAIN_FILE=/root/android.toolchain.cmake -DANDROID_ABI=arm64-v8a -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 -DCMAKE_BUILD_TYPE=Release -DCMAKE_INSTALL_PREFIX=/opt/android/arm64-v8a ..
-cmake -DCMAKE_TOOLCHAIN_FILE=/root/android.toolchain.cmake -DANDROID_ABI=arm64-v8a -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 -DCMAKE_BUILD_TYPE=Release -DCMAKE_INSTALL_PREFIX=/opt/android/arm64-v8a ..
+cmake -DCMAKE_TOOLCHAIN_FILE=$pwd/android.toolchain.cmake -DANDROID_ABI=arm64-v8a -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 -DCMAKE_BUILD_TYPE=Release -DCMAKE_INSTALL_PREFIX=$prefix/arm64-v8a ..
+cmake -DCMAKE_TOOLCHAIN_FILE=$pwd/android.toolchain.cmake -DANDROID_ABI=arm64-v8a -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 -DCMAKE_BUILD_TYPE=Release -DCMAKE_INSTALL_PREFIX=$prefix/arm64-v8a ..
 make
 make install
 cd
@@ -143,11 +156,11 @@ cd opencv
 git checkout tags/3.2.0
 mkdir build
 cd build
-cmake -DCMAKE_TOOLCHAIN_FILE=/root/android.toolchain.cmake -DANDROID_ABI=armeabi-v7a -DOPENCV_EXTRA_MODULES_PATH=/root/opencv_contrib/modules -DCMAKE_BUILD_TYPE=Release -DBUILD_SHARED_LIBS=OFF -DBUILD_TESTS=OFF -DBUILD_PERF_TESTS=OFF -DCMAKE_INSTALL_PREFIX=/opt/android/armeabi-v7a ..
+cmake -DCMAKE_TOOLCHAIN_FILE=$pwd/android.toolchain.cmake -DANDROID_ABI=armeabi-v7a -DOPENCV_EXTRA_MODULES_PATH=$pwd/opencv_contrib/modules -DCMAKE_BUILD_TYPE=Release -DBUILD_SHARED_LIBS=OFF -DBUILD_TESTS=OFF -DBUILD_PERF_TESTS=OFF -DCMAKE_INSTALL_PREFIX=$prefix/armeabi-v7a ..
 make
 make install
 rm -r *
-cmake -DCMAKE_TOOLCHAIN_FILE=/root/android.toolchain.cmake -DANDROID_ABI=arm64-v8a -DOPENCV_EXTRA_MODULES_PATH=/root/opencv_contrib/modules -DCMAKE_BUILD_TYPE=Release -DBUILD_SHARED_LIBS=OFF -DBUILD_TESTS=OFF -DBUILD_PERF_TESTS=OFF -DCMAKE_INSTALL_PREFIX=/opt/android/arm64-v8a ..
+cmake -DCMAKE_TOOLCHAIN_FILE=$pwd/android.toolchain.cmake -DANDROID_ABI=arm64-v8a -DOPENCV_EXTRA_MODULES_PATH=$pwd/opencv_contrib/modules -DCMAKE_BUILD_TYPE=Release -DBUILD_SHARED_LIBS=OFF -DBUILD_TESTS=OFF -DBUILD_PERF_TESTS=OFF -DCMAKE_INSTALL_PREFIX=$prefix/arm64-v8a ..
 make
 make install
 cd

docker/xenial/android/setup_android_env.bash

@@ -22,9 +22,4 @@ echo "wget android-ndk..."
 wget -nv https://dl.google.com/android/repository/android-ndk-r14-linux-x86_64.zip
 unzip -qq android-ndk-r14-linux-x86_64.zip
 rm android-ndk-r14-linux-x86_64.zip
-mv android-ndk-r14 /opt/.
-
-# Install directory for all dependencies
-mkdir -p /opt/android/armeabi-v7a
-mkdir -p /opt/android/arm64-v8a
-wget -nv https://github.com/introlab/rtabmap/raw/master/cmake_modules/android.toolchain.cmake
+mv android-ndk-r14 $ANDROID_NDK

docker/xenial/android/tango/Dockerfile

@@ -6,6 +6,6 @@ WORKDIR /root/
 
 ADD build.bash /root/build.bash
 RUN chmod +x build.bash
-RUN /bin/bash -c "./build.bash"
+RUN /bin/bash -c "./build.bash /opt/android"
 
 WORKDIR /root/

docker/xenial/android/tango/build.bash

@@ -1,5 +1,12 @@
 #!/bin/bash
 
+if [ $# -ne 1 ]; then
+    echo "build.bash android_install_prefix   # Example: build.bash /opt/android"
+    exit 1
+fi
+
+prefix=$1
+
 # get rtabmap
 git clone https://github.com/introlab/rtabmap.git rtabmap-tango
 
@@ -7,33 +14,39 @@ git clone https://github.com/introlab/rtabmap.git rtabmap-tango
 wget https://developers.google.com/tango/downloads/TangoSDK_Ikariotikos_C.zip
 unzip -qq TangoSDK_Ikariotikos_C.zip
 rm TangoSDK_Ikariotikos_C.zip
-cp -r lib_tango_client_api/include/* /opt/android/armeabi-v7a/include/.
-cp -r lib_tango_client_api/include/* /opt/android/arm64-v8a/include/.
-cp -r lib_tango_client_api/lib/armeabi-v7a/* /opt/android/armeabi-v7a/lib/.
-cp -r lib_tango_client_api/lib/arm64-v8a/* /opt/android/arm64-v8a/lib/.
+cp -r lib_tango_client_api/include/* $prefix/armeabi-v7a/include/.
+cp -r lib_tango_client_api/include/* $prefix/arm64-v8a/include/.
+cp -r lib_tango_client_api/lib/armeabi-v7a/* $prefix/armeabi-v7a/lib/.
+cp -r lib_tango_client_api/lib/arm64-v8a/* $prefix/arm64-v8a/lib/.
 rm -r lib_tango_client_api
 wget https://developers.google.com/tango/downloads/TangoSupport_Ikariotikos_C.zip
 unzip -qq TangoSupport_Ikariotikos_C.zip
 rm TangoSupport_Ikariotikos_C.zip
-cp -r lib_tango_support_api/include/* /opt/android/armeabi-v7a/include/.
-cp -r lib_tango_support_api/include/* /opt/android/arm64-v8a/include/.
-cp -r lib_tango_support_api/lib/armeabi-v7a/* /opt/android/armeabi-v7a/lib/.
-cp -r lib_tango_support_api/lib/arm64-v8a/* /opt/android/arm64-v8a/lib/.
+cp -r lib_tango_support_api/include/* $prefix/armeabi-v7a/include/.
+cp -r lib_tango_support_api/include/* $prefix/arm64-v8a/include/.
+cp -r lib_tango_support_api/lib/armeabi-v7a/* $prefix/armeabi-v7a/lib/.
+cp -r lib_tango_support_api/lib/arm64-v8a/* $prefix/arm64-v8a/lib/.
 cp -r lib_tango_support_api/lib/* rtabmap-tango/app/android/jni/third-party/lib/.
 rm -r lib_tango_support_api
 wget https://developers.google.com/tango/downloads/TangoSDK_Ikariotikos_Java.jar
 mv TangoSDK_Ikariotikos_Java.jar rtabmap-tango/app/android/libs/.
 
+# resource tool
+cd rtabmap-tango/build
+cmake -DANDROID_PREBUILD=ON ..
+make
+cd
+
 # rtabmap
 mkdir rtabmap-tango/build/armeabi-v7a
 cd rtabmap-tango/build/armeabi-v7a
-cmake -DCMAKE_TOOLCHAIN_FILE=../../cmake_modules/android.toolchain.cmake -DANDROID_ABI=armeabi-v7a -DBUILD_SHARED_LIBS=OFF -DBUILD_EXAMPLES=OFF -DBUILD_TOOLS=OFF -DCMAKE_BUILD_TYPE=Release -DOpenCV_DIR=/opt/android/armeabi-v7a/sdk/native/jni -DCMAKE_INSTALL_PREFIX=/opt/android/armeabi-v7a ../..
+cmake -DCMAKE_TOOLCHAIN_FILE=../../cmake_modules/android.toolchain.cmake -DANDROID_ABI=armeabi-v7a -DBUILD_SHARED_LIBS=OFF -DBUILD_EXAMPLES=OFF -DBUILD_TOOLS=OFF -DCMAKE_BUILD_TYPE=Release -DOpenCV_DIR=$prefix/armeabi-v7a/sdk/native/jni -DCMAKE_INSTALL_PREFIX=$prefix/armeabi-v7a ../..
 make
 
 cd
 mkdir rtabmap-tango/build/arm64-v8a
 cd rtabmap-tango/build/arm64-v8a
-cmake -DCMAKE_TOOLCHAIN_FILE=../../cmake_modules/android.toolchain.cmake -DANDROID_ABI=arm64-v8a -DBUILD_SHARED_LIBS=OFF -DBUILD_EXAMPLES=OFF -DBUILD_TOOLS=OFF -DCMAKE_BUILD_TYPE=Release -DOpenCV_DIR=/opt/android/arm64-v8a/sdk/native/jni -DCMAKE_INSTALL_PREFIX=/opt/android/arm64-v8a ../..
+cmake -DCMAKE_TOOLCHAIN_FILE=../../cmake_modules/android.toolchain.cmake -DANDROID_ABI=arm64-v8a -DBUILD_SHARED_LIBS=OFF -DBUILD_EXAMPLES=OFF -DBUILD_TOOLS=OFF -DCMAKE_BUILD_TYPE=Release -DOpenCV_DIR=$prefix/arm64-v8a/sdk/native/jni -DCMAKE_INSTALL_PREFIX=$prefix/arm64-v8a ../..
 make
 
 # package with binaries of both architectures

docker/zesty/Dockerfile

@@ -0,0 +1,33 @@
+# Image: introlab3it/rtabmap:zesty
+
+FROM ubuntu:17.04
+
+# Install build dependencies
+RUN apt-get update && apt-get install -y \
+      libsqlite3-dev \
+      libpcl-dev \
+      libopencv-dev \
+      git \
+      cmake \
+      libproj-dev \
+      libqt5svg5-dev \
+      software-properties-common
+
+# Issue: http://www.pcl-users.org/Build-failure-on-Ubuntu-17-04-td4044552.html
+RUN sed -i 's|/usr/lib/libmpi.so;||g' /usr/lib/x86_64-linux-gnu/cmake/pcl/PCLConfig.cmake
+
+# Clone source code
+WORKDIR /root/
+RUN git clone https://github.com/introlab/rtabmap.git
+
+# Build RTAB-Map project
+RUN cd rtabmap/build && \
+    cmake .. && \
+    make -j$(nproc) && \
+    make install && \
+    cd ../.. && \
+    rm -rf rtabmap && \
+    ldconfig
+
+WORKDIR /root
+    

examples/RGBDMapping/CMakeLists.txt

@@ -29,6 +29,12 @@ else()
     	IF(NOT Qt5_FOUND)
        		FIND_PACKAGE(Qt4 COMPONENTS QtCore QtGui QtSvg)
     	ENDIF(NOT Qt5_FOUND)
+
+	# fix libproj.so not found on Xenial
+	if(NOT "${PCL_LIBRARIES}" STREQUAL "")
+   		list(REMOVE_ITEM PCL_LIBRARIES "vtkproj4")
+	endif()
+
 endif()
 
 if(POLICY CMP0020)

examples/RGBDMapping/main.cpp

@@ -38,6 +38,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #include <stdio.h>
 #include <pcl/io/pcd_io.h>
 #include <pcl/io/ply_io.h>
+#include <pcl/filters/filter.h>
 
 #include "MapBuilder.h"
 
@@ -216,8 +217,14 @@ int main(int argc, char * argv[])
 				node.sensorData(),
 				4,           // image decimation before creating the clouds
 				4.0f,        // maximum depth of the cloud
-				0.01f);  // Voxel grid filtering
-		*cloud += *util3d::transformPointCloud(tmp, iter->second); // transform the point cloud to its pose
+				0.0f);
+		pcl::PointCloud<pcl::PointXYZRGB>::Ptr tmpNoNaN(new pcl::PointCloud<pcl::PointXYZRGB>);
+		std::vector<int> index;
+		pcl::removeNaNFromPointCloud(*tmp, *tmpNoNaN, index);
+		if(!tmpNoNaN->empty())
+		{
+			*cloud += *util3d::transformPointCloud(tmpNoNaN, iter->second); // transform the point cloud to its pose
+		}
 	}
 	if(cloud->size())
 	{

guilib/include/rtabmap/gui/GraphViewer.h

@@ -135,6 +135,7 @@ public:
 
 signals:
 	void configChanged();
+	void mapShownRequested();
 
 public slots:
 	void restoreDefaults();

guilib/src/CMakeLists.txt

@@ -161,7 +161,11 @@ ADD_LIBRARY(rtabmap_gui ${SRC_FILES})
 
 TARGET_LINK_LIBRARIES(rtabmap_gui rtabmap_core rtabmap_utilite ${LIBRARIES})
 IF(Qt5_FOUND)
-    QT5_USE_MODULES(rtabmap_gui Widgets Core Gui Svg PrintSupport)
+    IF(Qt5Svg_FOUND)
+        QT5_USE_MODULES(rtabmap_gui Widgets Core Gui Svg PrintSupport)
+    ELSE()
+        QT5_USE_MODULES(rtabmap_gui Widgets Core Gui PrintSupport)
+    ENDIF()
 ENDIF(Qt5_FOUND)
 
 SET_TARGET_PROPERTIES(

guilib/src/DatabaseViewer.cpp

@@ -296,6 +296,7 @@ DatabaseViewer::DatabaseViewer(const QString & ini, QWidget * parent) :
 	connect(ui_->horizontalSlider_iterations, SIGNAL(sliderMoved(int)), this, SLOT(sliderIterationsValueChanged(int)));
 	connect(ui_->spinBox_optimizationsFrom, SIGNAL(editingFinished()), this, SLOT(updateGraphView()));
 	connect(ui_->checkBox_spanAllMaps, SIGNAL(stateChanged(int)), this, SLOT(updateGraphView()));
+	connect(ui_->graphViewer, SIGNAL(mapShownRequested()), this, SLOT(updateGraphView()));
 	connect(ui_->checkBox_ignorePoseCorrection, SIGNAL(stateChanged(int)), this, SLOT(updateGraphView()));
 	connect(ui_->checkBox_ignorePoseCorrection, SIGNAL(stateChanged(int)), this, SLOT(updateConstraintView()));
 	connect(ui_->checkBox_ignoreGlobalLoop, SIGNAL(stateChanged(int)), this, SLOT(updateGraphView()));
@@ -2559,6 +2560,7 @@ void DatabaseViewer::update(int value,
 							if(!data.imageRaw().empty())
 							{
 								pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud;
+								pcl::IndicesPtr indices(new std::vector<int>);
 								if(!data.depthRaw().empty() && data.cameraModels().size()==1)
 								{
 									cv::Mat depth = data.depthRaw();
@@ -2569,8 +2571,9 @@ void DatabaseViewer::update(int value,
 									cloud = util3d::cloudFromDepthRGB(
 											data.imageRaw(),
 											depth,
-											data.cameraModels()[0]);
-									if(cloud->size())
+											data.cameraModels()[0],
+											1,0,0,indices.get());
+									if(indices->size())
 									{
 										cloud = util3d::transformPointCloud(cloud, data.cameraModels()[0].localTransform());
 									}
@@ -2578,13 +2581,13 @@ void DatabaseViewer::update(int value,
 								}
 								else
 								{
-									cloud = util3d::cloudRGBFromSensorData(data, 1, 0, 0, 0, ui_->parameters_toolbox->getParameters());
+									cloud = util3d::cloudRGBFromSensorData(data, 1, 0, 0, indices.get(), ui_->parameters_toolbox->getParameters());
 								}
-								if(cloud->size())
+								if(indices->size())
 								{
 									if(ui_->doubleSpinBox_voxelSize->value() > 0.0)
 									{
-										cloud = util3d::voxelize(cloud, ui_->doubleSpinBox_voxelSize->value());
+										cloud = util3d::voxelize(cloud, indices, ui_->doubleSpinBox_voxelSize->value());
 									}
 
 									if(ui_->checkBox_showMesh->isChecked() && !cloud->is_dense)
@@ -2645,12 +2648,13 @@ void DatabaseViewer::update(int value,
 							else if(ui_->checkBox_showCloud->isChecked())
 							{
 								pcl::PointCloud<pcl::PointXYZ>::Ptr cloud;
-								cloud = util3d::cloudFromSensorData(data, 1, 0, 0, 0, ui_->parameters_toolbox->getParameters());
-								if(cloud->size())
+								pcl::IndicesPtr indices(new std::vector<int>);
+								cloud = util3d::cloudFromSensorData(data, 1, 0, 0, indices.get(), ui_->parameters_toolbox->getParameters());
+								if(indices->size())
 								{
 									if(ui_->doubleSpinBox_voxelSize->value() > 0.0)
 									{
-										cloud = util3d::voxelize(cloud, ui_->doubleSpinBox_voxelSize->value());
+										cloud = util3d::voxelize(cloud, indices, ui_->doubleSpinBox_voxelSize->value());
 									}
 
 									cloudViewer_->addCloud("cloud", cloud, pose);
@@ -3498,15 +3502,15 @@ void DatabaseViewer::updateConstraintView(
 				cloudTo=util3d::cloudRGBFromSensorData(dataTo, 1, 0, 0, indicesTo.get(), ui_->parameters_toolbox->getParameters());
 			}
 
-			if(cloudTo.get() && cloudTo->size())
+			if(cloudTo.get() && indicesTo->size())
 			{
 				cloudTo = rtabmap::util3d::transformPointCloud(cloudTo, t);
 			}
 
 			// Gain compensation
 			if(ui_->doubleSpinBox_gainCompensationRadius->value()>0.0 &&
-				cloudFrom.get() && cloudFrom->size() &&
-				cloudTo.get() && cloudTo->size())
+				cloudFrom.get() && indicesFrom->size() &&
+				cloudTo.get() && indicesTo->size())
 			{
 				UTimer t;
 				GainCompensator compensator(ui_->doubleSpinBox_gainCompensationRadius->value());
@@ -3516,19 +3520,19 @@ void DatabaseViewer::updateConstraintView(
 				UINFO("Gain compensation time = %fs", t.ticks());
 			}
 
-			if(cloudFrom.get() && cloudFrom->size())
+			if(cloudFrom.get() && indicesFrom->size())
 			{
 				if(ui_->doubleSpinBox_voxelSize->value() > 0.0)
 				{
-					cloudFrom = util3d::voxelize(cloudFrom, ui_->doubleSpinBox_voxelSize->value());
+					cloudFrom = util3d::voxelize(cloudFrom, indicesFrom, ui_->doubleSpinBox_voxelSize->value());
 				}
 				constraintsViewer_->addCloud("cloud0", cloudFrom, pose, Qt::red);
 			}
-			if(cloudTo.get() && cloudTo->size())
+			if(cloudTo.get() && indicesTo->size())
 			{
 				if(ui_->doubleSpinBox_voxelSize->value() > 0.0)
 				{
-					cloudTo = util3d::voxelize(cloudTo, ui_->doubleSpinBox_voxelSize->value());
+					cloudTo = util3d::voxelize(cloudTo, indicesTo, ui_->doubleSpinBox_voxelSize->value());
 				}
 				constraintsViewer_->addCloud("cloud1", cloudTo, pose, Qt::cyan);
 			}
@@ -3751,45 +3755,51 @@ void DatabaseViewer::updateConstraintView(
 			// Added loop closure scans
 			constraintsViewer_->removeCloud("scan0");
 			constraintsViewer_->removeCloud("scan1");
-			if(dataFrom.laserScanRaw().channels() == 6)
+			if(!dataFrom.laserScanRaw().empty())
 			{
-				pcl::PointCloud<pcl::PointNormal>::Ptr scan;
-				scan = rtabmap::util3d::laserScanToPointCloudNormal(dataFrom.laserScanRaw(), dataFrom.laserScanInfo().localTransform());
-				if(ui_->doubleSpinBox_voxelSize->value() > 0.0)
+				if(dataFrom.laserScanRaw().channels() == 6)
 				{
-					scan = util3d::voxelize(scan, ui_->doubleSpinBox_voxelSize->value());
+					pcl::PointCloud<pcl::PointNormal>::Ptr scan;
+					scan = rtabmap::util3d::laserScanToPointCloudNormal(dataFrom.laserScanRaw(), dataFrom.laserScanInfo().localTransform());
+					if(ui_->doubleSpinBox_voxelSize->value() > 0.0)
+					{
+						scan = util3d::voxelize(scan, ui_->doubleSpinBox_voxelSize->value());
+					}
+					constraintsViewer_->addCloud("scan0", scan, pose, Qt::yellow);
+				}
+				else
+				{
+					pcl::PointCloud<pcl::PointXYZ>::Ptr scan;
+					scan = rtabmap::util3d::laserScanToPointCloud(dataFrom.laserScanRaw(), dataFrom.laserScanInfo().localTransform());
+					if(ui_->doubleSpinBox_voxelSize->value() > 0.0)
+					{
+						scan = util3d::voxelize(scan, ui_->doubleSpinBox_voxelSize->value());
+					}
+					constraintsViewer_->addCloud("scan0", scan, pose, Qt::yellow);
 				}
-				constraintsViewer_->addCloud("scan0", scan, pose, Qt::yellow);
 			}
-			else
+			if(!dataTo.laserScanRaw().empty())
 			{
-				pcl::PointCloud<pcl::PointXYZ>::Ptr scan;
-				scan = rtabmap::util3d::laserScanToPointCloud(dataFrom.laserScanRaw(), dataFrom.laserScanInfo().localTransform());
-				if(ui_->doubleSpinBox_voxelSize->value() > 0.0)
+				if(dataTo.laserScanRaw().channels() == 6)
 				{
-					scan = util3d::voxelize(scan, ui_->doubleSpinBox_voxelSize->value());
+					pcl::PointCloud<pcl::PointNormal>::Ptr scan;
+					scan = rtabmap::util3d::laserScanToPointCloudNormal(dataTo.laserScanRaw(), t*dataTo.laserScanInfo().localTransform());
+					if(ui_->doubleSpinBox_voxelSize->value() > 0.0)
+					{
+						scan = util3d::voxelize(scan, ui_->doubleSpinBox_voxelSize->value());
+					}
+					constraintsViewer_->addCloud("scan1", scan, pose, Qt::magenta);
 				}
-				constraintsViewer_->addCloud("scan0", scan, pose, Qt::yellow);
-			}
-			if(dataTo.laserScanRaw().channels() == 6)
-			{
-				pcl::PointCloud<pcl::PointNormal>::Ptr scan;
-				scan = rtabmap::util3d::laserScanToPointCloudNormal(dataTo.laserScanRaw(), t*dataTo.laserScanInfo().localTransform());
-				if(ui_->doubleSpinBox_voxelSize->value() > 0.0)
+				else
 				{
-					scan = util3d::voxelize(scan, ui_->doubleSpinBox_voxelSize->value());
+					pcl::PointCloud<pcl::PointXYZ>::Ptr scan;
+					scan = rtabmap::util3d::laserScanToPointCloud(dataTo.laserScanRaw(), t*dataTo.laserScanInfo().localTransform());
+					if(ui_->doubleSpinBox_voxelSize->value() > 0.0)
+					{
+						scan = util3d::voxelize(scan, ui_->doubleSpinBox_voxelSize->value());
+					}
+					constraintsViewer_->addCloud("scan1", scan, pose, Qt::magenta);
 				}
-				constraintsViewer_->addCloud("scan1", scan, pose, Qt::magenta);
-			}
-			else
-			{
-				pcl::PointCloud<pcl::PointXYZ>::Ptr scan;
-				scan = rtabmap::util3d::laserScanToPointCloud(dataTo.laserScanRaw(), t*dataTo.laserScanInfo().localTransform());
-				if(ui_->doubleSpinBox_voxelSize->value() > 0.0)
-				{
-					scan = util3d::voxelize(scan, ui_->doubleSpinBox_voxelSize->value());
-				}
-				constraintsViewer_->addCloud("scan1", scan, pose, Qt::magenta);
 			}
 		}
 
@@ -4090,7 +4100,7 @@ void DatabaseViewer::sliderIterationsValueChanged(int value)
 					++iter;
 				}
 			}
-			UINFO("Update local maps list... done");
+			UINFO("Update local maps list... done (%d local maps, graph size=%d)", (int)localMaps.size(), (int)graph.size());
 		}
 
 		ui_->graphViewer->updateGTGraph(groundTruthPoses_);
@@ -4835,6 +4845,15 @@ bool DatabaseViewer::addConstraint(int from, int to, bool silent)
 				toS->sensorData().setFeatures(std::vector<cv::KeyPoint>(), std::vector<cv::Point3f>(), cv::Mat());
 			}
 		}
+		else if(!reextractVisualFeatures && fromS->getWords().empty() && toS->getWords().empty())
+		{
+			UWARN("\"%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());
+		}
 
 		t = reg->computeTransformationMod(*fromS, *toS, Transform(), &info);
 		delete reg;

guilib/src/DepthCalibrationDialog.cpp

@@ -227,25 +227,54 @@ void DepthCalibrationDialog::calibrate(
 	_ui->label_width->setText("NA");
 	_ui->label_height->setText("NA");
 	_imageSize = cv::Size();
+	CameraModel model;
 	if(cachedSignatures.size())
 	{
 		const Signature & s = cachedSignatures.begin().value();
 		const SensorData & data = s.sensorData();
-		if(data.cameraModels().size() == 1 && data.cameraModels()[0].isValidForReprojection())
+		cv::Mat depth;
+		data.uncompressDataConst(0, &depth);
+		if(data.cameraModels().size() == 1 && data.cameraModels()[0].isValidForProjection() && !depth.empty())
 		{
-			_imageSize = data.cameraModels()[0].imageSize();
-			_ui->label_width->setNum(data.cameraModels()[0].imageWidth());
-			_ui->label_height->setNum(data.cameraModels()[0].imageHeight());
+			// use depth image size
+			_imageSize = depth.size();
+			_ui->label_width->setNum(_imageSize.width);
+			_ui->label_height->setNum(_imageSize.height);
 
-			if(data.cameraModels()[0].imageWidth() % _ui->spinBox_bin_width->value() != 0 ||
-			   data.cameraModels()[0].imageHeight() % _ui->spinBox_bin_height->value() != 0)
+			if(_imageSize.width % _ui->spinBox_bin_width->value() != 0 ||
+				_imageSize.height % _ui->spinBox_bin_height->value() != 0)
 			{
 				size_t bin_width, bin_height;
-				clams::DiscreteDepthDistortionModel::getBinSize(data.cameraModels()[0].imageWidth(), data.cameraModels()[0].imageHeight(), bin_width, bin_height);
+				clams::DiscreteDepthDistortionModel::getBinSize(_imageSize.width, _imageSize.height, bin_width, bin_height);
 				_ui->spinBox_bin_width->setValue(bin_width);
 				_ui->spinBox_bin_height->setValue(bin_height);
 			}
 		}
+		else if(data.cameraModels().size() > 1)
+		{
+			QMessageBox::warning(this, tr("Depth Calibration"),tr("Multi-camera not supported!"));
+			return;
+		}
+		else if(data.cameraModels().size() != 1)
+		{
+			QMessageBox::warning(this, tr("Depth Calibration"), tr("Camera model not found."));
+			return;
+		}
+		else if(data.cameraModels().size() == 1 && !data.cameraModels()[0].isValidForProjection())
+		{
+			QMessageBox::warning(this, tr("Depth Calibration"), tr("Camera model %1 not valid for projection.").arg(s.id()));
+			return;
+		}
+		else
+		{
+			QMessageBox::warning(this, tr("Depth Calibration"), tr("Depth image cannot be found in the cache, make sure to update cache before doing calibration."));
+			return;
+		}
+	}
+	else
+	{
+		QMessageBox::warning(this, tr("Depth Calibration"), tr("No signatures detected! Map is empty!?"));
+		return;
 	}
 
 	if(this->exec() == QDialog::Accepted)
@@ -285,65 +314,63 @@ void DepthCalibrationDialog::calibrate(
 				{
 					const Signature & s = cachedSignatures.find(iter->first).value();
 					SensorData data = s.sensorData();
-					if(data.cameraModels().size() == 1 && data.cameraModels()[0].isValidForReprojection())
+
+					cv::Mat  depth, laserScan;
+					data.uncompressData(0, &depth, _ui->checkBox_laserScan->isChecked()?&laserScan:0);
+					if(data.cameraModels().size() == 1 && data.cameraModels()[0].isValidForProjection() && !depth.empty())
 					{
-						cv::Mat image, depth, laserScan;
-						data.uncompressData(&image, &depth, _ui->checkBox_laserScan->isChecked()?&laserScan:0);
-						if(!image.empty() && !depth.empty())
+						UASSERT(iter->first == data.id());
+						pcl::PointCloud<pcl::PointXYZ>::Ptr cloud;
+
+						if(_ui->checkBox_laserScan->isChecked())
 						{
-							UASSERT(iter->first == data.id());
-							pcl::PointCloud<pcl::PointXYZ>::Ptr cloud;
-
-							if(_ui->checkBox_laserScan->isChecked())
+							cloud = util3d::laserScanToPointCloud(laserScan);
+							indices->resize(cloud->size());
+							for(unsigned int i=0; i<indices->size(); ++i)
 							{
-								cloud = util3d::laserScanToPointCloud(laserScan);
-								indices->resize(cloud->size());
-								for(unsigned int i=0; i<indices->size(); ++i)
-								{
-									indices->at(i) = i;
-								}
+								indices->at(i) = i;
 							}
-							else
+						}
+						else
+						{
+							cloud = util3d::cloudFromSensorData(
+									data,
+									_ui->spinBox_decimation->value(),
+									_ui->doubleSpinBox_maxDepth->value(),
+									_ui->doubleSpinBox_minDepth->value(),
+									indices.get(),
+									parameters);
+						}
+
+						if(indices->size())
+						{
+							if(_ui->doubleSpinBox_voxelSize->value() > 0.0)
 							{
-								cloud = util3d::cloudFromSensorData(
-										data,
-										_ui->spinBox_decimation->value(),
-										_ui->doubleSpinBox_maxDepth->value(),
-										_ui->doubleSpinBox_minDepth->value(),
-										indices.get(),
-										parameters);
+								cloud = util3d::voxelize(cloud, indices, _ui->doubleSpinBox_voxelSize->value());
 							}
 
-							if(indices->size())
+							cloud = util3d::transformPointCloud(cloud, iter->second);
+
+							points+=cloud->size();
+
+							*map += *cloud;
+
+							sequence.insert(std::make_pair(iter->first, data));
+
+							cv::Size size = depth.size();
+							if(_model &&
+								(_model->getWidth()!=size.width ||
+								 _model->getHeight()!=size.height))
 							{
-								if(_ui->doubleSpinBox_voxelSize->value() > 0.0)
-								{
-									cloud = util3d::voxelize(cloud, indices, _ui->doubleSpinBox_voxelSize->value());
-								}
-
-								cloud = util3d::transformPointCloud(cloud, iter->second);
-
-								points+=cloud->size();
-
-								*map += *cloud;
-
-								sequence.insert(std::make_pair(iter->first, data));
-
-								cv::Size size = data.cameraModels()[0].imageSize();
-								if(_model &&
-									(_model->getWidth()!=size.width ||
-								     _model->getHeight()!=size.height))
-								{
-									QString msg = tr("Depth images (%1x%2) in the map don't have the "
-											   "same size then in the current model (%3x%4). You may want "
-											   "to check \"Reset previous model\" before trying again.")
-											   .arg(size.width).arg(size.height)
-											   .arg(_model->getWidth()).arg(_model->getHeight());
-									QMessageBox::warning(this, tr("Depth Calibration"), msg);
-									_progressDialog->appendText(msg, Qt::darkRed);
-									_progressDialog->setAutoClose(false);
-									return;
-								}
+								QString msg = tr("Depth images (%1x%2) in the map don't have the "
+										   "same size then in the current model (%3x%4). You may want "
+										   "to check \"Reset previous model\" before trying again.")
+										   .arg(size.width).arg(size.height)
+										   .arg(_model->getWidth()).arg(_model->getHeight());
+								QMessageBox::warning(this, tr("Depth Calibration"), msg);
+								_progressDialog->appendText(msg, Qt::darkRed);
+								_progressDialog->setAutoClose(false);
+								return;
 							}
 						}
 					}
@@ -434,7 +461,7 @@ void DepthCalibrationDialog::calibrate(
 			QDialog * dialog = new QDialog(this->parentWidget()?this->parentWidget():this, Qt::Window);
 			dialog->setAttribute(Qt::WA_DeleteOnClose, true);
 			dialog->setWindowTitle(tr("Original/Map"));
-			dialog->setMinimumWidth(sequence.begin()->second.cameraModels()[0].imageWidth());
+			dialog->setMinimumWidth(_imageSize.width);
 			ImageView * imageView1 = new ImageView(dialog);
 			imageView1->setMinimumSize(320, 240);
 			ImageView * imageView2 = new ImageView(dialog);
@@ -450,7 +477,7 @@ void DepthCalibrationDialog::calibrate(
 			}
 
 			//clams::DiscreteDepthDistortionModel model = clams::calibrate(sequence, poses, map);
-			const cv::Size & imageSize = sequence.begin()->second.cameraModels()[0].imageSize();
+			const cv::Size & imageSize = _imageSize;
 			if(_model == 0)
 			{
 				size_t bin_width = _ui->spinBox_bin_width->value();
@@ -487,24 +514,33 @@ void DepthCalibrationDialog::calibrate(
 				  cv::Mat depthImage;
 				  ster->second.uncompressDataConst(0, &depthImage);
 
-				  cv::Mat mapDepth;
-				  clams::FrameProjector projector(ster->second.cameraModels()[0]);
-				  mapDepth = projector.estimateMapDepth(
-						  map,
-						  iter->second.inverse(),
-						  depthImage,
-						 _ui->doubleSpinBox_coneRadius->value(),
-						 _ui->doubleSpinBox_coneStdDevThresh->value());
-
-				  if(ULogger::level() == ULogger::kDebug)
+				  if(ster->second.cameraModels().size() == 1 && ster->second.cameraModels()[0].isValidForProjection() && !depthImage.empty())
 				  {
-					  imageView1->setImage(uCvMat2QImage(depthImage));
-					  imageView2->setImage(uCvMat2QImage(mapDepth));
-				  }
+					  cv::Mat mapDepth;
+					  CameraModel model = ster->second.cameraModels()[0];
+					  if(model.imageWidth() != depthImage.cols)
+					  {
+						  UASSERT_MSG(model.imageHeight() % depthImage.rows == 0, uFormat("rgb=%d depth=%d", model.imageHeight(), depthImage.rows).c_str());
+						  model = model.scaled(double(depthImage.rows) / double(model.imageHeight()));
+					  }
+					  clams::FrameProjector projector(model);
+					  mapDepth = projector.estimateMapDepth(
+							  map,
+							  iter->second.inverse(),
+							  depthImage,
+							 _ui->doubleSpinBox_coneRadius->value(),
+							 _ui->doubleSpinBox_coneStdDevThresh->value());
 
-				  counts = _model->accumulate(mapDepth, depthImage);
-				  _progressDialog->appendText(tr("Added %1 training examples from node %2 (%3/%4).").arg(counts).arg(iter->first).arg(++index).arg(sequence.size()));
-			  }
+					  if(ULogger::level() == ULogger::kDebug)
+					  {
+						  imageView1->setImage(uCvMat2QImage(depthImage));
+						  imageView2->setImage(uCvMat2QImage(mapDepth));
+					  }
+
+					  counts = _model->accumulate(mapDepth, depthImage);
+					  _progressDialog->appendText(tr("Added %1 training examples from node %2 (%3/%4).").arg(counts).arg(iter->first).arg(++index).arg(sequence.size()));
+				  }
+				 }
 			  _progressDialog->incrementStep();
 			  QApplication::processEvents();
 			}

guilib/src/GraphViewer.cpp

@@ -37,7 +37,9 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #include <QtGui/QDesktopServices>
 #include <QtGui/QContextMenuEvent>
 #include <QColorDialog>
+#ifdef QT_SVG_LIB
 #include <QtSvg/QSvgGenerator>
+#endif
 #include <QInputDialog>
 #include <QMessageBox>
 
@@ -1176,19 +1178,19 @@ void GraphViewer::setMaxLinkLength(float value)
 }
 void GraphViewer::setGraphVisible(bool visible)
 {
-	_graphRoot->setVisible(!_graphRoot->isVisible());
+	_graphRoot->setVisible(visible);
 }
 void GraphViewer::setGlobalPathVisible(bool visible)
 {
-	_globalPathRoot->setVisible(!_globalPathRoot->isVisible());
+	_globalPathRoot->setVisible(visible);
 }
 void GraphViewer::setLocalPathVisible(bool visible)
 {
-	_localPathRoot->setVisible(!_localPathRoot->isVisible());
+	_localPathRoot->setVisible(visible);
 }
 void GraphViewer::setGtGraphVisible(bool visible)
 {
-	_gtGraphRoot->setVisible(!_gtGraphRoot->isVisible());
+	_gtGraphRoot->setVisible(visible);
 }
 
 void GraphViewer::restoreDefaults()
@@ -1233,6 +1235,9 @@ void GraphViewer::contextMenuEvent(QContextMenuEvent * event)
 	QMenu menu;
 	QAction * aScreenShotPNG = menu.addAction(tr("Take a screenshot (PNG)"));
 	QAction * aScreenShotSVG = menu.addAction(tr("Take a screenshot (SVG)"));
+#ifndef QT_SVG_LIB
+	aScreenShotSVG->setEnabled(false);
+#endif
 	menu.addSeparator();
 
 	QAction * aChangeNodeColor = menu.addAction(createIcon(_nodeColor), tr("Set node color..."));
@@ -1361,7 +1366,6 @@ void GraphViewer::contextMenuEvent(QContextMenuEvent * event)
 	{
 		aShowHideGtGraph = menu.addAction(tr("Show ground truth graph"));
 	}
-	aShowHideGridMap->setEnabled(!_gridMap->pixmap().isNull());
 	aShowHideGraph->setEnabled(_nodeItems.size());
 	aShowHideGlobalPath->setEnabled(_globalPathLinkItems.size());
 	aShowHideLocalPath->setEnabled(_localPathLinkItems.size());
@@ -1423,6 +1427,7 @@ void GraphViewer::contextMenuEvent(QContextMenuEvent * event)
 			}
 			else
 			{
+#ifdef QT_SVG_LIB
 				QSvgGenerator svgGen;
 
 				svgGen.setFileName( targetDir + name );
@@ -1434,6 +1439,9 @@ void GraphViewer::contextMenuEvent(QContextMenuEvent * event)
 				QPainter painter( &svgGen );
 
 				this->scene()->render(&painter);
+#else
+				UERROR("RTAB-MAp is not built with Qt's SVG library, cannot save picture in svg format.");
+#endif
 			}
 
 			//reset scale
@@ -1626,6 +1634,10 @@ void GraphViewer::contextMenuEvent(QContextMenuEvent * event)
 	else if(r == aShowHideGridMap)
 	{
 		this->setGridMapVisible(!this->isGridMapVisible());
+		if(_gridMap->isVisible())
+		{
+			emit mapShownRequested();
+		}
 	}
 	else if(r == aShowHideOrigin)
 	{

guilib/src/MainWindow.cpp

@@ -5008,9 +5008,11 @@ void MainWindow::postProcessing()
 	bool optimizeFromGraphEnd =  Parameters::defaultRGBDOptimizeFromGraphEnd();
 	float optimizeMaxError =  Parameters::defaultRGBDOptimizeMaxError();
 	int optimizeIterations =  Parameters::defaultOptimizerIterations();
+	bool reextractFeatures = Parameters::defaultRGBDLoopClosureReextractFeatures();
 	Parameters::parse(parameters, Parameters::kRGBDOptimizeFromGraphEnd(), optimizeFromGraphEnd);
 	Parameters::parse(parameters, Parameters::kRGBDOptimizeMaxError(), optimizeMaxError);
 	Parameters::parse(parameters, Parameters::kOptimizerIterations(), optimizeIterations);
+	Parameters::parse(parameters, Parameters::kRGBDLoopClosureReextractFeatures(), reextractFeatures);
 
 	bool warn = false;
 	int loopClosuresAdded = 0;
@@ -5089,6 +5091,40 @@ void MainWindow::postProcessing()
 									uInsert(parameters, ParametersPair(Parameters::kRegStrategy(), "2"));
 								}
 								Registration * registration = Registration::create(parameters);
+
+								if(reextractFeatures)
+								{
+									signatureFrom.sensorData().uncompressData();
+									signatureTo.sensorData().uncompressData();
+
+									if(signatureFrom.sensorData().imageRaw().empty() &&
+									   signatureTo.sensorData().imageRaw().empty())
+									{
+										UWARN("\"%s\" is false and signatures (%d and %d) don't have raw "
+												"images. Update the cache.",
+											Parameters::kRGBDLoopClosureReextractFeatures().c_str());
+									}
+									else
+									{
+										signatureFrom.setWords(std::multimap<int, cv::KeyPoint>());
+										signatureFrom.setWords3(std::multimap<int, cv::Point3f>());
+										signatureFrom.setWordsDescriptors(std::multimap<int, cv::Mat>());
+										signatureFrom.sensorData().setFeatures(std::vector<cv::KeyPoint>(), std::vector<cv::Point3f>(), cv::Mat());
+										signatureTo.setWords(std::multimap<int, cv::KeyPoint>());
+										signatureTo.setWords3(std::multimap<int, cv::Point3f>());
+										signatureTo.setWordsDescriptors(std::multimap<int, cv::Mat>());
+										signatureTo.sensorData().setFeatures(std::vector<cv::KeyPoint>(), std::vector<cv::Point3f>(), cv::Mat());
+									}
+								}
+								else if(!reextractFeatures && signatureFrom.getWords().empty() && signatureTo.getWords().empty())
+								{
+									UWARN("\"%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(),
+											signatureFrom.id(),
+											signatureTo.id(),
+											Parameters::kRGBDLoopClosureReextractFeatures().c_str());
+								}
 								transform = registration->computeTransformation(signatureFrom, signatureTo, Transform(), &info);
 								delete registration;
 								if(!transform.isNull())
@@ -5218,13 +5254,13 @@ void MainWindow::postProcessing()
 										_currentLinksMap.insert(std::make_pair(from, Link(from, to, Link::kUserClosure, transform, info.covariance.inv())));
 										++loopClosuresAdded;
 										_initProgressDialog->appendText(tr("Detected loop closure %1->%2! (%3/%4)").arg(from).arg(to).arg(i+1).arg(clusters.size()));
-										QApplication::processEvents();
 									}
 								}
 							}
 						}
 					}
 				}
+				QApplication::processEvents();
 				_initProgressDialog->incrementStep();
 			}
 			_initProgressDialog->appendText(tr("Iteration %1/%2: Detected %3 loop closures!").arg(n+1).arg(detectLoopClosureIterations).arg(addedLinks.size()/2));

guilib/src/PreferencesDialog.cpp

@@ -197,8 +197,8 @@ PreferencesDialog::PreferencesDialog(QWidget * parent) :
 #if CV_MAJOR_VERSION >= 3
 	_ui->groupBox_fast_opencv2->setEnabled(false);
 #else
-	_ui->comboBox_detector_strategy->setItemData(9, 0, Qt::UserRole - 1); // No FREAK (detector+descriptor version)
-	_ui->reextract_type->setItemData(9, 0, Qt::UserRole - 1); // No FREAK (detector+descriptor version)
+	_ui->comboBox_detector_strategy->setItemData(9, 0, Qt::UserRole - 1); // No KAZE
+	_ui->reextract_type->setItemData(9, 0, Qt::UserRole - 1); // No KAZE
 #endif
 
 	_ui->comboBox_cameraImages_odomFormat->setItemData(4, 0, Qt::UserRole - 1);
@@ -746,8 +746,16 @@ PreferencesDialog::PreferencesDialog(QWidget * parent) :
 	_ui->spinBox_BRISK_octaves->setObjectName(Parameters::kBRISKOctaves().c_str());
 	_ui->doubleSpinBox_BRISK_patterScale->setObjectName(Parameters::kBRISKPatternScale().c_str());
 
+	//KAZE
+	_ui->checkBox_kaze_extended->setObjectName(Parameters::kKAZEExtended().c_str());
+	_ui->checkBox_kaze_upright->setObjectName(Parameters::kKAZEUpright().c_str());
+	_ui->doubleSpinBox_kaze_threshold->setObjectName(Parameters::kKAZEThreshold().c_str());
+	_ui->spinBox_kaze_octaves->setObjectName(Parameters::kKAZENOctaves().c_str());
+	_ui->spinBox_kaze_octavelayers->setObjectName(Parameters::kKAZENOctaveLayers().c_str());
+	_ui->spinBox_kaze_diffusivity->setObjectName(Parameters::kKAZEDiffusivity().c_str());
+
 	// verifyHypotheses
-	_ui->comboBox_vh_strategy->setObjectName(Parameters::kRtabmapVhStrategy().c_str());
+	_ui->groupBox_vh_epipolar2->setObjectName(Parameters::kVhEpEnabled().c_str());
 	_ui->surf_spinBox_matchCountMinAccepted->setObjectName(Parameters::kVhEpMatchCountMin().c_str());
 	_ui->surf_doubleSpinBox_ransacParam1->setObjectName(Parameters::kVhEpRansacParam1().c_str());
 	_ui->surf_doubleSpinBox_ransacParam2->setObjectName(Parameters::kVhEpRansacParam2().c_str());
@@ -1603,6 +1611,7 @@ void PreferencesDialog::resetSettings(QGroupBox * groupBox)
 		if(groupBox->objectName() == _ui->groupBox_odometry1->objectName())
 		{
 			_ui->odom_registration->setCurrentIndex(3);
+			updateOdometryVisibility();
 		}
 	}
 }
@@ -2363,19 +2372,33 @@ bool PreferencesDialog::validateForm()
 #endif
 
 #if CV_MAJOR_VERSION < 3
-	if (_ui->comboBox_detector_strategy->currentIndex() == Feature2D::kFeatureFreak)
+	if (_ui->comboBox_detector_strategy->currentIndex() == Feature2D::kFeatureKaze)
 	{
+#ifdef RTABMAP_NONFREE
 		QMessageBox::warning(this, tr("Parameter warning"),
-			tr("Selected feature type (FREAK detector) is not available on OpenCV2. ORB is set instead "
+			tr("Selected feature type (KAZE) is not available on OpenCV2. SURF is set instead "
+				"for the bag-of-words dictionary."));
+		_ui->comboBox_detector_strategy->setCurrentIndex(Feature2D::kFeatureSurf);
+#else
+		QMessageBox::warning(this, tr("Parameter warning"),
+			tr("Selected feature type (KAZE) is not available on OpenCV2. ORB is set instead "
 				"for the bag-of-words dictionary."));
 		_ui->comboBox_detector_strategy->setCurrentIndex(Feature2D::kFeatureOrb);
+#endif
 	}
-	if (_ui->reextract_type->currentIndex() == Feature2D::kFeatureFreak)
+	if (_ui->reextract_type->currentIndex() == Feature2D::kFeatureKaze)
 	{
+#ifdef RTABMAP_NONFREE
 		QMessageBox::warning(this, tr("Parameter warning"),
-			tr("Selected feature type (FREAK detector) is not available on OpenCV2. ORB is set instead "
+			tr("Selected feature type (KAZE) is not available on OpenCV2. SURF is set instead "
+				"for the re-extraction of features on loop closure."));
+				_ui->reextract_type->setCurrentIndex(Feature2D::kFeatureSurf);
+#else
+		QMessageBox::warning(this, tr("Parameter warning"),
+			tr("Selected feature type (KAZE) is not available on OpenCV2. ORB is set instead "
 				"for the re-extraction of features on loop closure."));
 				_ui->reextract_type->setCurrentIndex(Feature2D::kFeatureOrb);
+#endif
 	}
 #endif
 

guilib/src/ui/preferencesDialog.ui

@@ -63,7 +63,7 @@
           <property name="geometry">
            <rect>
             <x>0</x>
-            <y>-637</y>
+            <y>0</y>
             <width>678</width>
             <height>2739</height>
            </rect>
@@ -95,7 +95,7 @@
                 <enum>QFrame::Raised</enum>
                </property>
                <property name="currentIndex">
-                <number>18</number>
+                <number>34</number>
                </property>
                <widget class="QWidget" name="page_22">
                 <layout class="QVBoxLayout" name="verticalLayout_29" stretch="0,1">
@@ -6732,7 +6732,7 @@ generate the number of words requested.</string>
                            </item>
                            <item>
                             <property name="text">
-                             <string>FREAK</string>
+                             <string>KAZE</string>
                             </property>
                            </item>
                           </widget>
@@ -7480,13 +7480,13 @@ see Sqlite3 doc 'PRAGMA temp_store'.</string>
                 </layout>
                </widget>
                <widget class="QWidget" name="page_23">
-                <layout class="QVBoxLayout" name="verticalLayout_2">
+                <layout class="QVBoxLayout" name="verticalLayout_2" stretch="0,1">
                  <item>
                   <widget class="QGroupBox" name="groupBox_bayes1">
                    <property name="title">
                     <string>Loop Closure Detection</string>
                    </property>
-                   <layout class="QVBoxLayout" name="verticalLayout_22">
+                   <layout class="QVBoxLayout" name="verticalLayout_22" stretch="0,0,0,1,0,0">
                     <item>
                      <widget class="QLabel" name="label">
                       <property name="text">
@@ -7630,139 +7630,96 @@ see Sqlite3 doc 'PRAGMA temp_store'.</string>
                      </layout>
                     </item>
                     <item>
-                     <widget class="QGroupBox" name="groupBox_vh_strategy2">
-                      <property name="minimumSize">
-                       <size>
-                        <width>0</width>
-                        <height>0</height>
-                       </size>
-                      </property>
+                     <widget class="QGroupBox" name="groupBox_vh_epipolar2">
                       <property name="title">
                        <string>Hypothesis Verification</string>
                       </property>
-                      <layout class="QVBoxLayout" name="verticalLayout_3">
-                       <item>
-                        <layout class="QGridLayout" name="gridLayout_10" columnstretch="0,1">
-                         <item row="0" column="0">
-                          <widget class="QComboBox" name="comboBox_vh_strategy">
-                           <property name="sizeAdjustPolicy">
-                            <enum>QComboBox::AdjustToContents</enum>
-                           </property>
-                           <item>
-                            <property name="text">
-                             <string>No verification</string>
-                            </property>
-                           </item>
-                           <item>
-                            <property name="text">
-                             <string>Epipolar constraints</string>
-                            </property>
-                           </item>
-                          </widget>
-                         </item>
-                         <item row="0" column="1">
-                          <widget class="QLabel" name="label_50">
-                           <property name="text">
-                            <string>Hypothesis verification.</string>
-                           </property>
-                           <property name="textInteractionFlags">
-                            <set>Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse</set>
-                           </property>
-                          </widget>
-                         </item>
-                        </layout>
+                      <property name="checkable">
+                       <bool>true</bool>
+                      </property>
+                      <layout class="QGridLayout" name="gridLayout" columnstretch="0,10">
+                       <item row="0" column="0">
+                        <widget class="QSpinBox" name="surf_spinBox_matchCountMinAccepted">
+                         <property name="minimum">
+                          <number>8</number>
+                         </property>
+                         <property name="maximum">
+                          <number>100000</number>
+                         </property>
+                         <property name="value">
+                          <number>11</number>
+                         </property>
+                        </widget>
                        </item>
-                       <item>
-                        <widget class="QGroupBox" name="groupBox_vh_epipolar2">
-                         <property name="title">
-                          <string>Epipolar Constraints</string>
+                       <item row="0" column="1">
+                        <widget class="QLabel" name="label_25">
+                         <property name="text">
+                          <string>Minimum match count to accept a loop closure.</string>
+                         </property>
+                         <property name="wordWrap">
+                          <bool>true</bool>
+                         </property>
+                         <property name="textInteractionFlags">
+                          <set>Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse</set>
+                         </property>
+                        </widget>
+                       </item>
+                       <item row="1" column="0">
+                        <widget class="QDoubleSpinBox" name="surf_doubleSpinBox_ransacParam1">
+                         <property name="decimals">
+                          <number>1</number>
+                         </property>
+                         <property name="maximum">
+                          <double>10.000000000000000</double>
+                         </property>
+                         <property name="singleStep">
+                          <double>0.100000000000000</double>
+                         </property>
+                         <property name="value">
+                          <double>3.000000000000000</double>
+                         </property>
+                        </widget>
+                       </item>
+                       <item row="1" column="1">
+                        <widget class="QLabel" name="label_27">
+                         <property name="text">
+                          <string>Fundamental Matrix : Distance (pixels) for inliers.</string>
+                         </property>
+                         <property name="wordWrap">
+                          <bool>true</bool>
+                         </property>
+                         <property name="textInteractionFlags">
+                          <set>Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse</set>
+                         </property>
+                        </widget>
+                       </item>
+                       <item row="2" column="0">
+                        <widget class="QDoubleSpinBox" name="surf_doubleSpinBox_ransacParam2">
+                         <property name="decimals">
+                          <number>2</number>
+                         </property>
+                         <property name="maximum">
+                          <double>0.990000000000000</double>
+                         </property>
+                         <property name="singleStep">
+                          <double>0.010000000000000</double>
+                         </property>
+                         <property name="value">
+                          <double>0.990000000000000</double>
+                         </property>
+                        </widget>
+                       </item>
+                       <item row="2" column="1">
+                        <widget class="QLabel" name="label_28">
+                         <property name="text">
+                          <string>Fundamental Matrix : Ransac performance.</string>
+                         </property>
+                         <property name="wordWrap">
+                          <bool>true</bool>
+                         </property>
+                         <property name="textInteractionFlags">
+                          <set>Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse</set>
                          </property>
-                         <layout class="QGridLayout" name="gridLayout" columnstretch="0,10">
-                          <item row="0" column="0">
-                           <widget class="QSpinBox" name="surf_spinBox_matchCountMinAccepted">
-                            <property name="minimum">
-                             <number>8</number>
-                            </property>
-                            <property name="maximum">
-                             <number>100000</number>
-                            </property>
-                            <property name="value">
-                             <number>11</number>
-                            </property>
-                           </widget>
-                          </item>
-                          <item row="0" column="1">
-                           <widget class="QLabel" name="label_25">
-                            <property name="text">
-                             <string>Minimum match count to accept a loop closure.</string>
-                            </property>
-                            <property name="wordWrap">
-                             <bool>true</bool>
-                            </property>
-                            <property name="textInteractionFlags">
-                             <set>Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse</set>
-                            </property>
-                           </widget>
-                          </item>
-                          <item row="1" column="0">
-                           <widget class="QDoubleSpinBox" name="surf_doubleSpinBox_ransacParam1">
-                            <property name="decimals">
-                             <number>1</number>
-                            </property>
-                            <property name="maximum">
-                             <double>10.000000000000000</double>
-                            </property>
-                            <property name="singleStep">
-                             <double>0.100000000000000</double>
-                            </property>
-                            <property name="value">
-                             <double>3.000000000000000</double>
-                            </property>
-                           </widget>
-                          </item>
-                          <item row="1" column="1">
-                           <widget class="QLabel" name="label_27">
-                            <property name="text">
-                             <string>Fundamental Matrix : Distance (pixels) for inliers.</string>
-                            </property>
-                            <property name="wordWrap">
-                             <bool>true</bool>
-                            </property>
-                            <property name="textInteractionFlags">
-                             <set>Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse</set>
-                            </property>
-                           </widget>
-                          </item>
-                          <item row="2" column="0">
-                           <widget class="QDoubleSpinBox" name="surf_doubleSpinBox_ransacParam2">
-                            <property name="decimals">
-                             <number>2</number>
-                            </property>
-                            <property name="maximum">
-                             <double>0.990000000000000</double>
-                            </property>
-                            <property name="singleStep">
-                             <double>0.010000000000000</double>
-                            </property>
-                            <property name="value">
-                             <double>0.990000000000000</double>
-                            </property>
-                           </widget>
-                          </item>
-                          <item row="2" column="1">
-                           <widget class="QLabel" name="label_28">
-                            <property name="text">
-                             <string>Fundamental Matrix : Ransac performance.</string>
-                            </property>
-                            <property name="wordWrap">
-                             <bool>true</bool>
-                            </property>
-                            <property name="textInteractionFlags">
-                             <set>Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse</set>
-                            </property>
-                           </widget>
-                          </item>
-                         </layout>
                         </widget>
                        </item>
                       </layout>
@@ -13288,7 +13245,7 @@ Lower the ratio -&gt; higher the precision. 0 means disabled, matching the neare
                            </item>
                            <item>
                             <property name="text">
-                             <string>FREAK</string>
+                             <string>KAZE</string>
                             </property>
                            </item>
                           </widget>
@@ -15906,6 +15863,179 @@ Lower the ratio -&gt; higher the precision. 0 means disabled, matching the neare
                  </item>
                 </layout>
                </widget>
+               <widget class="QWidget" name="page_71">
+                <layout class="QVBoxLayout" name="verticalLayout_3">
+                 <item>
+                  <widget class="QGroupBox" name="groupBox_detector_kaze2">
+                   <property name="title">
+                    <string>KAZE</string>
+                   </property>
+                   <layout class="QGridLayout" name="gridLayout_95" columnstretch="0,1">
+                    <item row="0" column="0">
+                     <widget class="QCheckBox" name="checkBox_kaze_extended">
+                      <property name="text">
+                       <string/>
+                      </property>
+                     </widget>
+                    </item>
+                    <item row="1" column="1">
+                     <widget class="QLabel" name="label_420">
+                      <property name="text">
+                       <string>Set to enable use of upright descriptors (non rotation-invariant).</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="1">
+                     <widget class="QLabel" name="label_416">
+                      <property name="text">
+                       <string>Maximum octave evolution of the image.</string>
+                      </property>
+                      <property name="wordWrap">
+                       <bool>true</bool>
+                      </property>
+                      <property name="textInteractionFlags">
+                       <set>Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse</set>
+                      </property>
+                     </widget>
+                    </item>
+                    <item row="1" column="0">
+                     <widget class="QCheckBox" name="checkBox_kaze_upright">
+                      <property name="text">
+                       <string/>
+                      </property>
+                     </widget>
+                    </item>
+                    <item row="4" column="1">
+                     <widget class="QLabel" name="label_417">
+                      <property name="text">
+                       <string>Default number of sublevels per scale level.</string>
+                      </property>
+                      <property name="wordWrap">
+                       <bool>true</bool>
+                      </property>
+                      <property name="textInteractionFlags">
+                       <set>Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse</set>
+                      </property>
+                     </widget>
+                    </item>
+                    <item row="4" column="0">
+                     <widget class="QSpinBox" name="spinBox_kaze_octavelayers">
+                      <property name="minimum">
+                       <number>1</number>
+                      </property>
+                      <property name="maximum">
+                       <number>16</number>
+                      </property>
+                      <property name="value">
+                       <number>3</number>
+                      </property>
+                     </widget>
+                    </item>
+                    <item row="3" column="0">
+                     <widget class="QSpinBox" name="spinBox_kaze_octaves">
+                      <property name="minimum">
+                       <number>1</number>
+                      </property>
+                      <property name="maximum">
+                       <number>16</number>
+                      </property>
+                      <property name="value">
+                       <number>16</number>
+                      </property>
+                     </widget>
+                    </item>
+                    <item row="0" column="1">
+                     <widget class="QLabel" name="label_419">
+                      <property name="text">
+                       <string>Set to enable extraction of extended (128-byte) descriptor.</string>
+                      </property>
+                      <property name="wordWrap">
+                       <bool>true</bool>
+                      </property>
+                      <property name="textInteractionFlags">
+                       <set>Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse</set>
+                      </property>
+                     </widget>
+                    </item>
+                    <item row="2" column="0">
+                     <widget class="QDoubleSpinBox" name="doubleSpinBox_kaze_threshold">
+                      <property name="decimals">
+                       <number>4</number>
+                      </property>
+                      <property name="minimum">
+                       <double>0.000100000000000</double>
+                      </property>
+                      <property name="maximum">
+                       <double>9999.000000000000000</double>
+                      </property>
+                      <property name="singleStep">
+                       <double>0.001000000000000</double>
+                      </property>
+                      <property name="value">
+                       <double>0.001000000000000</double>
+                      </property>
+                     </widget>
+                    </item>
+                    <item row="2" column="1">
+                     <widget class="QLabel" name="label_418">
+                      <property name="text">
+                       <string>Detector response threshold to accept point.</string>
+                      </property>
+                      <property name="wordWrap">
+                       <bool>true</bool>
+                      </property>
+                      <property name="textInteractionFlags">
+                       <set>Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse</set>
+                      </property>
+                     </widget>
+                    </item>
+                    <item row="5" column="1">
+                     <widget class="QLabel" name="label_421">
+                      <property name="text">
+                       <string>Diffusivity type: 0=DIFF_PM_G1, 1=DIFF_PM_G2, 2=DIFF_WEICKERT or 3=DIFF_CHARBONNIER.</string>
+                      </property>
+                      <property name="wordWrap">
+                       <bool>true</bool>
+                      </property>
+                      <property name="textInteractionFlags">
+                       <set>Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse</set>
+                      </property>
+                     </widget>
+                    </item>
+                    <item row="5" column="0">
+                     <widget class="QSpinBox" name="spinBox_kaze_diffusivity">
+                      <property name="maximum">
+                       <number>3</number>
+                      </property>
+                      <property name="value">
+                       <number>3</number>
+                      </property>
+                     </widget>
+                    </item>
+                   </layout>
+                  </widget>
+                 </item>
+                 <item>
+                  <spacer name="verticalSpacer_65">
+                   <property name="orientation">
+                    <enum>Qt::Vertical</enum>
+                   </property>
+                   <property name="sizeHint" stdset="0">
+                    <size>
+                     <width>20</width>
+                     <height>2487</height>
+                    </size>
+                   </property>
+                  </spacer>
+                 </item>
+                </layout>
+               </widget>
               </widget>
              </item>
             </layout>

package.xml

@@ -1,7 +1,7 @@
 <?xml version="1.0"?>
 <package>
   <name>rtabmap</name>
-  <version>0.13.0</version>
+  <version>0.13.2</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>

utilite/CMakeLists.txt

@@ -6,4 +6,7 @@ if(UNIX AND NOT ANDROID)
 ENDIF(UNIX AND NOT ANDROID)
 
 ADD_SUBDIRECTORY( src )
-ADD_SUBDIRECTORY( resource_generator )
+
+IF(NOT ANDROID)
+   ADD_SUBDIRECTORY( resource_generator )
+ENDIF(NOT ANDROID)