Increased ROS package version to 0.11.7

CMakeLists.txt

@@ -20,7 +20,7 @@ SET(CMAKE_MODULE_PATH "${PROJECT_SOURCE_DIR}/cmake_modules")
 #######################
 SET(RTABMAP_MAJOR_VERSION 0)
 SET(RTABMAP_MINOR_VERSION 11)
-SET(RTABMAP_PATCH_VERSION 4)
+SET(RTABMAP_PATCH_VERSION 7)
 SET(RTABMAP_VERSION
   ${RTABMAP_MAJOR_VERSION}.${RTABMAP_MINOR_VERSION}.${RTABMAP_PATCH_VERSION})
   
@@ -136,6 +136,7 @@ option(WITH_TORO          "Include TORO support"                 ON)
 option(WITH_VERTIGO       "Include Vertigo support"              ON)
 option(WITH_CVSBA         "Include cvsba support"                ON)
 option(WITH_FLYCAPTURE2   "Include FlyCapture2/Triclops support" ON)
+option(WITH_ZED           "Include ZED sdk support"              ON)
 
 FIND_PACKAGE(OpenCV REQUIRED QUIET)
 FIND_PACKAGE(PCL 1.7 REQUIRED QUIET)
@@ -170,15 +171,22 @@ IF(ZLIB_FOUND)
 ENDIF(ZLIB_FOUND)
 
 IF(WITH_QT)
+    FIND_PACKAGE(VTK)
+    IF(NOT VTK_FOUND)
+        MESSAGE(FATAL_ERROR "VTK is required when using Qt. Set -DWITH_QT=OFF if you don't want gui tools.")
+    ENDIF(NOT VTK_FOUND)
+
     # If Qt is here, the GUI will be built
-    # look for Qt5 before Qt4
-    FIND_PACKAGE(Qt5 COMPONENTS Widgets Core Gui Svg QUIET)
+    # look for Qt5 (if vtk>5 is installed) before Qt4
+    IF("${VTK_MAJOR_VERSION}" GREATER 5)
+        FIND_PACKAGE(Qt5 COMPONENTS Widgets Core Gui Svg QUIET)
+    ENDIF("${VTK_MAJOR_VERSION}" GREATER 5)
+
     IF(NOT Qt5_FOUND)
         FIND_PACKAGE(Qt4 COMPONENTS QtCore QtGui QtSvg)
     ENDIF(NOT Qt5_FOUND)
         
     IF(QT4_FOUND OR Qt5_FOUND)
-        FIND_PACKAGE(VTK REQUIRED)
         IF("${VTK_MAJOR_VERSION}" EQUAL 5)
             FIND_PACKAGE(QVTK REQUIRED) # only for VTK 5
         ENDIF("${VTK_MAJOR_VERSION}" EQUAL 5)
@@ -228,7 +236,50 @@ IF(WITH_GTSAM)
     FIND_PACKAGE(GTSAM QUIET)
 ENDIF(WITH_GTSAM)
 
-IF(G2O_FOUND OR GTSAM_FOUND)
+IF(WITH_FLYCAPTURE2)
+    FIND_PACKAGE(FlyCapture2 QUIET)
+    IF(FlyCapture2_FOUND)
+       MESSAGE(STATUS "Found FlyCapture2: ${FlyCapture2_INCLUDE_DIRS}")
+    ENDIF(FlyCapture2_FOUND)
+ENDIF(WITH_FLYCAPTURE2)
+
+IF(WITH_CVSBA)
+	FIND_PACKAGE(cvsba QUIET)
+    IF(cvsba_FOUND)
+       MESSAGE(STATUS "Found cvsba: ${cvsba_INCLUDE_DIRS}")
+    ENDIF(cvsba_FOUND)
+ENDIF(WITH_CVSBA)
+
+IF(WITH_ZED)
+	IF(WIN32) # Windows
+		SET(ZED_INCLUDE_DIRS $ENV{ZED_INCLUDE_DIRS})
+		if (CMAKE_CL_64) # 64 bits
+			SET(ZED_LIBRARIES $ENV{ZED_LIBRARIES_64})
+		else(CMAKE_CL_64) # 32 bits
+			message("32bits compilation is no more available with CUDA7.0")
+		endif(CMAKE_CL_64)
+		SET(ZED_LIBRARY_DIR $ENV{ZED_LIBRARY_DIR})
+		IF(ZED_LIBRARIES AND ZED_INCLUDE_DIRS)
+			SET(ZED_FOUND TRUE)
+			LINK_DIRECTORIES( ${LINK_DIRECTORIES} ${ZED_LIBRARY_DIR})
+		ENDIF(ZED_LIBRARIES AND ZED_INCLUDE_DIRS)
+	ELSE() # Linux
+		find_package(ZED 0.9 QUIET)
+	ENDIF(WIN32)
+	
+	IF(ZED_FOUND)
+       MESSAGE(STATUS "Found ZED sdk: ${ZED_INCLUDE_DIRS}")
+	   ## look for CUDA
+	   find_package(CUDA)
+	   IF(CUDA_FOUND)
+		MESSAGE(STATUS "Found CUDA: ${CUDA_INCLUDE_DIRS}")
+	   ELSE()
+	    MESSAGE(FATAL_ERROR "CUDA is required to build with Zed sdk! Set -DWITH_ZED=OFF if you don't have CUDA.")
+	   ENDIF()
+    ENDIF(ZED_FOUND)
+ENDIF(WITH_ZED)
+
+IF(G2O_FOUND OR GTSAM_FOUND OR ZED_FOUND)
    #Newest versions require std11
    IF(NOT MSVC)
      include(CheckCXXCompilerFlag)
@@ -242,21 +293,7 @@ IF(G2O_FOUND OR GTSAM_FOUND)
 		message(STATUS "The compiler ${CMAKE_CXX_COMPILER} has no C++11 support. Please use a different C++ compiler if you want to use g2o or gtsam (set \"-DWITH_G2O=OFF -DWITH_GTSAM=OFF\" to build without g2o and gtsam).")
 	  ENDIF()
    ENDIF()
-ENDIF(G2O_FOUND OR GTSAM_FOUND)
-
-IF(WITH_FLYCAPTURE2)
-    FIND_PACKAGE(FlyCapture2 QUIET)
-    IF(FlyCapture2_FOUND)
-       MESSAGE(STATUS "Found FlyCapture2: ${FlyCapture2_INCLUDE_DIRS}")
-    ENDIF(FlyCapture2_FOUND)
-ENDIF(WITH_FLYCAPTURE2)
-
-IF(WITH_CVSBA)
-    FIND_PACKAGE(cvsba QUIET)
-    IF(cvsba_FOUND)
-       MESSAGE(STATUS "Found cvsba: ${cvsba_INCLUDE_DIRS}")
-    ENDIF(cvsba_FOUND)
-ENDIF(WITH_CVSBA)
+ENDIF(G2O_FOUND OR GTSAM_FOUND OR ZED_FOUND)
 
 ####### OSX BUNDLE CMAKE_INSTALL_PREFIX #######
 IF(APPLE AND BUILD_AS_BUNDLE)
@@ -287,15 +324,24 @@ ENDIF(APPLE AND BUILD_AS_BUNDLE)
 
 
 ####### SOURCES (Projects) #######
+
+# CONF_DEPENDENCIES contains only dependencies not required by the headers
+SET(CONF_DEPENDENCIES 
+	${ZLIB_LIBRARIES}
+)
 IF(NOT (OPENCV_NONFREE_FOUND OR OPENCV_XFEATURES2D_FOUND))
    SET(NONFREE "//")
 ENDIF(NOT (OPENCV_NONFREE_FOUND OR OPENCV_XFEATURES2D_FOUND))
 IF(NOT G2O_FOUND)
    SET(G2O "//")
-ENDIF(NOT G2O_FOUND)
+ELSE()
+   SET(CONF_DEPENDENCIES ${CONF_DEPENDENCIES} ${G2O_LIBRARIES})
+ENDIF()
 IF(NOT GTSAM_FOUND)
    SET(GTSAM "//")
-ENDIF(NOT GTSAM_FOUND)
+ELSE()
+   SET(CONF_DEPENDENCIES ${CONF_DEPENDENCIES} ${GTSAM_LIBRARIES})
+ENDIF()
 IF(NOT WITH_TORO)
    SET(TORO "//")
 ENDIF(NOT WITH_TORO)
@@ -304,22 +350,39 @@ IF(NOT WITH_VERTIGO)
 ENDIF(NOT WITH_VERTIGO)
 IF(NOT cvsba_FOUND)
    SET(CVSBA "//")
-ENDIF(NOT cvsba_FOUND)
+ELSE()
+   SET(CONF_DEPENDENCIES ${CONF_DEPENDENCIES} ${cvsba_LIBRARIES})
+ENDIF()
 IF(NOT Freenect_FOUND)
    SET(FREENECT "//")
-ENDIF(NOT Freenect_FOUND)
+ELSE()
+   SET(CONF_DEPENDENCIES ${CONF_DEPENDENCIES} ${Freenect_LIBRARIES})
+ENDIF()
 IF(NOT freenect2_FOUND)
    SET(FREENECT2 "//")
-ENDIF(NOT freenect2_FOUND)
+ELSE()
+   SET(CONF_DEPENDENCIES ${CONF_DEPENDENCIES} ${freenect2_LIBRARIES})
+ENDIF()
 IF(NOT OpenNI2_FOUND)
    SET(OPENNI2 "//")
-ENDIF(NOT OpenNI2_FOUND)
+ELSE()
+   SET(CONF_DEPENDENCIES ${CONF_DEPENDENCIES} ${OpenNI2_LIBRARIES})
+ENDIF()
 IF(NOT DC1394_FOUND)
    SET(DC1394 "//")
-ENDIF(NOT DC1394_FOUND)
+ELSE()
+   SET(CONF_DEPENDENCIES ${CONF_DEPENDENCIES} ${DC1394_LIBRARIES})
+ENDIF()
 IF(NOT FlyCapture2_FOUND)
    SET(FLYCAPTURE2 "//")
-ENDIF(NOT FlyCapture2_FOUND)
+ELSE()
+   SET(CONF_DEPENDENCIES ${CONF_DEPENDENCIES} ${FlyCapture2_LIBRARIES})
+ENDIF()
+IF(NOT ZED_FOUND)
+   SET(ZED "//")
+ELSE()
+   SET(CONF_DEPENDENCIES ${CONF_DEPENDENCIES} ${ZED_LIBRARIES} ${CUDA_LIBRARIES}) 
+ENDIF()
 IF(NOT (OpenCV_FOUND AND OpenCV_VERSION_MAJOR EQUAL 3))
    SET(OPENCV3 "//")
 ENDIF(NOT (OpenCV_FOUND AND OpenCV_VERSION_MAJOR EQUAL 3))
@@ -373,6 +436,11 @@ set(CONF_INCLUDE_DIRS "${PROJECT_SOURCE_DIR}/corelib/include"
 set(CONF_LIB_DIR "${CMAKE_ARCHIVE_OUTPUT_DIRECTORY} ${CMAKE_RUNTIME_OUTPUT_DIRECTORY}")
 IF(QT4_FOUND OR Qt5_FOUND)
    set(CONF_WITH_GUI ON)
+   IF(QT4_FOUND)
+      set(CONF_QT_VERSION 4)
+   ELSE()
+      set(CONF_QT_VERSION 5)
+   ENDIF()
 ELSE()
    set(CONF_WITH_GUI OFF)
 ENDIF()
@@ -589,6 +657,18 @@ ELSE()
 MESSAGE(STATUS "  With cvsba                = NO (cvsba not found)")
 ENDIF()
 
+IF(ZED_FOUND)
+IF(CUDA_FOUND)
+MESSAGE(STATUS "  With ZED                  = YES (With CUDA)")
+ELSE()
+MESSAGE(STATUS "  With ZED                  = YES (Without CUDA)")
+ENDIF()
+ELSEIF(NOT WITH_ZED)
+MESSAGE(STATUS "  With ZED                  = NO (WITH_ZED=OFF)")
+ELSE()
+MESSAGE(STATUS "  With ZED                  = NO (ZED sdk not found)")
+ENDIF()
+
 IF(QT4_FOUND)
 MESSAGE(STATUS "  With Qt4                  = YES (License: Open Source or Commercial)")
 ELSEIF(Qt5_FOUND)

RTABMapConfig.cmake.in

@@ -61,9 +61,13 @@ if(@CONF_WITH_GUI@)
    set(RTABMap_LIBRARIES ${RTABMap_LIBRARIES} ${RTABMap_GUI})
 endif(@CONF_WITH_GUI@)
 
+# Dependencies
+set(RTABMap_LIBRARIES ${RTABMap_LIBRARIES} @CONF_DEPENDENCIES@)
+
 #backward compatibilities
 set(RTABMAP_CORE ${RTABMap_CORE})
 set(RTABMAP_UTILITE ${RTABMap_UTILITE})
 if(RTABMap_GUI)
    set(RTABMAP_GUI ${RTABMap_GUI})
-endif(RTABMap_GUI)
+   set(RTABMAP_QT_VERSION @CONF_QT_VERSION@)
+endif(RTABMap_GUI)

Version.h.in

@@ -49,6 +49,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 @CVSBA@#define RTABMAP_CVSBA
 @DC1394@#define RTABMAP_DC1394
 @FLYCAPTURE2@#define RTABMAP_FLYCAPTURE2
+@ZED@#define RTABMAP_ZED
 
 #endif /* VERSION_H_ */
 

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="8"
+        android:versionCode="9"
         android:versionName="@RTABMAP_VERSION@">
 
     <uses-permission android:name="android.permission.CAMERA" />
@@ -11,7 +11,7 @@
     <uses-permission android:name="android.permission.READ_FRAME_BUFFER" />
     <uses-permission android:name="android.permission.ACCESS_SURFACE_FLINGER" />
     <uses-feature android:glEsVersion="0x00020000" />
-    <uses-library android:name="com.projecttango.libtango_device" android:required="true" />
+    <uses-library android:name="com.projecttango.libtango_device2" android:required="true" />
 
     <!-- This is the platform API where NativeActivity was introduced. -->
     <uses-sdk android:minSdkVersion="17" />

app/android/jni/RTABMapApp.cpp

@@ -56,6 +56,10 @@ const int kVersionStringLength = 128;
 static JavaVM *jvm;
 static jobject RTABMapActivity = 0;
 
+namespace {
+constexpr int kTangoCoreMinimumVersion = 9377;
+}  // anonymous namespace.
+
 rtabmap::ParametersMap RTABMapApp::getRtabmapParameters()
 {
 	rtabmap::ParametersMap parameters;
@@ -101,7 +105,9 @@ RTABMapApp::RTABMapApp() :
 		clearSceneOnNextRender_(false),
 		totalPoints_(0),
 		totalPolygons_(0),
-		lastDrawnCloudsCount_(0)
+		lastDrawnCloudsCount_(0),
+		renderingFPS_(0.0f)
+
 {
 
 }
@@ -122,19 +128,19 @@ RTABMapApp::~RTABMapApp() {
   }
 }
 
-int RTABMapApp::TangoInitialize(JNIEnv* env, jobject caller_activity)
+void RTABMapApp::onCreate(JNIEnv* env, jobject caller_activity)
 {
-
 	env->GetJavaVM(&jvm);
 	RTABMapActivity = env->NewGlobalRef(caller_activity);
 
-	LOGI("RTABMapApp::TangoInitialize()");
+	LOGI("RTABMapApp::onCreate()");
 	createdMeshes_.clear();
 	rawPoses_.clear();
 	clearSceneOnNextRender_ = true;
 	totalPoints_ = 0;
 	totalPolygons_ = 0;
 	lastDrawnCloudsCount_ = 0;
+	renderingFPS_ = 0.0f;
 
 	if(camera_)
 	{
@@ -158,12 +164,6 @@ int RTABMapApp::TangoInitialize(JNIEnv* env, jobject caller_activity)
 	this->registerToEventsManager();
 
 	camera_ = new rtabmap::CameraTango(fullResolution_?1:2, autoExposure_);
-
-
-  // The first thing we need to do for any Tango enabled application is to
-  // initialize the service. We'll do that here, passing on the JNI environment
-  // and jobject corresponding to the Android activity that is calling us.
-  return TangoService_initialize(env, caller_activity);
 }
 
 void RTABMapApp::openDatabase(const std::string & databasePath)
@@ -218,22 +218,27 @@ void RTABMapApp::openDatabase(const std::string & databasePath)
 	rtabmapMutex_.unlock();
 }
 
-int RTABMapApp::onResume()
+bool RTABMapApp::onTangoServiceConnected(JNIEnv* env, jobject iBinder)
 {
-	LOGW("onResume()");
+	LOGW("onTangoServiceConnected()");
 	if(camera_)
 	{
 		camera_->join(true);
 
+		if (TangoService_setBinder(env, iBinder) != TANGO_SUCCESS) {
+		    LOGE("TangoHandler::ConnectTango, TangoService_setBinder error");
+		    return false;
+		}
+
 		if(camera_->init())
 		{
 			LOGI("Start camera thread");
 			camera_->start();
-			return TANGO_SUCCESS;
+			return true;
 		}
 		LOGE("Failed camera initialization!");
 	}
-	return TANGO_ERROR;
+	return false;
 }
 
 void RTABMapApp::onPause()
@@ -307,6 +312,7 @@ int RTABMapApp::Render()
 		totalPoints_ = 0;
 		totalPolygons_ = 0;
 		lastDrawnCloudsCount_ = 0;
+		renderingFPS_ = 0.0f;
 	}
 
 	// Process events
@@ -543,7 +549,9 @@ int RTABMapApp::Render()
 		}
 	}
 
+	UTimer fpsTime;
     lastDrawnCloudsCount_ = main_scene_.Render();
+    renderingFPS_ = 1.0/fpsTime.elapsed();
 
     if(rtabmapEvents.size())
     {
@@ -647,10 +655,7 @@ void RTABMapApp::setAutoExposure(bool enabled)
 		autoExposure_ = enabled;
 		if(camera_)
 		{
-			camera_->join(true);
-			camera_->close();
 			camera_->setAutoExposure(autoExposure_);
-			onResume();
 		}
 	}
 }
@@ -1104,7 +1109,7 @@ void RTABMapApp::handleEvent(UEvent * event)
 				jclass clazz = env->GetObjectClass(RTABMapActivity);
 				if(clazz)
 				{
-					jmethodID methodID = env->GetMethodID(clazz, "updateStatsCallback", "(IIIIFIIIIIFII)V" );
+					jmethodID methodID = env->GetMethodID(clazz, "updateStatsCallback", "(IIIIFIIIIIFIFI)V" );
 					if(methodID)
 					{
 						env->CallVoidMethod(RTABMapActivity, methodID,
@@ -1120,6 +1125,7 @@ void RTABMapApp::handleEvent(UEvent * event)
 								featuresExtracted,
 								hypothesis,
 								lastDrawnCloudsCount_,
+								renderingFPS_,
 								rejected);
 						success = true;
 					}

app/android/jni/RTABMapApp.h

@@ -50,14 +50,11 @@ class RTABMapApp : public UEventsHandler {
   RTABMapApp();
   ~RTABMapApp();
 
-  // Initialize the Tango Service, this function starts the communication
-  // between the application and the Tango Service.
-  // The activity object is used for checking if the API version is outdated.
-  int TangoInitialize(JNIEnv* env, jobject caller_activity);
+  void onCreate(JNIEnv* env, jobject caller_activity);
 
   void openDatabase(const std::string & databasePath);
 
-  int onResume();
+  bool onTangoServiceConnected(JNIEnv* env, jobject iBinder);
 
   // Explicitly reset motion tracking and restart the pipeline.
   // Note that this will cause motion tracking to re-initialize.
@@ -163,6 +160,7 @@ class RTABMapApp : public UEventsHandler {
   int totalPoints_;
   int totalPolygons_;
   int lastDrawnCloudsCount_;
+  float renderingFPS_;
 
   // main_scene_ includes all drawable object for visualizing Tango device's
   // movement and point cloud.

app/android/jni/jni_interface.cpp

@@ -48,11 +48,11 @@ void GetJStringContent(JNIEnv *AEnv, jstring AStr, std::string &ARes) {
   AEnv->ReleaseStringUTFChars(AStr,s);
 }
 
-JNIEXPORT jint JNICALL
-Java_com_introlab_rtabmap_RTABMapLib_initialize(
+JNIEXPORT void JNICALL
+Java_com_introlab_rtabmap_RTABMapLib_onCreate(
     JNIEnv* env, jobject, jobject activity)
 {
-	return app.TangoInitialize(env, activity);
+	return app.onCreate(env, activity);
 }
 
 JNIEXPORT void JNICALL
@@ -64,10 +64,10 @@ Java_com_introlab_rtabmap_RTABMapLib_openDatabase(
 	return app.openDatabase(databasePathC);
 }
 
-JNIEXPORT jint JNICALL
-Java_com_introlab_rtabmap_RTABMapLib_onResume(
-    JNIEnv*, jobject) {
-  return app.onResume();
+JNIEXPORT bool JNICALL
+Java_com_introlab_rtabmap_RTABMapLib_onTangoServiceConnected(
+		JNIEnv* env, jobject, jobject iBinder) {
+  return app.onTangoServiceConnected(env, iBinder);
 }
 
 JNIEXPORT void JNICALL

app/android/res/layout/activity_rtabmap.xml

@@ -219,6 +219,21 @@
                 android:layout_width="wrap_content"
                 android:layout_height="wrap_content" />
         </LinearLayout>
+        <LinearLayout
+            android:layout_width="wrap_content"
+            android:layout_height="wrap_content"
+            android:orientation="horizontal" >
+
+            <TextView
+                android:layout_width="wrap_content"
+                android:layout_height="wrap_content"
+                android:text="@string/fps" />
+
+            <TextView
+                android:id="@+id/fps"
+                android:layout_width="wrap_content"
+                android:layout_height="wrap_content" />
+        </LinearLayout>
            
     </LinearLayout>
 

app/android/res/values/strings.xml

@@ -20,5 +20,6 @@
     <string name="polygons">"Polygons: "</string>
     <string name="memory">"Memory (MB): "</string>
     <string name="hypothesis">"Hypothesis: "</string>
+    <string name="fps">"FPS (rendering): "</string>
     
 </resources>

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

@@ -9,8 +9,10 @@ import android.app.Notification;
 import android.app.NotificationManager;
 import android.app.PendingIntent;
 import android.app.ProgressDialog;
+import android.content.ComponentName;
 import android.content.DialogInterface;
 import android.content.Intent;
+import android.content.ServiceConnection;
 import android.content.pm.PackageInfo;
 import android.content.pm.PackageManager;
 import android.content.pm.PackageManager.NameNotFoundException;
@@ -20,6 +22,7 @@ import android.os.Bundle;
 import android.os.Environment;
 import android.os.Handler;
 import android.os.Debug;
+import android.os.IBinder;
 import android.text.Editable;
 import android.text.InputType;
 import android.util.Log;
@@ -99,6 +102,24 @@ public class RTABMapActivity extends Activity implements OnClickListener {
   
   private Toast mToast = null;
   
+  //Tango Service connection.
+  ServiceConnection mTangoServiceConnection = new ServiceConnection() {
+     public void onServiceConnected(ComponentName name, IBinder service) {
+       if(!RTABMapLib.onTangoServiceConnected(service))
+       {
+    	   mToast.makeText(getApplicationContext(), 
+    				String.format("Failed to intialize Tango!"), mToast.LENGTH_SHORT).show();
+       }
+     }
+
+     public void onServiceDisconnected(ComponentName name) {
+       // Handle this if you need to gracefully shutdown/retry
+       // in the event that Tango itself crashes/gets upgraded while running.
+       mToast.makeText(getApplicationContext(), 
+ 				String.format("Tango disconnected!"), mToast.LENGTH_SHORT).show();
+     }
+   };
+  
   @Override
   protected void onCreate(Bundle savedInstanceState) {
     super.onCreate(savedInstanceState);
@@ -172,7 +193,7 @@ public class RTABMapActivity extends Activity implements OnClickListener {
 						Environment.getExternalStorageState()), mToast.LENGTH_LONG).show();
     }
     
-    RTABMapLib.initialize(this);
+    RTABMapLib.onCreate(this);
     RTABMapLib.openDatabase(mTempDatabasePath);
   }
   
@@ -193,6 +214,8 @@ public class RTABMapActivity extends Activity implements OnClickListener {
   protected void onResume() {
     super.onResume();
     
+    TangoInitializationHelper.bindTangoService(this, mTangoServiceConnection);
+    
     Log.i(TAG, String.format("onResume()"));
     
     if (Tango.hasPermission(this, Tango.PERMISSIONTYPE_MOTION_TRACKING)) {
@@ -209,12 +232,6 @@ public class RTABMapActivity extends Activity implements OnClickListener {
     		mItemPostProcessing.setEnabled(false);
         }
 
-        if(RTABMapLib.onResume()!=0)
-        {
-        	mToast.makeText(getApplicationContext(), 
-    				String.format("Failed to connect with Tango!"), mToast.LENGTH_SHORT).show();
-        }
-
     } else {
     	Log.i(TAG, String.format("Asking for motion tracking permission"));
         startActivityForResult(
@@ -232,6 +249,8 @@ public class RTABMapActivity extends Activity implements OnClickListener {
     RTABMapLib.onPause();
     mOpenedDatabasePath = "";
     RTABMapLib.openDatabase(mTempDatabasePath);
+    
+    unbindService(mTangoServiceConnection);
   }
 
   @Override
@@ -318,6 +337,7 @@ public class RTABMapActivity extends Activity implements OnClickListener {
 		  int featuresExtracted,
 		  float hypothesis,
 		  int nodesDrawn,
+		  float fps,
 		  int rejected)
   {
 	  if(mItemPause!=null)
@@ -335,6 +355,7 @@ public class RTABMapActivity extends Activity implements OnClickListener {
 	  ((TextView)findViewById(R.id.features)).setText(String.format("%d / %s", featuresExtracted, mMaxFeaturesValues[mParamMaxFeaturesIndex]));
 	  ((TextView)findViewById(R.id.update_time)).setText(String.format("%.3f / %s", updateTime, mTimeThrValues[mParamTimeThrMsIndex]));
 	  ((TextView)findViewById(R.id.hypothesis)).setText(String.format("%.3f / %s (%d)", hypothesis, mLoopThrValues[mParamLoopThrMsIndex], loopClosureId>0?loopClosureId:highestHypId));
+	  ((TextView)findViewById(R.id.fps)).setText(String.format("%.3f Hz", fps));
 	  if(mItemPause!=null && !mItemPause.isChecked())
 	  {
 		  if(loopClosureId > 0)
@@ -367,13 +388,14 @@ public class RTABMapActivity extends Activity implements OnClickListener {
 		  final int features,
 		  final float hypothesis,
 		  final int nodesDrawn,
+		  final float fps,
 		  final int rejected)
   {
 	  Log.i(TAG, String.format("updateStatsCallback()"));
 
 	  runOnUiThread(new Runnable() {
 		    public void run() {
-		    	updateStatsUI(nodes, words, points, polygons, updateTime, loopClosureId, highestHypId, databaseMemoryUsed, inliers, features, hypothesis, nodesDrawn, rejected);
+		    	updateStatsUI(nodes, words, points, polygons, updateTime, loopClosureId, highestHypId, databaseMemoryUsed, inliers, features, hypothesis, nodesDrawn, fps, rejected);
 		    } 
 		});
   }
@@ -469,6 +491,7 @@ public class RTABMapActivity extends Activity implements OnClickListener {
 			((TextView)findViewById(R.id.features)).setText(String.valueOf(0));
 			((TextView)findViewById(R.id.update_time)).setText(String.valueOf(0));
 			((TextView)findViewById(R.id.hypothesis)).setText(String.valueOf(0));
+			((TextView)findViewById(R.id.fps)).setText(String.valueOf(0));
 			mTotalLoopClosures = 0;
 			((TextView)findViewById(R.id.total_loop)).setText(String.valueOf(mTotalLoopClosures));
 			
@@ -764,6 +787,10 @@ public class RTABMapActivity extends Activity implements OnClickListener {
       {
     	  item.setChecked(!item.isChecked());
     	  RTABMapLib.setAutoExposure(item.isChecked());
+    	  
+    	  // restart Tango service
+    	  onPause();
+    	  onResume();    	  
       }
       else if(itemId == R.id.resolution)
       {
@@ -1024,6 +1051,7 @@ public class RTABMapActivity extends Activity implements OnClickListener {
     	  ((TextView)findViewById(R.id.features)).setText(String.valueOf(0));
     	  ((TextView)findViewById(R.id.update_time)).setText(String.valueOf(0));
     	  ((TextView)findViewById(R.id.hypothesis)).setText(String.valueOf(0));
+    	  ((TextView)findViewById(R.id.fps)).setText(String.valueOf(0));
 		  mTotalLoopClosures = 0;
 		  ((TextView)findViewById(R.id.total_loop)).setText(String.valueOf(mTotalLoopClosures));
     	  

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

@@ -1,6 +1,8 @@
 
 package com.introlab.rtabmap;
+import android.os.IBinder;
 import android.view.KeyEvent;
+import android.util.Log;
 
 
 // Wrapper for native library
@@ -8,19 +10,29 @@ import android.view.KeyEvent;
 public class RTABMapLib
 {
 
-    static
-    {
-      System.loadLibrary("NativeRTABMap");
+	static {
+        // This project depends on tango_client_api, so we need to make sure we load
+        // the correct library first.
+        if (TangoInitializationHelper.loadTangoSharedLibrary() ==
+                TangoInitializationHelper.ARCH_ERROR) {
+            Log.e(RTABMapActivity.class.getSimpleName(), "ERROR! Unable to load libtango_client_api.so!");
+        }
+        System.loadLibrary("NativeRTABMap");
     }
 
  // Initialize the Tango Service, this function starts the communication
     // between the application and Tango Service.
     // The activity object is used for checking if the API version is outdated.
-    public static native int initialize(RTABMapActivity activity);
+    public static native void onCreate(RTABMapActivity activity);
     
     public static native void openDatabase(String databasePath);
     
-    public static native int onResume();
+    /*
+     * Called when the Tango service is connected.
+     *
+     * @param binder The native binder object.
+     */
+    public static native boolean onTangoServiceConnected(IBinder binder);
 
     // Release all non OpenGl resources that are allocated from the program.
     public static native void onPause();

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

@@ -0,0 +1,136 @@
+/*
+ * Copyright 2016 Google Inc. All Rights Reserved.
+ *
+ * Licensed under the Apache License, Version 2.0 (the "License");
+ * you may not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ *      http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS,
+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ * 
+ * Copied for convenience from https://github.com/googlesamples/tango-examples-c/blob/master/cpp_example_util/app/src/main/java/com/projecttango/examples/cpp/util/TangoInitializationHelper.java
+ */
+
+package com.introlab.rtabmap;
+
+import android.content.Context;
+import android.content.Intent;
+import android.content.ServiceConnection;
+import android.os.Build;
+import android.os.IBinder;
+import android.util.Log;
+
+import java.io.File;
+
+/**
+ * Functions for simplifying the process of initializing TangoService, and function
+ * handles loading correct libtango_client_api.so.
+ */
+public class TangoInitializationHelper {
+  public static final int ARCH_ERROR = -2;
+  public static final int ARCH_FALLBACK = -1;
+  public static final int ARCH_DEFAULT = 0;
+  public static final int ARCH_ARM64 = 1;
+  public static final int ARCH_ARM32 = 2;
+  public static final int ARCH_X86_64 = 3;
+  public static final int ARCH_X86 = 4;
+
+  /**
+   * Only for apps using the C API:
+   * Initializes the underlying TangoService for native apps.
+   *
+   * @return returns false if the device doesn't have the Tango running as Android Service.
+   *     Otherwise ture.
+   */
+  public static final boolean bindTangoService(final Context context,
+                                               ServiceConnection connection) {
+    Intent intent = new Intent();
+    intent.setClassName("com.google.tango", "com.google.atap.tango.TangoService");
+
+    boolean hasJavaService = (context.getPackageManager().resolveService(intent, 0) != null);
+
+    // User doesn't have the latest packagename for TangoCore, fallback to the previous name.
+    if (!hasJavaService) {
+      intent = new Intent();
+      intent.setClassName("com.projecttango.tango", "com.google.atap.tango.TangoService");
+      hasJavaService = (context.getPackageManager().resolveService(intent, 0) != null);
+    }
+
+    // User doesn't have a Java-fied TangoCore at all; fallback to the deprecated approach
+    // of doing nothing and letting the native side auto-init to the system-service version
+    // of Tango.
+    if (!hasJavaService) {
+      return false;
+    }
+
+    return context.bindService(intent, connection, Context.BIND_AUTO_CREATE);
+  }
+
+    
+  /**
+   * Load the libtango_client_api.so library based on different Tango device setup.
+   *
+   * @return returns the loaded architecture id.
+   */
+  public static final int loadTangoSharedLibrary() {
+    int loadedSoId = ARCH_ERROR;
+    String basePath = "/data/data/com.google.tango/libfiles/";
+    if (!(new File(basePath).exists())) {
+      basePath = "/data/data/com.projecttango.tango/libfiles/";
+    }
+    Log.i("TangoInitializationHelper", "basePath: " + basePath);
+
+    try {
+      System.load(basePath + "arm64-v8a/libtango_client_api.so");
+      loadedSoId = ARCH_ARM64;
+      Log.i("TangoInitializationHelper", "Success! Using arm64-v8a/libtango_client_api.");
+    } catch (UnsatisfiedLinkError e) {
+    }
+    if (loadedSoId < ARCH_DEFAULT) {
+      try {
+        System.load(basePath + "armeabi-v7a/libtango_client_api.so");
+        loadedSoId = ARCH_ARM32;
+        Log.i("TangoInitializationHelper", "Success! Using armeabi-v7a/libtango_client_api.");
+      } catch (UnsatisfiedLinkError e) {
+      }
+    }
+    if (loadedSoId < ARCH_DEFAULT) {
+      try {
+        System.load(basePath + "x86_64/libtango_client_api.so");
+        loadedSoId = ARCH_X86_64;
+        Log.i("TangoInitializationHelper", "Success! Using x86_64/libtango_client_api.");
+      } catch (UnsatisfiedLinkError e) {
+      }
+    }
+    if (loadedSoId < ARCH_DEFAULT) {
+      try {
+        System.load(basePath + "x86/libtango_client_api.so");
+        loadedSoId = ARCH_X86;
+        Log.i("TangoInitializationHelper", "Success! Using x86/libtango_client_api.");
+      } catch (UnsatisfiedLinkError e) {
+      }
+    }
+    if (loadedSoId < ARCH_DEFAULT) {
+      try {
+        System.load(basePath + "default/libtango_client_api.so");
+        loadedSoId = ARCH_DEFAULT;
+        Log.i("TangoInitializationHelper", "Success! Using default/libtango_client_api.");
+      } catch (UnsatisfiedLinkError e) {
+      }
+    }
+    if (loadedSoId < ARCH_DEFAULT) {
+      try {
+        System.loadLibrary("tango_client_api");
+        loadedSoId = ARCH_FALLBACK;
+        Log.i("TangoInitializationHelper", "Falling back to libtango_client_api.so symlink.");
+      } catch (UnsatisfiedLinkError e) {
+      }
+    }
+    return loadedSoId;
+  }
+}

corelib/include/rtabmap/core/CameraStereo.h

@@ -39,6 +39,14 @@ namespace FlyCapture2
 class Camera;
 }
 
+namespace sl
+{
+namespace zed
+{
+class Camera;
+}
+}
+
 namespace rtabmap
 {
 
@@ -94,6 +102,32 @@ private:
 	void * triclopsCtx_; // TriclopsContext
 };
 
+/////////////////////////
+// CameraStereoZED
+/////////////////////////
+class RTABMAP_EXP CameraStereoZed :
+	public Camera
+{
+public:
+	static bool available();
+
+public:
+	CameraStereoZed(bool rgbdMode, float imageRate=0.0f, const Transform & localTransform = Transform::getIdentity());
+	virtual ~CameraStereoZed();
+
+	virtual bool init(const std::string & calibrationFolder = ".", const std::string & cameraName = "");
+	virtual bool isCalibrated() const;
+	virtual std::string getSerial() const;
+
+protected:
+	virtual SensorData captureImage();
+
+private:
+	sl::zed::Camera * zed_;
+	StereoCameraModel stereoModel_;
+	bool rgbdMode_;
+};
+
 /////////////////////////
 // CameraStereoImages
 /////////////////////////
@@ -147,6 +181,10 @@ public:
 			bool rectifyImages = false,
 			float imageRate=0.0f,
 			const Transform & localTransform = Transform::getIdentity());
+	CameraStereoVideo(
+			int device,
+			float imageRate = 0.0f,
+			const Transform & localTransform = Transform::getIdentity());
 	virtual ~CameraStereoVideo();
 
 	virtual bool init(const std::string & calibrationFolder = ".", const std::string & cameraName = "");
@@ -162,6 +200,8 @@ private:
 	bool rectifyImages_;
 	StereoCameraModel stereoModel_;
 	std::string cameraName_;
+	CameraVideo::Source src_;
+	int usbDevice_;
 };
 
 } // namespace rtabmap

corelib/include/rtabmap/core/Graph.h

@@ -79,6 +79,13 @@ std::multimap<int, int>::const_iterator RTABMAP_EXP findLink(
 		int to,
 		bool checkBothWays = true);
 
+std::multimap<int, Link> RTABMAP_EXP filterLinks(
+		const std::multimap<int, Link> & links,
+		Link::Type filteredType);
+std::map<int, Link> RTABMAP_EXP filterLinks(
+		const std::map<int, Link> & links,
+		Link::Type filteredType);
+
 //Note: This assumes a coordinate system where X is forward, * Y is up, and Z is right.
 std::map<int, Transform> RTABMAP_EXP frustumPosesFiltering(
 		const std::map<int, Transform> & poses,

corelib/include/rtabmap/core/OdometryF2F.h

@@ -55,7 +55,7 @@ private:
 
 	Registration * registrationPipeline_;
 	Signature refFrame_;
-	Transform motionSinceLastKeyFrame_;
+	Transform lastKeyFramePose_;
 };
 
 }

corelib/include/rtabmap/core/Parameters.h

@@ -221,9 +221,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.");
+	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.");
 #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.");
+	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.");
 #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.");
@@ -323,7 +323,6 @@ class RTABMAP_EXP Parameters
 	// Local/Proximity loop closure detection
 	RTABMAP_PARAM(RGBD, ProximityByTime,              bool, false, 	"Detection over all locations in STM.");
 	RTABMAP_PARAM(RGBD, ProximityBySpace,             bool, true, 	"Detection over locations (in Working Memory or STM) near in space.");
-	RTABMAP_PARAM(RGBD, ProximityPathScansMerged,     bool, true, 	"Merge close laser scans on each path. If false, only the nearest laser scan on the path is used for ICP.");
 	RTABMAP_PARAM(RGBD, ProximityMaxGraphDepth,       int, 50,      "Maximum depth from the current/last loop closure location and the local loop closure hypotheses. Set 0 to ignore.");
 	RTABMAP_PARAM(RGBD, ProximityPathFilteringRadius, float, 0.5,   "Path filtering radius.");
 	RTABMAP_PARAM(RGBD, ProximityPathRawPosesUsed,    bool, true,   "When comparing to a local path, merge the scan using the odometry poses (with neighbor link optimizations) instead of the ones in the optimized local graph.");
@@ -397,7 +396,17 @@ class RTABMAP_EXP Parameters
 	RTABMAP_PARAM(Vis, EpipolarGeometryVar,      float, 0.02,   "[Vis/EstimationType = 2] Epipolar geometry maximum variance to accept the transformation.");
 	RTABMAP_PARAM(Vis, MinInliers,               int, 20, 		"Minimum feature correspondences to compute/accept the transformation.");
 	RTABMAP_PARAM(Vis, Iterations,               int, 100, 		"Maximum iterations to compute the transform.");
-	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.");
+#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.");
+#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.");
+#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.");
+#endif
+	
 	RTABMAP_PARAM(Vis, MaxFeatures,              int, 1000,     "0 no limits.");
 	RTABMAP_PARAM(Vis, MaxDepth, 	             float, 0.0,    "Max depth of the features (0 means no limit).");
 	RTABMAP_PARAM(Vis, MinDepth, 	             float, 0.0,    "Min depth of the features (0 means no limit).");

corelib/include/rtabmap/core/Rtabmap.h

@@ -196,7 +196,6 @@ private:
 	int _proximityMaxGraphDepth;
 	float _proximityFilteringRadius;
 	bool _proximityRawPosesUsed;
-	bool _proximityScansMerged;
 	float _proximityAngle;
 	std::string _databasePath;
 	bool _optimizeFromGraphEnd;

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

@@ -28,10 +28,15 @@ void segmentObstaclesFromGround(
 		float clusterRadius,
 		int minClusterSize,
 		bool segmentFlatObstacles,
-		float maxGroundHeight)
+		float maxGroundHeight,
+		pcl::IndicesPtr * flatObstacles)
 {
 	ground.reset(new std::vector<int>);
 	obstacles.reset(new std::vector<int>);
+	if(flatObstacles)
+	{
+		flatObstacles->reset(new std::vector<int>);
+	}
 
 	if(cloud->size())
 	{
@@ -71,10 +76,15 @@ void segmentObstaclesFromGround(
 						{
 							Eigen::Vector4f centroid;
 							pcl::compute3DCentroid(*cloud, *clusteredFlatSurfaces.at(i), centroid);
-							if(centroid[2] >= min[2] && centroid[2] <= max[2])
+							if(centroid[2] >= min[2]-0.01 &&
+							  (centroid[2] <= max[2]+0.01 || (maxGroundHeight>0 && centroid[2] <= maxGroundHeight+0.01))) // epsilon
 							{
 								ground = util3d::concatenate(ground, clusteredFlatSurfaces.at(i));
 							}
+							else if(flatObstacles)
+							{
+								*flatObstacles = util3d::concatenate(*flatObstacles, clusteredFlatSurfaces.at(i));
+							}
 						}
 					}
 				}
@@ -82,6 +92,10 @@ void segmentObstaclesFromGround(
 				{
 					// reject ground!
 					ground.reset(new std::vector<int>);
+					if(flatObstacles)
+					{
+						*flatObstacles = flatSurfaces;
+					}
 				}
 			}
 		}
@@ -95,6 +109,12 @@ void segmentObstaclesFromGround(
 			// Remove ground
 			pcl::IndicesPtr otherStuffIndices = util3d::extractIndices(cloud, ground, true);
 
+			// If ground height is set, remove obstacles under it
+			if(maxGroundHeight > 0.0f)
+			{
+				otherStuffIndices = rtabmap::util3d::passThrough(cloud, otherStuffIndices, "z", maxGroundHeight, std::numeric_limits<float>::max());
+			}
+
 			//Cluster remaining stuff (obstacles)
 			std::vector<pcl::IndicesPtr> clusteredObstaclesSurfaces = util3d::extractClusters(
 					cloud,
@@ -118,7 +138,8 @@ void segmentObstaclesFromGround(
 		float clusterRadius,
 		int minClusterSize,
 		bool segmentFlatObstacles,
-		float maxGroundHeight)
+		float maxGroundHeight,
+		pcl::IndicesPtr * flatObstacles)
 {
 	pcl::IndicesPtr indices(new std::vector<int>);
 	segmentObstaclesFromGround<PointT>(
@@ -131,7 +152,8 @@ void segmentObstaclesFromGround(
 			clusterRadius,
 			minClusterSize,
 			segmentFlatObstacles,
-			maxGroundHeight);
+			maxGroundHeight,
+			flatObstacles);
 }
 
 template<typename PointT>

corelib/include/rtabmap/core/util2d.h

@@ -32,6 +32,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 
 #include <opencv2/core/core.hpp>
 #include <rtabmap/core/Transform.h>
+#include <rtabmap/core/Parameters.h>
 
 namespace rtabmap
 {
@@ -68,7 +69,7 @@ void RTABMAP_EXP calcOpticalFlowPyrLKStereo( cv::InputArray _prevImg, cv::InputA
 cv::Mat RTABMAP_EXP disparityFromStereoImages(
 		const cv::Mat & leftImage,
 		const cv::Mat & rightImage,
-		int type = CV_32FC1); // CV_32FC1 or CV_16SC1
+	    const ParametersMap & parameters = ParametersMap());
 
 cv::Mat RTABMAP_EXP depthFromDisparity(const cv::Mat & disparity,
 		float fx, float baseline,

corelib/include/rtabmap/core/util3d.h

@@ -36,6 +36,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #include <pcl/TextureMesh.h>
 #include <rtabmap/core/Transform.h>
 #include <rtabmap/core/SensorData.h>
+#include <rtabmap/core/Parameters.h>
 #include <opencv2/core/core.hpp>
 #include <map>
 #include <list>
@@ -116,14 +117,16 @@ pcl::PointCloud<pcl::PointXYZRGB>::Ptr RTABMAP_EXP cloudFromStereoImages(
 		int decimation = 1,
 		float maxDepth = 0.0f,
 		float minDepth = 0.0f,
-		std::vector<int> * validIndices = 0);
+		std::vector<int> * validIndices = 0,
+		const ParametersMap & parameters = ParametersMap());
 
 pcl::PointCloud<pcl::PointXYZ>::Ptr RTABMAP_EXP cloudFromSensorData(
 		const SensorData & sensorData,
 		int decimation = 1,
 		float maxDepth = 0.0f,
 		float minDepth = 0.0f,
-		std::vector<int> * validIndices = 0);
+		std::vector<int> * validIndices = 0,
+		const ParametersMap & parameters = ParametersMap());
 
 /**
  * Create an RGB cloud from the images contained in SensorData. If there is only one camera,
@@ -143,7 +146,8 @@ pcl::PointCloud<pcl::PointXYZRGB>::Ptr RTABMAP_EXP cloudRGBFromSensorData(
 		int decimation = 1,
 		float maxDepth = 0.0f,
 		float minDepth = 0.0f,
-		std::vector<int> * validIndices = 0);
+		std::vector<int> * validIndices = 0,
+		const ParametersMap & parameters = ParametersMap());
 
 pcl::PointCloud<pcl::PointXYZ> RTABMAP_EXP laserScanFromDepthImage(
 					const cv::Mat & depthImage,

corelib/include/rtabmap/core/util3d_filtering.h

@@ -108,6 +108,20 @@ pcl::PointCloud<pcl::PointXYZRGB>::Ptr RTABMAP_EXP randomSampling(
 		int samples);
 
 
+pcl::IndicesPtr RTABMAP_EXP passThrough(
+		const pcl::PointCloud<pcl::PointXYZ>::Ptr & cloud,
+		const pcl::IndicesPtr & indices,
+		const std::string & axis,
+		float min,
+		float max,
+		bool negative = false);
+pcl::IndicesPtr RTABMAP_EXP passThrough(
+		const pcl::PointCloud<pcl::PointXYZRGB>::Ptr & cloud,
+		const pcl::IndicesPtr & indices,
+		const std::string & axis,
+		float min,
+		float max,
+		bool negative = false);
 pcl::PointCloud<pcl::PointXYZ>::Ptr RTABMAP_EXP passThrough(
 		const pcl::PointCloud<pcl::PointXYZ>::Ptr & cloud,
 		const std::string & axis,

corelib/include/rtabmap/core/util3d_mapping.h

@@ -91,7 +91,8 @@ void segmentObstaclesFromGround(
 		float clusterRadius,
 		int minClusterSize,
 		bool segmentFlatObstacles = false,
-		float maxGroundHeight = 0.0f);
+		float maxGroundHeight = 0.0f,
+		pcl::IndicesPtr * flatObstacles = 0);
 template<typename PointT>
 void segmentObstaclesFromGround(
 		const typename pcl::PointCloud<PointT>::Ptr & cloud,
@@ -102,7 +103,8 @@ void segmentObstaclesFromGround(
 		float clusterRadius,
 		int minClusterSize,
 		bool segmentFlatObstacles = false,
-		float maxGroundHeight = 0.0f);
+		float maxGroundHeight = 0.0f,
+		pcl::IndicesPtr * flatObstacles = 0);
 
 template<typename PointT>
 void occupancy2DFromCloud3D(

corelib/src/CMakeLists.txt

@@ -209,6 +209,27 @@ IF(cvsba_FOUND)
 	)
 ENDIF(cvsba_FOUND)
 
+IF(ZED_FOUND)
+	SET(INCLUDE_DIRS
+		${INCLUDE_DIRS}
+		${ZED_INCLUDE_DIRS}
+	)
+	SET(LIBRARIES
+		${LIBRARIES}
+		${ZED_LIBRARIES}
+	)
+	IF(CUDA_FOUND)
+		SET(INCLUDE_DIRS
+			${INCLUDE_DIRS}
+			${CUDA_INCLUDE_DIRS}
+		)
+		SET(LIBRARIES
+			${LIBRARIES}
+			${CUDA_LIBRARIES}
+		)
+	ENDIF(CUDA_FOUND)
+ENDIF(ZED_FOUND)
+
 ####################################
 # Generate resources files
 ####################################

corelib/src/CameraRGB.cpp

@@ -722,7 +722,7 @@ CameraVideo::~CameraVideo()
 
 bool CameraVideo::init(const std::string & calibrationFolder, const std::string & cameraName)
 {
-	_guid.clear();
+	_guid = cameraName;
 	if(_capture.isOpened())
 	{
 		_capture.release();
@@ -750,19 +750,22 @@ bool CameraVideo::init(const std::string & calibrationFolder, const std::string
 	}
 	else
 	{
-		unsigned int guid = (unsigned int)_capture.get(CV_CAP_PROP_GUID);
-		if(guid != 0 && guid != 0xffffffff)
+		if (_guid.empty())
 		{
-			_guid = uFormat("%08x", guid);
+			unsigned int guid = (unsigned int)_capture.get(CV_CAP_PROP_GUID);
+			if (guid != 0 && guid != 0xffffffff)
+			{
+				_guid = uFormat("%08x", guid);
+			}
 		}
 
 		// look for calibration files
-		if(!calibrationFolder.empty() && (!_guid.empty() || !cameraName.empty()))
+		if(!calibrationFolder.empty() && !_guid.empty())
 		{
-			if(!_model.load(calibrationFolder, (cameraName.empty()?_guid:cameraName)))
+			if(!_model.load(calibrationFolder, _guid))
 			{
 				UWARN("Missing calibration files for camera \"%s\" in \"%s\" folder, you should calibrate the camera!",
-						cameraName.empty()?_guid.c_str():cameraName.c_str(), calibrationFolder.c_str());
+						_guid.c_str(), calibrationFolder.c_str());
 			}
 			else
 			{

corelib/src/CameraStereo.cpp

@@ -48,6 +48,10 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #include <fc2triclops.h>
 #endif
 
+#ifdef RTABMAP_ZED
+#include <zed/Camera.hpp>
+#endif
+
 namespace rtabmap
 {
 
@@ -731,6 +735,149 @@ SensorData CameraStereoFlyCapture2::captureImage()
 	return data;
 }
 
+//
+// CameraStereoZED
+//
+bool CameraStereoZed::available()
+{
+#ifdef RTABMAP_ZED
+	return true;
+#else
+	return false;
+#endif
+}
+
+CameraStereoZed::CameraStereoZed(bool rgbdMode, float imageRate, const Transform & localTransform) :
+		Camera(imageRate, localTransform),
+		zed_(0),
+		rgbdMode_(rgbdMode)
+{
+}
+
+CameraStereoZed::~CameraStereoZed()
+{
+#ifdef RTABMAP_ZED
+	if(zed_)
+	{
+		delete zed_;
+	}
+#endif
+}
+
+bool CameraStereoZed::init(const std::string & calibrationFolder, const std::string & cameraName)
+{
+#ifdef RTABMAP_ZED
+	if(zed_)
+	{
+		delete zed_;
+		zed_ = 0;
+	}
+	
+	if(zed_->isZEDconnected())
+	{
+		zed_ = new sl::zed::Camera(sl::zed::HD720); // Use in Live Mode
+		//zed_ = new sl::zed::Camera(argv[1]); // Use in SVO playback mode
+
+		//init WITH self-calibration (- last parameter to false -)
+		sl::zed::ERRCODE err = zed_->init(sl::zed::MODE::PERFORMANCE, 0, true, false, false);
+
+		// Quit if an error occurred
+		if (err != sl::zed::SUCCESS)
+		{
+			UERROR("ZED camera initialization failed: %s", sl::zed::errcode2str(err).c_str());
+			delete zed_;
+			zed_ = 0;
+			return false;
+		}
+	}
+	else
+	{
+		UERROR("ZED camera initialization failed: ZED is not connected!");
+		return false;
+	}
+	
+	sl::zed::StereoParameters * stereoParams = zed_->getParameters();
+	sl::zed::resolution res = zed_->getImageSize();
+				
+	stereoModel_ = StereoCameraModel(
+		stereoParams->LeftCam.fx, 
+		stereoParams->LeftCam.fy, 
+		stereoParams->LeftCam.cx, 
+		stereoParams->LeftCam.cy, 
+		stereoParams->baseline/1000.0f,
+		this->getLocalTransform(),
+		cv::Size(res.width, res.height));
+
+	return true;
+#else
+	UERROR("CameraStereoZED: RTAB-Map is not built with ZED sdk support!");
+#endif
+	return false;
+}
+
+bool CameraStereoZed::isCalibrated() const
+{
+	return stereoModel_.isValidForProjection();
+}
+
+std::string CameraStereoZed::getSerial() const
+{
+#ifdef RTABMAP_ZED
+	if(zed_)
+	{
+		return uFormat("%x", zed_->getZEDSerial());
+	}
+#endif
+	return "";
+}
+
+SensorData CameraStereoZed::captureImage()
+{
+	SensorData data;
+#ifdef RTABMAP_ZED
+	if(zed_)
+	{
+		sl::zed::SENSING_MODE dm_type = sl::zed::RAW;
+		bool res = zed_->grab(dm_type);
+
+		if(!res)
+		{
+			// get left image
+			cv::Mat rgbaLeft = slMat2cvMat(zed_->retrieveImage(static_cast<sl::zed::SIDE> (sl::zed::STEREO_LEFT)));
+			cv::Mat left;
+			cv::cvtColor(rgbaLeft, left, cv::COLOR_BGRA2BGR);
+
+			if(rgbdMode_)
+			{
+				// get depth image
+				cv::Mat depth;
+				slMat2cvMat(zed_->retrieveMeasure(sl::zed::MEASURE::DEPTH)).copyTo(depth);
+				depth /= 1000.0;
+
+				data = SensorData(left, depth, stereoModel_.left(), this->getNextSeqID(), UTimer::now());
+			}
+			else
+			{
+				// get right image
+				cv::Mat rgbaRight = slMat2cvMat(zed_->retrieveImage(static_cast<sl::zed::SIDE> (sl::zed::STEREO_RIGHT)));
+				cv::Mat right;
+				cv::cvtColor(rgbaRight, right, cv::COLOR_BGRA2GRAY);
+			
+				data = SensorData(left, right, stereoModel_, this->getNextSeqID(), UTimer::now());
+			}
+		}
+		else
+		{
+			UERROR("CameraStereoZed: Failed to grab images!");
+		}
+	}
+#else
+	UERROR("CameraStereoZED: RTAB-Map is not built with ZED sdk support!");
+#endif
+	return data;
+}
+
+
 //
 // CameraStereoImages
 //
@@ -921,7 +1068,21 @@ CameraStereoVideo::CameraStereoVideo(
 		const Transform & localTransform) :
 		Camera(imageRate, localTransform),
 		path_(path),
-		rectifyImages_(rectifyImages)
+		rectifyImages_(rectifyImages),
+		src_(CameraVideo::kVideoFile),
+		usbDevice_(0)
+{
+}
+
+CameraStereoVideo::CameraStereoVideo(
+	int device,
+	float imageRate,
+	const Transform & localTransform) :
+	Camera(imageRate, localTransform),
+	path_(""),
+	rectifyImages_(false),
+	src_(CameraVideo::kUsbDevice),
+	usbDevice_(device)
 {
 }
 
@@ -932,29 +1093,51 @@ CameraStereoVideo::~CameraStereoVideo()
 
 bool CameraStereoVideo::init(const std::string & calibrationFolder, const std::string & cameraName)
 {
+	cameraName_ = cameraName;
 	if(capture_.isOpened())
 	{
 		capture_.release();
 	}
-	ULOGGER_DEBUG("Camera: filename=\"%s\"", path_.c_str());
-	capture_.open(path_.c_str());
+
+	if (src_ == CameraVideo::kUsbDevice)
+	{
+		ULOGGER_DEBUG("CameraStereoVideo: Usb device initialization on device %d", usbDevice_);
+		capture_.open(usbDevice_);
+	}
+	else if (src_ == CameraVideo::kVideoFile)
+	{
+		ULOGGER_DEBUG("CameraStereoVideo: filename=\"%s\"", path_.c_str());
+		capture_.open(path_.c_str());
+	}
+	else
+	{
+		ULOGGER_ERROR("CameraStereoVideo: Unknown source...");
+	}
 
 	if(!capture_.isOpened())
 	{
-		ULOGGER_ERROR("Camera: Failed to create a capture object!");
+		ULOGGER_ERROR("CameraStereoVideo: Failed to create a capture object!");
 		capture_.release();
 		return false;
 	}
 	else
 	{
-		// look for calibration files
-		cameraName_ = cameraName;
-		if(!calibrationFolder.empty() && !cameraName.empty())
+		if (cameraName_.empty())
 		{
-			if(!stereoModel_.load(calibrationFolder, cameraName))
+			unsigned int guid = (unsigned int)capture_.get(CV_CAP_PROP_GUID);
+			if (guid != 0 && guid != 0xffffffff)
+			{
+				cameraName_ = uFormat("%08x", guid);
+			}
+		}
+
+		// look for calibration files
+		if(!calibrationFolder.empty() && !cameraName_.empty())
+		{
+			if(!stereoModel_.load(calibrationFolder, cameraName_))
 			{
 				UWARN("Missing calibration files for camera \"%s\" in \"%s\" folder, you should calibrate the camera!",
-						cameraName.c_str(), calibrationFolder.c_str());
+					cameraName_.c_str(), calibrationFolder.c_str());
 			}
 			else
 			{
@@ -1007,7 +1190,7 @@ SensorData CameraStereoVideo::captureImage()
 				rightCvt = true;
 			}
 
-			if(rectifyImages_ && stereoModel_.left().isValidForRectification() && stereoModel_.right().isValidForRectification())
+			if((src_ != CameraVideo::kVideoFile || rectifyImages_) && stereoModel_.left().isValidForRectification() && stereoModel_.right().isValidForRectification())
 			{
 				leftImage = stereoModel_.left().rectifyImage(leftImage);
 				rightImage = stereoModel_.right().rectifyImage(rightImage);

corelib/src/Features2d.cpp

@@ -308,10 +308,9 @@ cv::Rect Feature2D::computeRoi(const cv::Mat & image, const std::vector<float> &
 		}
 
 		//right roi
-		roi.width = width - roi.x;
 		if(roiRatios[1] > 0 && roiRatios[1] < 1 - roiRatios[0])
 		{
-			roi.width -= width * roiRatios[1];
+			roi.width -= width * roiRatios[1] + width * roiRatios[0];
 		}
 
 		//top roi
@@ -321,10 +320,9 @@ cv::Rect Feature2D::computeRoi(const cv::Mat & image, const std::vector<float> &
 		}
 
 		//bottom roi
-		roi.height = height - roi.y;
 		if(roiRatios[3] > 0 && roiRatios[3] < 1 - roiRatios[2])
 		{
-			roi.height -= height * roiRatios[3];
+			roi.height -= height * roiRatios[3] + height * roiRatios[2];
 		}
 		UDEBUG("roi = %d, %d, %d, %d", roi.x, roi.y, roi.width, roi.height);
 

corelib/src/Graph.cpp

@@ -561,6 +561,36 @@ std::multimap<int, int>::const_iterator findLink(
 	return links.end();
 }
 
+std::multimap<int, Link> filterLinks(
+		const std::multimap<int, Link> & links,
+		Link::Type filteredType)
+{
+	std::multimap<int, Link> output;
+	for(std::multimap<int, Link>::const_iterator iter=links.begin(); iter!=links.end(); ++iter)
+	{
+		if(iter->second.type() != filteredType)
+		{
+			output.insert(*iter);
+		}
+	}
+	return output;
+}
+
+std::map<int, Link> filterLinks(
+		const std::map<int, Link> & links,
+		Link::Type filteredType)
+{
+	std::map<int, Link> output;
+	for(std::map<int, Link>::const_iterator iter=links.begin(); iter!=links.end(); ++iter)
+	{
+		if(iter->second.type() != filteredType)
+		{
+			output.insert(*iter);
+		}
+	}
+	return output;
+}
+
 std::map<int, Transform> frustumPosesFiltering(
 		const std::map<int, Transform> & poses,
 		const Transform & cameraPose,

corelib/src/Memory.cpp

@@ -495,7 +495,8 @@ void Memory::parseParameters(const ParametersMap & parameters)
 			if((_memoryChanged || _linksChanged) && _dbDriver)
 			{
 				UWARN("Switching from Mapping to Localization mode, the database will be saved and reloaded.");
-				this->init(_dbDriver->getUrl());
+				this->init(_dbDriver->getUrl());
+				UWARN("Switching from Mapping to Localization mode, the database is reloaded!");
 			}
 		}
 		_incrementalMemory = value;
@@ -2138,10 +2139,14 @@ Transform Memory::computeTransform(
 			// Remove descriptors, this will avoid recomputation of the correspondences in regVis
 			tmpFrom.setWordsDescriptors(std::multimap<int, cv::Mat>());
 			tmpTo.setWordsDescriptors(std::multimap<int, cv::Mat>());
-			guess = regVis.computeTransformation(tmpFrom, tmpTo, guess, info);
+			Transform t = regVis.computeTransformation(tmpFrom, tmpTo, guess, info);
 			// set back descriptors
 			tmpFrom.setWordsDescriptors(fromS.getWordsDescriptors());
 			tmpTo.setWordsDescriptors(toS.getWordsDescriptors());
+			if(!t.isNull())
+			{
+				guess = t;
+			}
 		}
 
 		if(_reextractLoopClosureFeatures)
@@ -2157,44 +2162,32 @@ Transform Memory::computeTransform(
 			tmpTo.sensorData().setFeatures(std::vector<cv::KeyPoint>(), cv::Mat());
 		}
 
-		if(guess.isNull())
-		{
-			if(!_registrationPipeline->isImageRequired())
-			{
-				UDEBUG("");
-				// no visual in the pipeline, make visual registration for guess
-				guess = regVis.computeTransformation(tmpFrom, tmpTo, guess, info);
-			}
-			else
-			{
-				UDEBUG("");
-				guess.setIdentity();
-			}
-		}
-
-		if(!guess.isNull())
+		if(guess.isNull() && !_registrationPipeline->isImageRequired())
 		{
 			UDEBUG("");
-			transform = _registrationPipeline->computeTransformation(tmpFrom, tmpTo, guess, info);
+			// no visual in the pipeline, make visual registration for guess
+			guess = regVis.computeTransformation(tmpFrom, tmpTo, guess, info);
+		}
 
-			if(!transform.isNull())
+		transform = _registrationPipeline->computeTransformation(tmpFrom, tmpTo, guess, info);
+
+		if(!transform.isNull())
+		{
+			UDEBUG("");
+			// verify if it is a 180 degree transform, well verify > 90
+			float x,y,z, roll,pitch,yaw;
+			transform.getTranslationAndEulerAngles(x,y,z, roll,pitch,yaw);
+			if(fabs(roll) > CV_PI/2 ||
+			   fabs(pitch) > CV_PI/2 ||
+			   fabs(yaw) > CV_PI/2)
 			{
-				UDEBUG("");
-				// verify if it is a 180 degree transform, well verify > 90
-				float x,y,z, roll,pitch,yaw;
-				transform.getTranslationAndEulerAngles(x,y,z, roll,pitch,yaw);
-				if(fabs(roll) > CV_PI/2 ||
-				   fabs(pitch) > CV_PI/2 ||
-				   fabs(yaw) > CV_PI/2)
+				transform.setNull();
+				std::string msg = uFormat("Too large rotation detected! (roll=%f, pitch=%f, yaw=%f)",
+						roll, pitch, yaw);
+				UINFO(msg.c_str());
+				if(info)
 				{
-					transform.setNull();
-					std::string msg = uFormat("Too large rotation detected! (roll=%f, pitch=%f, yaw=%f)",
-							roll, pitch, yaw);
-					UINFO(msg.c_str());
-					if(info)
-					{
-						info->rejectedMsg = msg;
-					}
+					info->rejectedMsg = msg;
 				}
 			}
 		}
@@ -2298,6 +2291,7 @@ Transform Memory::computeIcpTransformMulti(
 		SensorData assembledData;
 		Transform toPose = poses.at(toId);
 		std::string msg;
+		int maxPoints = fromScan.cols;
 		pcl::PointCloud<pcl::PointXYZ>::Ptr assembledToClouds(new pcl::PointCloud<pcl::PointXYZ>);
 		for(std::map<int, Transform>::const_iterator iter = poses.begin(); iter!=poses.end(); ++iter)
 		{
@@ -2309,6 +2303,10 @@ Transform Memory::computeIcpTransformMulti(
 					cv::Mat scan;
 					s->sensorData().uncompressData(0, 0, &scan);
 					pcl::PointCloud<pcl::PointXYZ>::Ptr cloud = util3d::laserScanToPointCloud(scan, toPose.inverse() * iter->second);
+					if(scan.cols > maxPoints)
+					{
+						maxPoints = scan.cols;
+					}
 					*assembledToClouds += *cloud;
 				}
 				else
@@ -2319,7 +2317,7 @@ Transform Memory::computeIcpTransformMulti(
 		}
 		if(assembledToClouds->size())
 		{
-			assembledData.setLaserScanRaw(util3d::laserScanFromPointCloud(*assembledToClouds, Transform()), fromS->sensorData().laserScanMaxPts(), fromS->sensorData().laserScanMaxRange());
+			assembledData.setLaserScanRaw(util3d::laserScanFromPointCloud(*assembledToClouds, Transform()), fromS->sensorData().laserScanMaxPts()?fromS->sensorData().laserScanMaxPts():maxPoints, fromS->sensorData().laserScanMaxRange());
 		}
 
 		Transform guess = poses.at(fromId).inverse() * poses.at(toId);
@@ -3427,7 +3425,7 @@ Signature * Memory::createSignature(const SensorData & data, const Transform & p
 		UASSERT(keypoints3D.size() == 0 || keypoints3D.size() == wordIds.size());
 		unsigned int i=0;
 		float decimationRatio = preDecimation / _imagePostDecimation;
-		double log2value = log(preDecimation)/log(2);
+		double log2value = log(double(preDecimation))/log(2.0);
 		for(std::list<int>::iterator iter=wordIds.begin(); iter!=wordIds.end() && i < keypoints.size(); ++iter, ++i)
 		{
 			cv::KeyPoint kpt = keypoints[i];

corelib/src/Odometry.cpp

@@ -253,7 +253,7 @@ Transform Odometry::process(SensorData & data, OdometryInfo * info)
 
 		// transform back the keypoints in the original image
 		std::vector<cv::KeyPoint> kpts = decimatedData.keypoints();
-		double log2value = log(_imageDecimation)/log(2);
+		double log2value = log(double(_imageDecimation))/log(2.0);
 		for(unsigned int i=0; i<kpts.size(); ++i)
 		{
 			kpts[i].pt.x *= _imageDecimation;
@@ -401,15 +401,7 @@ Transform Odometry::process(SensorData & data, OdometryInfo * info)
 			else if(!_holonomic)
 			{
 				// arc trajectory around ICR
-				float tmpY = vyaw!=0.0f ? vx / tan((CV_PI-vyaw)/2.0f) : 0.0f;
-				if(fabs(tmpY) < fabs(vy) || (tmpY<=0 && vy >=0) || (tmpY>=0 && vy<=0))
-				{
-					vy = tmpY;
-				}
-				else
-				{
-					vyaw = (atan(vx/vy)*2.0f-CV_PI)*-1;
-				}
+				vy = vyaw!=0.0f ? vx / tan((CV_PI-vyaw)/2.0f) : 0.0f;
 				if(_force3DoF)
 				{
 					vz = 0.0f;

corelib/src/OdometryF2F.cpp

@@ -39,8 +39,7 @@ namespace rtabmap {
 OdometryF2F::OdometryF2F(const ParametersMap & parameters) :
 	Odometry(parameters),
 	keyFrameThr_(Parameters::defaultOdomKeyFrameThr()),
-	scanKeyFrameThr_(Parameters::defaultOdomScanKeyFrameThr()),
-	motionSinceLastKeyFrame_(Transform::getIdentity())
+	scanKeyFrameThr_(Parameters::defaultOdomScanKeyFrameThr())
 {
 	registrationPipeline_ = Registration::create(parameters);
 	Parameters::parse(parameters, Parameters::kOdomKeyFrameThr(), keyFrameThr_);
@@ -58,7 +57,7 @@ void OdometryF2F::reset(const Transform & initialPose)
 {
 	Odometry::reset(initialPose);
 	refFrame_ = Signature();
-	motionSinceLastKeyFrame_.setIdentity();
+	lastKeyFramePose_.setNull();
 }
 
 // return not null transform if odometry is correctly computed
@@ -83,6 +82,13 @@ Transform OdometryF2F::computeTransform(
 
 	RegistrationInfo regInfo;
 
+	UASSERT(!this->getPose().isNull());
+	if(lastKeyFramePose_.isNull())
+	{
+		lastKeyFramePose_ = this->getPose(); // reset to current pose
+	}
+	Transform motionSinceLastKeyFrame = lastKeyFramePose_.inverse()*this->getPose();
+
 	Signature newFrame(data);
 	if(refFrame_.sensorData().isValid())
 	{
@@ -90,7 +96,7 @@ Transform OdometryF2F::computeTransform(
 		output = registrationPipeline_->computeTransformationMod(
 				tmpRefFrame,
 				newFrame,
-				!guess.isNull()?motionSinceLastKeyFrame_*guess:Transform(),
+				!guess.isNull()?motionSinceLastKeyFrame*guess:Transform(),
 				&regInfo);
 
 		if(info && this->isInfoDataFilled())
@@ -117,7 +123,7 @@ Transform OdometryF2F::computeTransform(
 				info->cornerInliers[i] = idToIndex.at(regInfo.inliersIDs[i]);
 			}
 
-			Transform t = this->getPose()*motionSinceLastKeyFrame_.inverse();
+			Transform t = this->getPose()*motionSinceLastKeyFrame.inverse();
 			for(std::multimap<int, cv::Point3f>::const_iterator iter=tmpRefFrame.getWords3().begin(); iter!=tmpRefFrame.getWords3().end(); ++iter)
 			{
 				info->localMap.insert(std::make_pair(iter->first, util3d::transformPoint(iter->second, t)));
@@ -135,8 +141,7 @@ Transform OdometryF2F::computeTransform(
 
 	if(!output.isNull())
 	{
-		output = motionSinceLastKeyFrame_.inverse() * output;
-		motionSinceLastKeyFrame_ *= output;
+		output = motionSinceLastKeyFrame.inverse() * output;
 
 		// new key-frame?
 		if( (registrationPipeline_->isImageRequired() && (keyFrameThr_ == 0 || float(regInfo.inliers) <= keyFrameThr_*float(refFrame_.sensorData().keypoints().size()))) ||
@@ -167,7 +172,7 @@ Transform OdometryF2F::computeTransform(
 				refFrame_.setWordsDescriptors(std::multimap<int, cv::Mat>());
 
 				//reset motion
-				motionSinceLastKeyFrame_.setIdentity();
+				lastKeyFramePose_.setNull();
 			}
 			else
 			{

corelib/src/Parameters.cpp

@@ -155,14 +155,17 @@ const std::map<std::string, std::pair<bool, std::string> > & Parameters::getRemo
 	{
 		// removed parameters
 
+		// 0.11.6
+		removedParameters_.insert(std::make_pair("RGBD/ProximityPathScansMerged", std::make_pair(false,  "")));
+
 		// 0.11.3
-		removedParameters_.insert(std::make_pair("Mem/ImageDecimation",      std::make_pair(true, Parameters::kMemImagePostDecimation())));
+		removedParameters_.insert(std::make_pair("Mem/ImageDecimation",           std::make_pair(true, Parameters::kMemImagePostDecimation())));
 
 		// 0.11.2
 		removedParameters_.insert(std::make_pair("OdomLocalMap/HistorySize",      std::make_pair(true, Parameters::kOdomF2MMaxSize())));
 		removedParameters_.insert(std::make_pair("OdomLocalMap/FixedMapPath",     std::make_pair(true, Parameters::kOdomF2MFixedMapPath())));
 		removedParameters_.insert(std::make_pair("OdomF2F/GuessMotion",           std::make_pair(true, Parameters::kOdomGuessMotion())));
-		removedParameters_.insert(std::make_pair("OdomF2F/KeyFrameThr",          std::make_pair(false, Parameters::kOdomKeyFrameThr())));
+		removedParameters_.insert(std::make_pair("OdomF2F/KeyFrameThr",           std::make_pair(false, Parameters::kOdomKeyFrameThr())));
 
 		// 0.11.0
 		removedParameters_.insert(std::make_pair("OdomBow/LocalHistorySize",      std::make_pair(true, Parameters::kOdomF2MMaxSize())));
@@ -244,7 +247,7 @@ const std::map<std::string, std::pair<bool, std::string> > & Parameters::getRemo
 		removedParameters_.insert(std::make_pair("RGBD/LocalLoopDetectionBySpace",             std::make_pair(true,  Parameters::kRGBDProximityBySpace())));
 		removedParameters_.insert(std::make_pair("RGBD/LocalLoopDetectionTime",                std::make_pair(true,  Parameters::kRGBDProximityByTime())));
 		removedParameters_.insert(std::make_pair("RGBD/LocalLoopDetectionSpace",               std::make_pair(true,  Parameters::kRGBDProximityBySpace())));
-		removedParameters_.insert(std::make_pair("RGBD/LocalLoopDetectionPathScansMerged",     std::make_pair(true,  Parameters::kRGBDProximityPathScansMerged())));
+		removedParameters_.insert(std::make_pair("RGBD/LocalLoopDetectionPathScansMerged",     std::make_pair(false,  "")));
 		removedParameters_.insert(std::make_pair("RGBD/LocalLoopDetectionMaxGraphDepth",       std::make_pair(true,  Parameters::kRGBDProximityMaxGraphDepth())));
 		removedParameters_.insert(std::make_pair("RGBD/LocalLoopDetectionPathFilteringRadius", std::make_pair(true,  Parameters::kRGBDProximityPathFilteringRadius())));
 		removedParameters_.insert(std::make_pair("RGBD/LocalLoopDetectionPathRawPosesUsed",    std::make_pair(true,  Parameters::kRGBDProximityPathRawPosesUsed())));

corelib/src/Registration.cpp

@@ -186,6 +186,11 @@ Transform Registration::computeTransformationMod(
 		info = *infoOut;
 	}
 
+	if(!guess.isNull() && force3DoF_)
+	{
+		guess = guess.to3DoF();
+	}
+
 	Transform t = computeTransformationImpl(from, to, guess, info);
 
 	if(varianceFromInliersCount_)
@@ -207,6 +212,11 @@ Transform Registration::computeTransformationMod(
 		{
 			t = child_->computeTransformationMod(from, to, force3DoF_?t.to3DoF():t, &info);
 		}
+		else if(!guess.isNull())
+		{
+			UDEBUG("This registration approach failed, continue with the guess for the next registration");
+			t = child_->computeTransformationMod(from, to, guess, &info);
+		}
 	}
 	else if(!t.isNull() && force3DoF_)
 	{

corelib/src/RegistrationIcp.cpp

@@ -113,7 +113,7 @@ Transform RegistrationIcp::computeTransformationImpl(
 	if(!guess.isNull() && !dataFrom.laserScanRaw().empty() && !dataTo.laserScanRaw().empty())
 	{
 		// ICP with guess transform
-		int maxLaserScans = dataTo.laserScanMaxPts();
+		int maxLaserScans = dataTo.laserScanMaxPts()?dataTo.laserScanMaxPts():dataFrom.laserScanMaxPts();
 		cv::Mat fromScan = dataFrom.laserScanRaw();
 		cv::Mat toScan = dataTo.laserScanRaw();
 		if(_downsamplingStep>1)
@@ -309,8 +309,13 @@ Transform RegistrationIcp::computeTransformationImpl(
 					}
 					else
 					{
-						UWARN("Maximum laser scans points not set for signature %d, correspondences ratio set relative instead of absolute!",
-								dataTo.id());
+						static bool warningShown = false;
+						if(!warningShown)
+						{
+							UWARN("Maximum laser scans points not set for signature %d, correspondences ratio set relative instead of absolute! This message will only appear once.",
+									dataTo.id());
+							warningShown = true;
+						}
 						correspondencesRatio = float(correspondences)/float(toScan.cols>fromScan.cols?toScan.cols:fromScan.cols);
 					}
 

corelib/src/Rtabmap.cpp

@@ -100,7 +100,6 @@ Rtabmap::Rtabmap() :
 	_proximityMaxGraphDepth(Parameters::defaultRGBDProximityMaxGraphDepth()),
 	_proximityFilteringRadius(Parameters::defaultRGBDProximityPathFilteringRadius()),
 	_proximityRawPosesUsed(Parameters::defaultRGBDProximityPathRawPosesUsed()),
-	_proximityScansMerged(Parameters::defaultRGBDProximityPathScansMerged()),
 	_proximityAngle(Parameters::defaultRGBDProximityAngle()*M_PI/180.0f),
 	_databasePath(""),
 	_optimizeFromGraphEnd(Parameters::defaultRGBDOptimizeFromGraphEnd()),
@@ -411,7 +410,6 @@ void Rtabmap::parseParameters(const ParametersMap & parameters)
 	Parameters::parse(parameters, Parameters::kRGBDProximityMaxGraphDepth(), _proximityMaxGraphDepth);
 	Parameters::parse(parameters, Parameters::kRGBDProximityPathFilteringRadius(), _proximityFilteringRadius);
 	Parameters::parse(parameters, Parameters::kRGBDProximityPathRawPosesUsed(), _proximityRawPosesUsed);
-	Parameters::parse(parameters, Parameters::kRGBDProximityPathScansMerged(), _proximityScansMerged);
 	Parameters::parse(parameters, Parameters::kRGBDProximityAngle(), _proximityAngle);
 	_proximityAngle *= M_PI/180.0f;
 	Parameters::parse(parameters, Parameters::kRGBDOptimizeFromGraphEnd(), _optimizeFromGraphEnd);
@@ -1013,17 +1011,16 @@ bool Rtabmap::process(
 			else
 			{
 				//============================================================
-				// Scan matching
+				// Refine neighbor links
 				//============================================================
 				if(!signature->sensorData().laserScanCompressed().empty())
 				{
-					UINFO("Odometry correction by scan matching");
-					Transform guess = signature->getLinks().begin()->second.transform().inverse();
+					UINFO("Odometry refining: guess = %s", guess.prettyPrint().c_str());
 					RegistrationInfo info;
-					Transform t = _memory->computeIcpTransform(oldId, signature->id(), guess, &info);
+					Transform t = _memory->computeTransform(oldId, signature->id(), guess, &info);
 					if(!t.isNull())
 					{
-						UINFO("Scan matching: update neighbor link (%d->%d, variance=%f) from %s to %s",
+						UINFO("Odometry refining: update neighbor link (%d->%d, variance=%f) from %s to %s",
 								oldId,
 								signature->id(),
 								info.variance,
@@ -1050,7 +1047,7 @@ bool Rtabmap::process(
 					}
 					else
 					{
-						UINFO("Scan matching rejected: %s", info.rejectedMsg.c_str());
+						UINFO("Odometry refining rejected: %s", info.rejectedMsg.c_str());
 						if(info.variance > 0)
 						{
 							double sqrtVar = sqrt(info.variance);
@@ -1065,7 +1062,7 @@ bool Rtabmap::process(
 				}
 			}
 			timeNeighborLinkRefining = timer.ticks();
-			ULOGGER_INFO("timeScanMatching=%fs", timeNeighborLinkRefining);
+			ULOGGER_INFO("timeOdometryRefining=%fs", timeNeighborLinkRefining);
 
 			UASSERT(oldS->hasLink(signature->id()));
 			UASSERT(uContains(_optimizedPoses, oldId));
@@ -1902,47 +1899,37 @@ bool Rtabmap::process(
 						   (_proximityFilteringRadius <= 0.0f ||
 							_optimizedPoses.at(signature->id()).getDistanceSquared(_optimizedPoses.at(nearestId)) < _proximityFilteringRadius*_proximityFilteringRadius))
 						{
-							if(!_proximityScansMerged)
-							{
-								//only keep the nearest node
-								std::map<int, Transform> tmp;
-								tmp.insert(*path.find(nearestId));
-								path = tmp;
-							}
-							else
+							// Assemble scans in the path and do ICP only
+							if(_proximityRawPosesUsed)
 							{
-								// Assemble scans in the path and do ICP only
-								if(_proximityRawPosesUsed)
+								//optimize the path's poses locally
+								path = optimizeGraph(nearestId, uKeysSet(path), std::map<int, Transform>(), false);
+								// transform local poses in optimized graph referential
+								UASSERT(uContains(path, nearestId));
+								Transform t = _optimizedPoses.at(nearestId) * path.at(nearestId).inverse();
+								for(std::map<int, Transform>::iterator jter=path.begin(); jter!=path.end(); ++jter)
 								{
-									//optimize the path's poses locally
-									path = optimizeGraph(nearestId, uKeysSet(path), std::map<int, Transform>(), false);
-									// transform local poses in optimized graph referential
-									UASSERT(uContains(path, nearestId));
-									Transform t = _optimizedPoses.at(nearestId) * path.at(nearestId).inverse();
-									for(std::map<int, Transform>::iterator jter=path.begin(); jter!=path.end(); ++jter)
-									{
-										jter->second = t * jter->second;
-									}
-								}
-								if(path.size() > 2 && _proximityFilteringRadius > 0.0f)
-								{
-									// path filtering
-									std::map<int, Transform> filteredPath = graph::radiusPosesFiltering(path, _proximityFilteringRadius, 0, true);
-									// make sure the current pose is still here
-									filteredPath.insert(*path.find(nearestId));
-									path = filteredPath;
+									jter->second = t * jter->second;
 								}
 							}
+							std::map<int, Transform> filteredPath = path;
+							if(path.size() > 2 && _proximityFilteringRadius > 0.0f)
+							{
+								// path filtering
+								filteredPath = graph::radiusPosesFiltering(path, _proximityFilteringRadius, 0, true);
+								// make sure the current pose is still here
+								filteredPath.insert(*path.find(nearestId));
+							}
 
-							if(path.size() > 0)
+							if(filteredPath.size() > 0)
 							{
 								// add current node to poses
-								path.insert(std::make_pair(signature->id(), _optimizedPoses.at(signature->id())));
+								filteredPath.insert(std::make_pair(signature->id(), _optimizedPoses.at(signature->id())));
 								//The nearest will be the reference for a loop closure transform
 								if(signature->getLinks().find(nearestId) == signature->getLinks().end())
 								{
 									RegistrationInfo info;
-									Transform transform = _memory->computeIcpTransformMulti(signature->id(), nearestId, path, &info);
+									Transform transform = _memory->computeIcpTransformMulti(signature->id(), nearestId, filteredPath, &info);
 									if(!transform.isNull())
 									{
 										if(_proximityFilteringRadius <= 0 || transform.getNormSquared() <= _proximityFilteringRadius*_proximityFilteringRadius)
@@ -1959,14 +1946,11 @@ bool Rtabmap::process(
 												stream << "SCANS:";
 												for(std::map<int, Transform>::iterator iter=path.begin(); iter!=path.end(); ++iter)
 												{
-													if(iter->first!=signature->id())
+													if(iter != path.begin())
 													{
-														if(iter != path.begin())
-														{
-															stream << ";";
-														}
-														stream << uNumber2Str(iter->first);
+														stream << ";";
 													}
+													stream << uNumber2Str(iter->first);
 												}
 												std::string scansStr = stream.str();
 												scanMatchingIds = cv::Mat(1, int(scansStr.size()+1), CV_8SC1, (void *)scansStr.c_str());
@@ -2051,14 +2035,17 @@ bool Rtabmap::process(
 	{
 		UASSERT(uContains(_optimizedPoses, signature->id()));
 
+		//used in localization mode: filter virtual links
+		std::map<int, Link> localizationLinks = graph::filterLinks(signature->getLinks(), Link::kVirtualClosure);
+
 		// Note that in localization mode, we don't re-optimize the graph
 		// if:
 		//  1- there are no signatures retrieved,
 		//  2- we are relocalizing on a node already in the optimized graph
 		if(!_memory->isIncremental() &&
 		   signaturesRetrieved.size() == 0 &&
-		   signature->getLinks().size() &&
-		   uContains(_optimizedPoses, signature->getLinks().begin()->first))
+		   localizationLinks.size() &&
+		   uContains(_optimizedPoses, localizationLinks.begin()->first))
 		{
 			// If there are no signatures retrieved, we don't
 			// need to re-optimize the graph. Just update the last
@@ -2070,8 +2057,8 @@ bool Rtabmap::process(
 				// update all previous nodes
 				// Normally _mapCorrection should be identity, but if _optimizeFromGraphEnd
 				// parameters just changed state, we should put back all poses without map correction.
-				Transform oldPose = _optimizedPoses.at(signature->getLinks().begin()->first);
-				Transform u = signature->getPose() * signature->getLinks().begin()->second.transform();
+				Transform oldPose = _optimizedPoses.at(localizationLinks.begin()->first);
+				Transform u = signature->getPose() * localizationLinks.begin()->second.transform();
 				Transform up = u * oldPose.inverse();
 				Transform mapCorrectionInv = _mapCorrection.inverse();
 				for(std::map<int, Transform>::iterator iter=_optimizedPoses.begin(); iter!=_optimizedPoses.end(); ++iter)
@@ -2082,7 +2069,7 @@ bool Rtabmap::process(
 			}
 			else
 			{
-				_optimizedPoses.at(signature->id()) = _optimizedPoses.at(signature->getLinks().begin()->first) * signature->getLinks().begin()->second.transform().inverse();
+				_optimizedPoses.at(signature->id()) = _optimizedPoses.at(localizationLinks.begin()->first) * localizationLinks.begin()->second.transform().inverse();
 			}
 		}
 		else

corelib/src/util2d.cpp

@@ -33,6 +33,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #include <rtabmap/utilite/UTimer.h>
 #include <rtabmap/utilite/UStl.h>
 #include <rtabmap/core/util3d_transforms.h>
+#include <rtabmap/core/StereoDense.h>
 #include <opencv2/calib3d/calib3d.hpp>
 #include <opencv2/imgproc/imgproc.hpp>
 #include <opencv2/video/tracking.hpp>
@@ -724,12 +725,11 @@ void calcOpticalFlowPyrLKStereo( cv::InputArray _prevImg, cv::InputArray _nextIm
 cv::Mat disparityFromStereoImages(
 		const cv::Mat & leftImage,
 		const cv::Mat & rightImage,
-		int type)
+		const ParametersMap & parameters)
 {
 	UASSERT(!leftImage.empty() && !rightImage.empty());
 	UASSERT(leftImage.cols == rightImage.cols && leftImage.rows == rightImage.rows);
 	UASSERT((leftImage.type() == CV_8UC1 || leftImage.type() == CV_8UC3) && rightImage.type() == CV_8UC1);
-	UASSERT(type == CV_32FC1 || type == CV_16SC1);
 
 	cv::Mat leftMono;
 	if(leftImage.channels() == 3)
@@ -741,32 +741,9 @@ cv::Mat disparityFromStereoImages(
 		leftMono = leftImage;
 	}
 	cv::Mat disparity;
-#if CV_MAJOR_VERSION < 3
-	cv::StereoBM stereo(cv::StereoBM::BASIC_PRESET);
-	stereo.state->SADWindowSize = 15;
-	stereo.state->minDisparity = 0;
-	stereo.state->numberOfDisparities = 64;
-	stereo.state->preFilterSize = 9;
-	stereo.state->preFilterCap = 31;
-	stereo.state->uniquenessRatio = 15;
-	stereo.state->textureThreshold = 10;
-	stereo.state->speckleWindowSize = 100;
-	stereo.state->speckleRange = 4;
-	stereo(leftMono, rightImage, disparity, type);
-#else
-	cv::Ptr<cv::StereoBM> stereo = cv::StereoBM::create();
-	stereo->setBlockSize(15);
-	stereo->setMinDisparity(0);
-	stereo->setNumDisparities(64);
-	stereo->setPreFilterSize(9);
-	stereo->setPreFilterCap(31);
-	stereo->setUniquenessRatio(15);
-	stereo->setTextureThreshold(10);
-	stereo->setSpeckleWindowSize(100);
-	stereo->setSpeckleRange(4);
-	stereo->compute(leftMono, rightImage, disparity);
-#endif
-	return disparity;
+
+	StereoBM stereo(parameters);
+	return stereo.computeDisparity(leftMono, rightImage);
 }
 
 cv::Mat depthFromDisparity(const cv::Mat & disparity,

corelib/src/util3d.cpp

@@ -249,8 +249,8 @@ pcl::PointCloud<pcl::PointXYZ>::Ptr cloudFromDepth(
 		std::vector<int> * validIndices)
 {
 	UASSERT(!imageDepth.empty() && (imageDepth.type() == CV_16UC1 || imageDepth.type() == CV_32FC1));
-	UASSERT(imageDepth.rows % decimation == 0);
-	UASSERT(imageDepth.cols % decimation == 0);
+	UASSERT_MSG(imageDepth.rows % decimation == 0, uFormat("rows=%d decimation=%d", imageDepth.rows, decimation).c_str());
+	UASSERT_MSG(imageDepth.cols % decimation == 0, uFormat("cols=%d decimation=%d", imageDepth.cols, decimation).c_str());
 
 	pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>);
 	if(decimation < 1)
@@ -588,7 +588,8 @@ pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloudFromStereoImages(
 		int decimation,
 		float maxDepth,
 		float minDepth,
-		std::vector<int> * validIndices)
+		std::vector<int> * validIndices,
+		const ParametersMap & parameters)
 {
 	UASSERT(!imageLeft.empty() && !imageRight.empty());
 	UASSERT(imageRight.type() == CV_8UC1);
@@ -623,7 +624,7 @@ pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloudFromStereoImages(
 
 	return cloudFromDisparityRGB(
 			leftColor,
-			util2d::disparityFromStereoImages(leftMono, rightMono),
+			util2d::disparityFromStereoImages(leftMono, rightMono, parameters),
 			modelDecimation,
 			decimation,
 			maxDepth,
@@ -636,7 +637,8 @@ pcl::PointCloud<pcl::PointXYZ>::Ptr RTABMAP_EXP cloudFromSensorData(
 		int decimation,
 		float maxDepth,
 		float minDepth,
-		std::vector<int> * validIndices)
+		std::vector<int> * validIndices,
+		const ParametersMap & parameters)
 {
 	pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>);
 
@@ -696,7 +698,7 @@ pcl::PointCloud<pcl::PointXYZ>::Ptr RTABMAP_EXP cloudFromSensorData(
 			leftMono = sensorData.imageRaw();
 		}
 		cloud = cloudFromDisparity(
-				util2d::disparityFromStereoImages(leftMono, sensorData.rightRaw()),
+				util2d::disparityFromStereoImages(leftMono, sensorData.rightRaw(), parameters),
 				sensorData.stereoCameraModel(),
 				decimation,
 				maxDepth,
@@ -719,7 +721,8 @@ pcl::PointCloud<pcl::PointXYZRGB>::Ptr RTABMAP_EXP cloudRGBFromSensorData(
 		int decimation,
 		float maxDepth,
 		float minDepth,
-		std::vector<int> * validIndices)
+		std::vector<int> * validIndices,
+		const ParametersMap & parameters)
 {
 	UASSERT(!sensorData.imageRaw().empty());
 	UASSERT((!sensorData.depthRaw().empty() && sensorData.cameraModels().size()) ||
@@ -804,7 +807,8 @@ pcl::PointCloud<pcl::PointXYZRGB>::Ptr RTABMAP_EXP cloudRGBFromSensorData(
 				decimation,
 				maxDepth,
 				minDepth,
-				validIndices);
+				validIndices,
+				parameters);
 
 		if(cloud->size())
 		{

corelib/src/util3d_filtering.cpp

@@ -45,6 +45,18 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #include <rtabmap/utilite/UMath.h>
 #include <rtabmap/utilite/UConversion.h>
 
+#if PCL_VERSION_COMPARE(>=, 1, 8, 0)
+#include <pcl/impl/instantiate.hpp>
+#include <pcl/point_types.h>
+#include <pcl/segmentation/impl/extract_clusters.hpp>
+#include <pcl/segmentation/extract_labeled_clusters.h>
+#include <pcl/segmentation/impl/extract_labeled_clusters.hpp>
+
+PCL_INSTANTIATE(EuclideanClusterExtraction, (pcl::PointXYZRGBNormal))
+PCL_INSTANTIATE(extractEuclideanClusters, (pcl::PointXYZRGBNormal))
+PCL_INSTANTIATE(extractEuclideanClusters_indices, (pcl::PointXYZRGBNormal))
+#endif
+
 namespace rtabmap
 {
 
@@ -88,7 +100,7 @@ pcl::PointCloud<pcl::PointXYZ>::Ptr downsample(
 	}
 	else
 	{
-		int finalSize = cloud->size()/step;
+		int finalSize = int(cloud->size())/step;
 		output->resize(finalSize);
 		int oi = 0;
 		for(unsigned int i=0; i<cloud->size()-step+1; i+=step)
@@ -111,7 +123,7 @@ pcl::PointCloud<pcl::PointXYZRGB>::Ptr downsample(
 	}
 	else
 	{
-		int finalSize = cloud->size()/step;
+		int finalSize = int(cloud->size())/step;
 		output->resize(finalSize);
 		int oi = 0;
 		for(int i=0; i<(int)cloud->size()-step+1; i+=step)
@@ -244,6 +256,48 @@ pcl::PointCloud<pcl::PointXYZRGB>::Ptr randomSampling(
 	return output;
 }
 
+pcl::IndicesPtr passThrough(
+		const pcl::PointCloud<pcl::PointXYZ>::Ptr & cloud,
+		const pcl::IndicesPtr & indices,
+		const std::string & axis,
+		float min,
+		float max,
+		bool negative)
+{
+	UASSERT(max > min);
+	UASSERT(axis.compare("x") == 0 || axis.compare("y") == 0 || axis.compare("z") == 0);
+
+	pcl::IndicesPtr output(new std::vector<int>);
+	pcl::PassThrough<pcl::PointXYZ> filter;
+	filter.setNegative(negative);
+	filter.setFilterFieldName(axis);
+	filter.setFilterLimits(min, max);
+	filter.setInputCloud(cloud);
+	filter.setIndices(indices);
+	filter.filter(*output);
+	return output;
+}
+pcl::IndicesPtr passThrough(
+		const pcl::PointCloud<pcl::PointXYZRGB>::Ptr & cloud,
+		const pcl::IndicesPtr & indices,
+		const std::string & axis,
+		float min,
+		float max,
+		bool negative)
+{
+	UASSERT(max > min);
+	UASSERT(axis.compare("x") == 0 || axis.compare("y") == 0 || axis.compare("z") == 0);
+
+	pcl::IndicesPtr output(new std::vector<int>);
+	pcl::PassThrough<pcl::PointXYZRGB> filter;
+	filter.setNegative(negative);
+	filter.setFilterFieldName(axis);
+	filter.setFilterLimits(min, max);
+	filter.setInputCloud(cloud);
+	filter.setIndices(indices);
+	filter.filter(*output);
+	return output;
+}
 
 pcl::PointCloud<pcl::PointXYZ>::Ptr passThrough(
 		const pcl::PointCloud<pcl::PointXYZ>::Ptr & cloud,

corelib/src/util3d_surface.cpp

@@ -46,6 +46,16 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #include "pcl18/surface/organized_fast_mesh.h"
 #else
 #include <pcl/surface/organized_fast_mesh.h>
+#include <pcl/surface/impl/marching_cubes.hpp>
+#include <pcl/surface/impl/organized_fast_mesh.hpp>
+#include <pcl/impl/instantiate.hpp>
+#include <pcl/point_types.h>
+
+// Instantiations of specific point types
+PCL_INSTANTIATE(OrganizedFastMesh, (pcl::PointXYZRGBNormal))
+
+#include <pcl/features/impl/normal_3d_omp.hpp>
+PCL_INSTANTIATE_PRODUCT(NormalEstimationOMP, ((pcl::PointXYZRGB))((pcl::Normal)))
 #endif
 
 namespace rtabmap
@@ -177,10 +187,10 @@ void appendMesh(
 	UDEBUG("cloudA=%d polygonsA=%d cloudB=%d polygonsB=%d", (int)cloudA.size(), (int)polygonsA.size(), (int)cloudB.size(), (int)polygonsB.size());
 	UASSERT(!cloudA.isOrganized() && !cloudB.isOrganized());
 
-	int sizeA = cloudA.size();
+	int sizeA = (int)cloudA.size();
 	cloudA += cloudB;
 
-	int sizePolygonsA = polygonsA.size();
+	int sizePolygonsA = (int)polygonsA.size();
 	polygonsA.resize(sizePolygonsA+polygonsB.size());
 
 	for(unsigned int i=0; i<polygonsB.size(); ++i)
@@ -203,10 +213,10 @@ void appendMesh(
 	UDEBUG("cloudA=%d polygonsA=%d cloudB=%d polygonsB=%d", (int)cloudA.size(), (int)polygonsA.size(), (int)cloudB.size(), (int)polygonsB.size());
 	UASSERT(!cloudA.isOrganized() && !cloudB.isOrganized());
 
-	int sizeA = cloudA.size();
+	int sizeA = (int)cloudA.size();
 	cloudA += cloudB;
 
-	int sizePolygonsA = polygonsA.size();
+	int sizePolygonsA = (int)polygonsA.size();
 	polygonsA.resize(sizePolygonsA+polygonsB.size());
 
 	for(unsigned int i=0; i<polygonsB.size(); ++i)

guilib/include/rtabmap/gui/CameraViewer.h

@@ -33,6 +33,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #include <rtabmap/utilite/UEventsHandler.h>
 #include <QDialog>
 #include <rtabmap/core/SensorData.h>
+#include <rtabmap/core/Parameters.h>
 
 class QSpinBox;
 class QCheckBox;
@@ -47,7 +48,9 @@ class RTABMAPGUI_EXP CameraViewer : public QDialog, public UEventsHandler
 {
 	Q_OBJECT
 public:
-	CameraViewer(QWidget * parent = 0);
+	CameraViewer(
+		QWidget * parent = 0,
+		const ParametersMap & parameters = ParametersMap());
 	virtual ~CameraViewer();
 
 public slots:
@@ -61,6 +64,7 @@ private:
 	bool processingImages_;
 	QSpinBox * decimationSpin_;
 	int validDecimationValue_;
+	ParametersMap parameters_;
 	QPushButton * pause_;
 	QCheckBox * showCloudCheckbox_;
 	QCheckBox * showScanCheckbox_;

guilib/include/rtabmap/gui/CloudViewer.h

@@ -142,7 +142,8 @@ public:
 	void removeOccupancyGridMap();
 
 	void updateCameraTargetPosition(
-		const Transform & pose);
+		const Transform & pose,
+		const Transform & localTransform = Transform::getIdentity());
 
 	void addOrUpdateCoordinate(
 			const std::string & id,
@@ -154,6 +155,15 @@ public:
 	void removeCoordinate(const std::string & id);
 	void removeAllCoordinates();
 
+	void addOrUpdateLine(
+				const std::string & id,
+				const Transform & from,
+				const Transform & to,
+				const QColor & color,
+				bool arrow = false);
+	void removeLine(const std::string & id);
+	void removeAllLines();
+
 	void addOrUpdateFrustum(
 			const std::string & id,
 			const Transform & transform,
@@ -282,6 +292,7 @@ private:
     std::set<std::string> _graphes;
     std::set<std::string> _coordinates;
     std::set<std::string> _texts;
+    std::set<std::string> _lines;
     std::set<std::string> _frustums;
     pcl::PointCloud<pcl::PointXYZ>::Ptr _trajectory;
     unsigned int _maxTrajectorySize;

guilib/include/rtabmap/gui/LoopClosureViewer.h

@@ -57,7 +57,7 @@ public slots:
 	void setDecimation(int decimation) {decimation_ = decimation;}
 	void setMaxDepth(int maxDepth) {maxDepth_ = maxDepth;}
 	void setMinDepth(int minDepth) {minDepth_ = minDepth;}
-	void updateView(const Transform & AtoB = Transform());
+	void updateView(const Transform & AtoB = Transform(), const ParametersMap & parameters = ParametersMap());
 
 protected:
 	virtual void showEvent(QShowEvent * event);

guilib/include/rtabmap/gui/MainWindow.h

@@ -153,6 +153,8 @@ private slots:
 	void selectFreenect2();
 	void selectStereoDC1394();
 	void selectStereoFlyCapture2();
+	void selectStereoZed();
+	void selectStereoUsb();
 	void dumpTheMemory();
 	void dumpThePrediction();
 	void sendGoal();
@@ -229,7 +231,6 @@ private:
 	void update3DMapVisibility(bool cloudsShown, bool scansShown);
 	void updateMapCloud(
 			const std::map<int, Transform> & poses,
-			const Transform & pose,
 			const std::multimap<int, Link> & constraints,
 			const std::map<int, int> & mapIds,
 			const std::map<int, std::string> & labels,

guilib/include/rtabmap/gui/OdometryViewer.h

@@ -31,6 +31,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #include "rtabmap/gui/RtabmapGuiExp.h" // DLL export/import defines
 
 #include "rtabmap/core/OdometryEvent.h"
+#include "rtabmap/core/Parameters.h"
 #include <QDialog>
 #include "rtabmap/utilite/UEventsHandler.h"
 
@@ -49,7 +50,14 @@ class RTABMAPGUI_EXP OdometryViewer : public QDialog, public UEventsHandler
 	Q_OBJECT
 
 public:
-	OdometryViewer(int maxClouds = 10, int decimation = 2, float voxelSize = 0.0f, float maxDepth = 0, int qualityWarningThr=0, QWidget * parent = 0);
+	OdometryViewer(
+		int maxClouds = 10, 
+		int decimation = 2, 
+		float voxelSize = 0.0f, 
+		float maxDepth = 0, 
+		int qualityWarningThr=0, 
+		QWidget * parent = 0,
+		const ParametersMap & parameters = ParametersMap());
 	virtual ~OdometryViewer();
 
 public slots:
@@ -83,6 +91,7 @@ private:
 	QCheckBox * featuresShown_;
 	QLabel * timeLabel_;
 	int validDecimationValue_;
+	ParametersMap parameters_;
 };
 
 } /* namespace rtabmap */

guilib/include/rtabmap/gui/PreferencesDialog.h

@@ -96,6 +96,8 @@ public:
 		kSrcFlyCapture2    = 101,
 		kSrcStereoImages   = 102,
 		kSrcStereoVideo    = 103,
+		kSrcStereoZed      = 104,
+		kSrcStereoUsb      = 105,
 
 		kSrcRGB            = 200,
 		kSrcUsbDevice      = 200,

guilib/src/CalibrationDialog.cpp

@@ -722,8 +722,8 @@ void CalibrationDialog::calibrate()
 		P.at<double>(2,3) = 1;
 		K.copyTo(P.colRange(0,3).rowRange(0,3));
 
-		std::cout << "cameraMatrix = " << K << std::endl;
-		std::cout << "distCoeffs = " << D << std::endl;
+		std::cout << "K = " << K << std::endl;
+		std::cout << "D = " << D << std::endl;
 		std::cout << "width = " << imageSize_[id].width << std::endl;
 		std::cout << "height = " << imageSize_[id].height << std::endl;
 
@@ -738,8 +738,8 @@ void CalibrationDialog::calibrate()
 			ui_->label_error->setNum(totalAvgErr);
 
 			std::stringstream strK, strD, strR, strP;
-			strK << models_[id].K();
-			strD << models_[id].D();
+			strK << models_[id].K_raw();
+			strD << models_[id].D_raw();
 			strR << models_[id].R();
 			strP << models_[id].P();
 			ui_->lineEdit_K->setText(strK.str().c_str());
@@ -756,8 +756,8 @@ void CalibrationDialog::calibrate()
 			ui_->label_error_2->setNum(totalAvgErr);
 
 			std::stringstream strK, strD, strR, strP;
-			strK << models_[id].K();
-			strD << models_[id].D();
+			strK << models_[id].K_raw();
+			strD << models_[id].D_raw();
 			strR << models_[id].R();
 			strP << models_[id].P();
 			ui_->lineEdit_K_2->setText(strK.str().c_str());
@@ -806,6 +806,11 @@ void CalibrationDialog::calibrate()
 #endif
 		UINFO("stereo calibration... done with RMS error=%f", rms);
 
+		std::cout << "R = " << R << std::endl;
+		std::cout << "T = " << T << std::endl;
+		std::cout << "E = " << E << std::endl;
+		std::cout << "F = " << F << std::endl;
+
 		if(imageSize_[0] == imageSize_[1])
 		{
 			//Stereo, compute stereo rectification

guilib/src/CameraViewer.cpp

@@ -47,12 +47,13 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 namespace rtabmap {
 
 
-CameraViewer::CameraViewer(QWidget * parent) :
+CameraViewer::CameraViewer(QWidget * parent, const ParametersMap & parameters) :
 		QDialog(parent),
 	imageView_(new ImageView(this)),
 	cloudView_(new CloudViewer(this)),
 	processingImages_(false),
-	validDecimationValue_(1)
+	validDecimationValue_(1),
+	parameters_(parameters)
 {
 	qRegisterMetaType<rtabmap::SensorData>("rtabmap::SensorData");
 
@@ -146,12 +147,12 @@ void CameraViewer::showImage(const rtabmap::SensorData & data)
 			if(!data.imageRaw().empty() && !data.depthOrRightRaw().empty())
 			{
 				showCloudCheckbox_->setEnabled(true);
-				cloudView_->addCloud("cloud", util3d::cloudRGBFromSensorData(data, validDecimationValue_));
+				cloudView_->addCloud("cloud", util3d::cloudRGBFromSensorData(data, validDecimationValue_, 0, 0, 0, parameters_));
 			}
 			else if(!data.depthOrRightRaw().empty())
 			{
 				showCloudCheckbox_->setEnabled(true);
-				cloudView_->addCloud("cloud", util3d::cloudFromSensorData(data, validDecimationValue_));
+				cloudView_->addCloud("cloud", util3d::cloudFromSensorData(data, validDecimationValue_, 0, 0, 0, parameters_));
 			}
 		}
 	}

guilib/src/CloudViewer.cpp

@@ -172,6 +172,7 @@ void CloudViewer::clear()
 	this->removeAllClouds();
 	this->removeAllGraphs();
 	this->removeAllCoordinates();
+	this->removeAllLines();
 	this->removeAllFrustums();
 	this->removeAllTexts();
 	this->clearTrajectory();
@@ -792,6 +793,70 @@ void CloudViewer::removeAllCoordinates()
 	UASSERT(_coordinates.empty());
 }
 
+void CloudViewer::addOrUpdateLine(
+			const std::string & id,
+			const Transform & from,
+			const Transform & to,
+			const QColor & color,
+			bool arrow)
+{
+	if(id.empty())
+	{
+		UERROR("id should not be empty!");
+		return;
+	}
+
+	removeLine(id);
+
+	if(!from.isNull() && !to.isNull())
+	{
+		_lines.insert(id);
+
+		QColor c = Qt::gray;
+		if(color.isValid())
+		{
+			c = color;
+		}
+
+		pcl::PointXYZ pt1(from.x(), from.y(), from.z());
+		pcl::PointXYZ pt2(to.x(), to.y(), to.z());
+
+		if(arrow)
+		{
+			_visualizer->addArrow(pt2, pt1, c.redF(), c.greenF(), c.blueF(), false, id);
+		}
+		else
+		{
+			_visualizer->addLine(pt2, pt1, c.redF(), c.greenF(), c.blueF(), id);
+		}
+	}
+}
+
+void CloudViewer::removeLine(const std::string & id)
+{
+	if(id.empty())
+	{
+		UERROR("id should not be empty!");
+		return;
+	}
+
+	if(_lines.find(id) != _lines.end())
+	{
+		_visualizer->removeShape(id);
+		_lines.erase(id);
+	}
+}
+
+void CloudViewer::removeAllLines()
+{
+	std::set<std::string> arrows = _lines;
+	for(std::set<std::string>::iterator iter = arrows.begin(); iter!=arrows.end(); ++iter)
+	{
+		this->removeLine(*iter);
+	}
+	UASSERT(_lines.empty());
+}
+
 static const float frustum_vertices[] = {
     0.0f,  0.0f, 0.0f,
 	1.0f, 1.0f, 1.0f,
@@ -1055,6 +1120,7 @@ void CloudViewer::setFrustumShown(bool shown)
 	if(!shown)
 	{
 		this->removeFrustum("reference_frustum");
+		this->removeLine("reference_frustum_line");
 		this->update();
 	}
 	_aShowFrustum->setChecked(shown);
@@ -1074,8 +1140,8 @@ void CloudViewer::setFrustumColor(QColor value)
 	if(_frustums.find("reference_frustum") != _frustums.end())
 	{
 		_visualizer->setShapeRenderingProperties(pcl::visualization::PCL_VISUALIZER_COLOR, value.redF(), value.greenF(), value.blueF(), "reference_frustum");
-		this->update();
 	}
+	this->update();
 	_frustumColor = value;
 }
 
@@ -1189,7 +1255,7 @@ void CloudViewer::setCameraPosition(
 	_visualizer->setCameraPosition(x,y,z, focalX,focalY,focalX, upX,upY,upZ);
 }
 
-void CloudViewer::updateCameraTargetPosition(const Transform & pose)
+void CloudViewer::updateCameraTargetPosition(const Transform & pose, const Transform & localTransform)
 {
 	if(!pose.isNull())
 	{
@@ -1305,7 +1371,17 @@ void CloudViewer::updateCameraTargetPosition(const Transform & pose)
 			}
 			else */ if(_aShowFrustum->isChecked())
 			{
-				this->addOrUpdateFrustum("reference_frustum", pose, _frustumScale, _frustumColor);
+				Transform baseToCamera = Transform::getIdentity();
+				Transform opticalRot(0, 0, 1, 0, -1, 0, 0, 0, 0, -1, 0, 0);
+				if(!localTransform.isNull() && !localTransform.isIdentity())
+				{
+					baseToCamera = localTransform*opticalRot.inverse();
+				}
+				this->addOrUpdateFrustum("reference_frustum", pose * baseToCamera, _frustumScale, _frustumColor);
+				if(!baseToCamera.isIdentity())
+				{
+					this->addOrUpdateLine("reference_frustum_line", pose, pose * baseToCamera, _frustumColor);
+				}
 			}
 
 			vtkRenderer* renderer = _visualizer->getRendererCollection()->GetFirstRenderer();
@@ -1578,7 +1654,7 @@ void CloudViewer::keyReleaseEvent(QKeyEvent * event) {
 	}
 	else
 	{
-		QWidget::keyPressEvent(event);
+		QVTKWidget::keyPressEvent(event);
 	}
 }
 
@@ -1678,7 +1754,7 @@ void CloudViewer::keyPressEvent(QKeyEvent * event)
 	}
 	else
 	{
-		QWidget::keyPressEvent(event);
+		QVTKWidget::keyPressEvent(event);
 	}
 }
 

guilib/src/DatabaseViewer.cpp

@@ -1509,7 +1509,7 @@ void DatabaseViewer::view3DMap()
 						pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud;
 						UASSERT(data.imageRaw().empty() || data.imageRaw().type()==CV_8UC3 || data.imageRaw().type() == CV_8UC1);
 						UASSERT(data.depthOrRightRaw().empty() || data.depthOrRightRaw().type()==CV_8UC1 || data.depthOrRightRaw().type() == CV_16UC1 || data.depthOrRightRaw().type() == CV_32FC1);
-						cloud = util3d::cloudRGBFromSensorData(data, decimation, maxDepth);
+						cloud = util3d::cloudRGBFromSensorData(data, decimation, maxDepth, 0, 0, ui_->parameters_toolbox->getParameters());
 
 						if(cloud->size())
 						{
@@ -1733,7 +1733,7 @@ void DatabaseViewer::generate3DMap()
 							UASSERT(data.imageRaw().empty() || data.imageRaw().type()==CV_8UC3 || data.imageRaw().type() == CV_8UC1);
 							UASSERT(data.depthOrRightRaw().empty() || data.depthOrRightRaw().type()==CV_8UC1 || data.depthOrRightRaw().type() == CV_16UC1 || data.depthOrRightRaw().type() == CV_32FC1);
 							pcl::IndicesPtr validIndices(new std::vector<int>);
-							cloud = util3d::cloudRGBFromSensorData(data, decimation, maxDepth, 0, validIndices.get());
+							cloud = util3d::cloudRGBFromSensorData(data, decimation, maxDepth, 0, validIndices.get(), ui_->parameters_toolbox->getParameters());
 
 							if(assemble)
 							{
@@ -2275,7 +2275,7 @@ void DatabaseViewer::update(int value,
 							}
 							else
 							{
-								cloud = util3d::cloudRGBFromSensorData(data);
+								cloud = util3d::cloudRGBFromSensorData(data, 1, 0, 0, 0, ui_->parameters_toolbox->getParameters());
 							}
 							if(cloud->size())
 							{
@@ -2312,7 +2312,7 @@ void DatabaseViewer::update(int value,
 						else
 						{
 							pcl::PointCloud<pcl::PointXYZ>::Ptr cloud;
-							cloud = util3d::cloudFromSensorData(data);
+							cloud = util3d::cloudFromSensorData(data, 1, 0, 0, 0, ui_->parameters_toolbox->getParameters());
 							if(cloud->size())
 							{
 								view3D->addCloud("0", cloud);
@@ -2945,11 +2945,11 @@ void DatabaseViewer::updateConstraintView(
 				pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloudFrom, cloudTo;
 				if(!dataFrom.imageRaw().empty() && !dataFrom.depthOrRightRaw().empty())
 				{
-					cloudFrom=util3d::cloudRGBFromSensorData(dataFrom, 1);
+					cloudFrom=util3d::cloudRGBFromSensorData(dataFrom, 1, 0, 0, 0, ui_->parameters_toolbox->getParameters());
 				}
 				if(!dataTo.imageRaw().empty() && !dataTo.depthOrRightRaw().empty())
 				{
-					cloudTo=util3d::cloudRGBFromSensorData(dataTo, 1);
+					cloudTo=util3d::cloudRGBFromSensorData(dataTo, 1, 0, 0, 0, ui_->parameters_toolbox->getParameters());
 				}
 
 				if(cloudFrom.get() && cloudFrom->size())
@@ -3080,6 +3080,7 @@ void DatabaseViewer::updateConstraintView(
 					   memcmp(userData.data, "SCANS:", 6) == 0)
 					{
 						std::string scansStr = (const char *)userData.data;
+						UINFO("Detected \"%s\" in links's user data", scansStr.c_str());
 						if(!scansStr.empty())
 						{
 							std::list<std::string> strs = uSplit(scansStr, ':');
@@ -3128,6 +3129,22 @@ void DatabaseViewer::updateConstraintView(
 									posesOut,
 									linksOut);
 
+							if(poses.size() != posesOut.size())
+							{
+								UWARN("Scan poses input and output are different! %d vs %d", (int)poses.size(), (int)posesOut.size());
+								UWARN("Input poses: ");
+								for(std::map<int, Transform>::iterator iter=poses.begin(); iter!=poses.end(); ++iter)
+								{
+									UWARN(" %d", iter->first);
+								}
+								UWARN("Input links: ");
+								std::multimap<int, Link> modifiedLinks = updateLinksWithModifications(links_);
+								for(std::multimap<int, Link>::iterator iter=modifiedLinks.begin(); iter!=modifiedLinks.end(); ++iter)
+								{
+									UWARN(" %d->%d", iter->second.from(), iter->second.to());
+								}
+							}
+
 							QTime time;
 							time.start();
 							std::map<int, rtabmap::Transform> finalPoses = optimizer->optimize(link.to(), posesOut, linksOut);
@@ -3329,7 +3346,8 @@ void DatabaseViewer::sliderIterationsValueChanged(int value)
 									ui_->spinBox_projDecimation->value(),
 									ui_->doubleSpinBox_projMaxDepth->value(),
 									ui_->doubleSpinBox_projMinDepth->value(),
-									validIndices.get());
+									validIndices.get(),
+									ui_->parameters_toolbox->getParameters());
 							UASSERT(ui_->doubleSpinBox_gridCellSize->value() > 0);
 							cloud = util3d::voxelize(cloud, validIndices, ui_->doubleSpinBox_gridCellSize->value());
 
@@ -3776,12 +3794,16 @@ void DatabaseViewer::refineConstraint(int from, int to, bool silent, bool update
 				dataFrom,
 				ui_->spinBox_icp_decimation->value(),
 				ui_->doubleSpinBox_icp_maxDepth->value(),
-				ui_->doubleSpinBox_icp_minDepth->value());
+				ui_->doubleSpinBox_icp_minDepth->value(),
+				0,
+				ui_->parameters_toolbox->getParameters());
 		pcl::PointCloud<pcl::PointXYZ>::Ptr cloudTo = util3d::cloudFromSensorData(
 				dataTo,
 				ui_->spinBox_icp_decimation->value(),
 				ui_->doubleSpinBox_icp_maxDepth->value(),
-				ui_->doubleSpinBox_icp_minDepth->value());
+				ui_->doubleSpinBox_icp_minDepth->value(),
+				0,
+				ui_->parameters_toolbox->getParameters());
 		int maxLaserScans = cloudFrom->size();
 		dataFrom.setLaserScanRaw(util3d::laserScanFromPointCloud(*util3d::removeNaNFromPointCloud(cloudFrom), Transform()), maxLaserScans, 0);
 		dataTo.setLaserScanRaw(util3d::laserScanFromPointCloud(*util3d::removeNaNFromPointCloud(cloudTo), Transform()), maxLaserScans, 0);

guilib/src/ExportCloudsDialog.cpp

@@ -337,7 +337,8 @@ void ExportCloudsDialog::exportClouds(
 		const std::map<int, int> & mapIds,
 		const QMap<int, Signature> & cachedSignatures,
 		const std::map<int, std::pair<pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr, pcl::IndicesPtr> > & createdClouds,
-		const QString & workingDirectory)
+		const QString & workingDirectory,
+		const ParametersMap & parameters)
 {
 	std::map<int, pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr> clouds;
 	std::map<int, pcl::PolygonMesh::Ptr> meshes;
@@ -351,6 +352,7 @@ void ExportCloudsDialog::exportClouds(
 			cachedSignatures,
 			createdClouds,
 			workingDirectory,
+			parameters,
 			clouds,
 			meshes,
 			textureMeshes))
@@ -387,7 +389,8 @@ void ExportCloudsDialog::viewClouds(
 		const std::map<int, int> & mapIds,
 		const QMap<int, Signature> & cachedSignatures,
 		const std::map<int, std::pair<pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr, pcl::IndicesPtr> > & createdClouds,
-		const QString & workingDirectory)
+		const QString & workingDirectory,
+		const ParametersMap & parameters)
 {
 	std::map<int, pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr> clouds;
 	std::map<int, pcl::PolygonMesh::Ptr> meshes;
@@ -401,6 +404,7 @@ void ExportCloudsDialog::viewClouds(
 			cachedSignatures,
 			createdClouds,
 			workingDirectory,
+			parameters,
 			clouds,
 			meshes,
 			textureMeshes))
@@ -517,6 +521,7 @@ bool ExportCloudsDialog::getExportedClouds(
 		const QMap<int, Signature> & cachedSignatures,
 		const std::map<int, std::pair<pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr, pcl::IndicesPtr> > & createdClouds,
 		const QString & workingDirectory,
+		const ParametersMap & parameters,
 		std::map<int, pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr> & cloudsWithNormals,
 		std::map<int, pcl::PolygonMesh::Ptr> & meshes,
 		std::map<int, pcl::TextureMesh::Ptr> & textureMeshes)
@@ -550,7 +555,8 @@ bool ExportCloudsDialog::getExportedClouds(
 		std::map<int, std::pair<pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr, pcl::IndicesPtr> > clouds = this->getClouds(
 				poses,
 				cachedSignatures,
-				createdClouds);
+				createdClouds,
+				parameters);
 
 		pcl::PointCloud<pcl::PointXYZ>::Ptr rawAssembledCloud(new pcl::PointCloud<pcl::PointXYZ>);
 		std::vector<int> rawCameraIndices;
@@ -1006,7 +1012,8 @@ bool ExportCloudsDialog::getExportedClouds(
 std::map<int, std::pair<pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr, pcl::IndicesPtr> > ExportCloudsDialog::getClouds(
 		const std::map<int, Transform> & poses,
 		const QMap<int, Signature> & cachedSignatures,
-		const std::map<int, std::pair<pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr, pcl::IndicesPtr> > & createdClouds) const
+		const std::map<int, std::pair<pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr, pcl::IndicesPtr> > & createdClouds,
+		const ParametersMap & parameters) const
 {
 	std::map<int, std::pair<pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr, pcl::IndicesPtr> > clouds;
 	int i=0;
@@ -1038,7 +1045,8 @@ std::map<int, std::pair<pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr, pcl::Indic
 								_ui->spinBox_decimation->value(),
 								_ui->doubleSpinBox_maxDepth->value(),
 								_ui->doubleSpinBox_minDepth->value(),
-								indices.get());
+								indices.get(),
+								parameters);
 
 						// Don't voxelize if we create organized mesh
 						if(!(_ui->comboBox_pipeline->currentIndex()==0 && _ui->groupBox_meshing->isChecked()))

guilib/src/ExportCloudsDialog.h

@@ -33,6 +33,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #include <QtCore/QSettings>
 
 #include <rtabmap/core/Signature.h>
+#include <rtabmap/core/Parameters.h>
 
 #include <pcl/point_cloud.h>
 #include <pcl/point_types.h>
@@ -63,14 +64,16 @@ public:
 			const std::map<int, int> & mapIds,
 			const QMap<int, Signature> & cachedSignatures,
 			const std::map<int, std::pair<pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr, pcl::IndicesPtr> > & createdClouds,
-			const QString & workingDirectory);
+			const QString & workingDirectory,
+			const ParametersMap & parameters);
 
 	void viewClouds(
 			const std::map<int, Transform> & poses,
 			const std::map<int, int> & mapIds,
 			const QMap<int, Signature> & cachedSignatures,
 			const std::map<int, std::pair<pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr, pcl::IndicesPtr> > & createdClouds,
-			const QString & workingDirectory);
+			const QString & workingDirectory,
+			const ParametersMap & parameters);
 
 signals:
 	void configChanged();
@@ -86,13 +89,15 @@ private:
 	std::map<int, std::pair<pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr, pcl::IndicesPtr> > getClouds(
 			const std::map<int, Transform> & poses,
 			const QMap<int, Signature> & cachedSignatures,
-			const std::map<int, std::pair<pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr, pcl::IndicesPtr> > & createdClouds) const;
+			const std::map<int, std::pair<pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr, pcl::IndicesPtr> > & createdClouds,
+			const ParametersMap & parameters) const;
 	bool getExportedClouds(
 				const std::map<int, Transform> & poses,
 				const std::map<int, int> & mapIds,
 				const QMap<int, Signature> & cachedSignatures,
 				const std::map<int, std::pair<pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr, pcl::IndicesPtr> > & createdClouds,
 				const QString & workingDirectory,
+				const ParametersMap & parameters,
 				std::map<int, pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr> & clouds,
 				std::map<int, pcl::PolygonMesh::Ptr> & meshes,
 				std::map<int, pcl::TextureMesh::Ptr> & textureMeshes);

guilib/src/GuiLib.qrc

@@ -29,6 +29,7 @@
         <file>images/sense.png</file>
         <file>images/xtion_pro_live.png</file>
         <file>images/bumblebee2.png</file>
-	<file>images/webcam.png</file>
+	    <file>images/webcam.png</file>
+		<file>images/zed.png</file>
     </qresource>
 </RCC>

guilib/src/LoopClosureViewer.cpp

@@ -67,7 +67,7 @@ void LoopClosureViewer::setData(const Signature & sA, const Signature & sB)
 	}
 }
 
-void LoopClosureViewer::updateView(const Transform & transform)
+void LoopClosureViewer::updateView(const Transform & transform, const ParametersMap & parameters)
 {
 	if(sA_.id()>0 && sB_.id()>0)
 	{
@@ -106,8 +106,8 @@ void LoopClosureViewer::updateView(const Transform & transform)
 		{
 			//cloud 3d
 			pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloudA, cloudB;
-			cloudA = util3d::cloudRGBFromSensorData(sA_.sensorData(), decimation, maxDepth, minDepth);
-			cloudB = util3d::cloudRGBFromSensorData(sB_.sensorData(), decimation, maxDepth, minDepth);
+			cloudA = util3d::cloudRGBFromSensorData(sA_.sensorData(), decimation, maxDepth, minDepth, 0, parameters);
+			cloudB = util3d::cloudRGBFromSensorData(sB_.sensorData(), decimation, maxDepth, minDepth, 0, parameters);
 
 			//cloud 2d
 			pcl::PointCloud<pcl::PointXYZ>::Ptr scanA, scanB;

guilib/src/MainWindow.cpp

@@ -399,6 +399,8 @@ MainWindow::MainWindow(PreferencesDialog * prefDialog, QWidget * parent) :
 	connect(_ui->actionFreenect2, SIGNAL(triggered()), this, SLOT(selectFreenect2()));
 	connect(_ui->actionStereoDC1394, SIGNAL(triggered()), this, SLOT(selectStereoDC1394()));
 	connect(_ui->actionStereoFlyCapture2, SIGNAL(triggered()), this, SLOT(selectStereoFlyCapture2()));
+	connect(_ui->actionStereoZed, SIGNAL(triggered()), this, SLOT(selectStereoZed()));
+	connect(_ui->actionStereoUsb, SIGNAL(triggered()), this, SLOT(selectStereoUsb()));
 	_ui->actionFreenect->setEnabled(CameraFreenect::available());
 	_ui->actionOpenNI_CV->setEnabled(CameraOpenNICV::available());
 	_ui->actionOpenNI_CV_ASUS->setEnabled(CameraOpenNICV::available());
@@ -408,6 +410,7 @@ MainWindow::MainWindow(PreferencesDialog * prefDialog, QWidget * parent) :
 	_ui->actionFreenect2->setEnabled(CameraFreenect2::available());
 	_ui->actionStereoDC1394->setEnabled(CameraStereoDC1394::available());
 	_ui->actionStereoFlyCapture2->setEnabled(CameraStereoFlyCapture2::available());
+	_ui->actionStereoZed->setEnabled(CameraStereoZed::available());
 	this->updateSelectSourceMenu();
 
 	connect(_ui->actionPreferences, SIGNAL(triggered()), this, SLOT(openPreferences()));
@@ -775,6 +778,8 @@ void MainWindow::handleEvent(UEvent* anEvent)
 		{
 			// we receive too many odometry events! just send without data
 			SensorData data(cv::Mat(), odomEvent->data().id(), odomEvent->data().stamp());
+			data.setCameraModels(odomEvent->data().cameraModels());
+			data.setStereoCameraModel(odomEvent->data().stereoCameraModel());
 			data.setGroundTruth(odomEvent->data().groundTruth());
 			OdometryEvent tmp(data, odomEvent->pose(), odomEvent->covariance(), odomEvent->info().copyWithoutData());
 			emit odometryReceived(tmp);
@@ -882,7 +887,8 @@ void MainWindow::processOdometry(const rtabmap::OdometryEvent & odom)
 						_preferencesDialog->getCloudDecimation(1),
 						_preferencesDialog->getCloudMaxDepth(1),
 						_preferencesDialog->getCloudMinDepth(1),
-						indices.get());
+						indices.get(),
+						_preferencesDialog->getAllParameters());
 				if(indices->size())
 				{
 					cloud = util3d::transformPointCloud(cloud, pose);
@@ -1036,7 +1042,17 @@ void MainWindow::processOdometry(const rtabmap::OdometryEvent & odom)
 	if(!odom.pose().isNull())
 	{
 		// update camera position
-		_cloudViewer->updateCameraTargetPosition(_odometryCorrection*odom.pose());
+		Transform localTransform;
+		if(odom.data().cameraModels().size() && !odom.data().cameraModels()[0].localTransform().isNull())
+		{
+			localTransform = odom.data().cameraModels()[0].localTransform();
+		}
+		else if(!odom.data().stereoCameraModel().localTransform().isNull())
+		{
+			localTransform = odom.data().stereoCameraModel().localTransform();
+		}
+
+		_cloudViewer->updateCameraTargetPosition(_odometryCorrection*odom.pose(), localTransform);
 	}
 	_cloudViewer->update();
 
@@ -1527,9 +1543,30 @@ void MainWindow::processStats(const rtabmap::Statistics & stat)
 			Transform groundTruthOffset = alignPosesToGroundTruth(poses, groundTruth);
 			UDEBUG("time= %d ms", time.restart());
 
+			if(!_odometryReceived && poses.size())
+			{
+				Transform localTransform = Transform::getIdentity();
+				std::map<int, Signature>::const_iterator iter = stat.getSignatures().find(poses.rbegin()->first);
+				if(iter != stat.getSignatures().end())
+				{
+					if(iter->second.sensorData().cameraModels().size() && !iter->second.sensorData().cameraModels()[0].localTransform().isNull())
+					{
+						localTransform = iter->second.sensorData().cameraModels()[0].localTransform();
+					}
+					else if(!iter->second.sensorData().stereoCameraModel().localTransform().isNull())
+					{
+						localTransform = iter->second.sensorData().stereoCameraModel().localTransform();
+					}
+				}
+				_cloudViewer->updateCameraTargetPosition(poses.rbegin()->second, localTransform);
+				if(_ui->graphicsView_graphView->isVisible())
+				{
+					_ui->graphicsView_graphView->updateReferentialPosition(poses.rbegin()->second);
+				}
+			}
+
 			updateMapCloud(
 					poses,
-					_odometryReceived||poses.size()==0?Transform():poses.rbegin()->second,
 					stat.constraints(),
 					mapIds,
 					labels,
@@ -1554,7 +1591,7 @@ void MainWindow::processStats(const rtabmap::Statistics & stat)
 				if(_ui->dockWidget_loopClosureViewer->isVisible())
 				{
 					UTimer loopTimer;
-					_loopClosureViewer->updateView();
+					_loopClosureViewer->updateView(Transform(), _preferencesDialog->getAllParameters());
 					UINFO("Updating loop closure cloud view time=%fs", loopTimer.elapsed());
 					_ui->statsToolBox->updateStat("GUI/RGB-D closure view/ms", stat.refImageId(), int(loopTimer.elapsed()*1000.0f));
 				}
@@ -1725,15 +1762,14 @@ void MainWindow::processStats(const rtabmap::Statistics & stat)
 
 void MainWindow::updateMapCloud(
 		const std::map<int, Transform> & posesIn,
-		const Transform & currentPose,
 		const std::multimap<int, Link> & constraints,
 		const std::map<int, int> & mapIdsIn,
 		const std::map<int, std::string> & labels,
 		const std::map<int, Transform> & groundTruths, // ground truth should contain only valid transforms
 		bool verboseProgress)
 {
-	UDEBUG("posesIn=%d constraints=%d mapIdsIn=%d labelsIn=%d currentPose=%s",
-			(int)posesIn.size(), (int)constraints.size(), (int)mapIdsIn.size(), (int)labels.size(), currentPose.prettyPrint().c_str());
+	UDEBUG("posesIn=%d constraints=%d mapIdsIn=%d labelsIn=%d",
+			(int)posesIn.size(), (int)constraints.size(), (int)mapIdsIn.size(), (int)labels.size());
 	if(posesIn.size())
 	{
 		_currentPosesMap = posesIn;
@@ -1810,7 +1846,7 @@ void MainWindow::updateMapCloud(
 	}
 
 	// Map updated! regenerate the assembled cloud, last pose is the new one
-	UDEBUG("Update map with %d locations (currentPose=%s)", poses.size(), currentPose.prettyPrint().c_str());
+	UDEBUG("Update map with %d locations", poses.size());
 	QMap<std::string, Transform> viewerClouds = _cloudViewer->getAddedClouds();
 	int i=1;
 	for(std::map<int, Transform>::const_iterator iter = poses.begin(); iter!=poses.end(); ++iter)
@@ -1835,13 +1871,14 @@ void MainWindow::updateMapCloud(
 							UERROR("Updating pose cloud %d failed!", iter->first);
 						}
 					}
-					_cloudViewer->setCloudVisibility(cloudName, true);
+					_cloudViewer->setCloudVisibility(cloudName, (_cloudViewer->isVisible() && _preferencesDialog->isCloudsShown(0)));
 					_cloudViewer->setCloudOpacity(cloudName, _preferencesDialog->getCloudOpacity(0));
 					_cloudViewer->setCloudPointSize(cloudName, _preferencesDialog->getCloudPointSize(0));
 				}
 				else if(_cachedSignatures.contains(iter->first))
 				{
 					this->createAndAddCloudToMap(iter->first, iter->second, uValue(mapIds, iter->first, -1));
+					_cloudViewer->setCloudVisibility(cloudName.c_str(), _cloudViewer->isVisible() && _preferencesDialog->isCloudsShown(0));
 				}
 			}
 			else if(viewerClouds.contains(cloudName))
@@ -2040,11 +2077,6 @@ void MainWindow::updateMapCloud(
 	if(_ui->graphicsView_graphView->isVisible())
 	{
 		_ui->graphicsView_graphView->updateGraph(posesIn, constraints, mapIdsIn);
-		if(!currentPose.isNull())
-		{
-			_ui->graphicsView_graphView->updateReferentialPosition(currentPose);
-		}
-
 		_ui->graphicsView_graphView->updateGTGraph(_currentGTPosesMap);
 	}
 	cv::Mat map8U;
@@ -2147,12 +2179,6 @@ void MainWindow::updateMapCloud(
 		}
 	}
 
-	if(!currentPose.isNull())
-	{
-		UDEBUG("");
-		_cloudViewer->updateCameraTargetPosition(currentPose);
-	}
-
 	UDEBUG("");
 	_cloudViewer->update();
 	UDEBUG("");
@@ -2197,7 +2223,8 @@ void MainWindow::createAndAddCloudToMap(int nodeId, const Transform & pose, int
 				_preferencesDialog->getCloudDecimation(0),
 				_preferencesDialog->getCloudMaxDepth(0),
 				_preferencesDialog->getCloudMinDepth(0),
-				indices.get());
+				indices.get(),
+				_preferencesDialog->getAllParameters());
 
 		//compute normals
 		pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr cloud = util3d::computeNormals(cloudWithoutNormals, 10);
@@ -2209,14 +2236,14 @@ void MainWindow::createAndAddCloudToMap(int nodeId, const Transform & pose, int
 			float groundNormalMaxAngle = M_PI_4;
 			int minClusterSize = 20;
 			cv::Mat ground, obstacles;
-			pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr voxelCloud = util3d::voxelize(cloud, indices, cellSize);
+			pcl::PointCloud<pcl::PointXYZRGB>::Ptr voxelCloud = util3d::voxelize(cloudWithoutNormals, indices, cellSize);
 
 			// add pose rotation without yaw
 			float roll, pitch, yaw;
 			pose.getEulerAngles(roll, pitch, yaw);
 			voxelCloud = util3d::transformPointCloud(voxelCloud, Transform(0,0,0, roll, pitch, 0));
 
-			util3d::occupancy2DFromCloud3D<pcl::PointXYZRGBNormal>(
+			util3d::occupancy2DFromCloud3D<pcl::PointXYZRGB>(
 					voxelCloud,
 					ground,
 					obstacles,
@@ -2538,7 +2565,7 @@ Transform MainWindow::alignPosesToGroundTruth(
 	Transform t = Transform::getIdentity();
 	if(groundTruth.size() && poses.size())
 	{
-		unsigned int maxSize = poses.size()>groundTruth.size()?poses.size():groundTruth.size();
+		unsigned int maxSize = poses.size()>groundTruth.size()? (unsigned int)poses.size(): (unsigned int)groundTruth.size();
 		pcl::PointCloud<pcl::PointXYZ> cloud1, cloud2;
 		cloud1.resize(maxSize);
 		cloud2.resize(maxSize);
@@ -2813,7 +2840,7 @@ void MainWindow::processRtabmapEvent3DMap(const rtabmap::RtabmapEvent3DMap & eve
 			QApplication::processEvents();
 			std::map<int, Transform> poses = event.getPoses();
 			alignPosesToGroundTruth(poses, groundTruth);
-			this->updateMapCloud(poses, Transform(), event.getConstraints(), mapIds, labels, groundTruth, true);
+			this->updateMapCloud(poses, event.getConstraints(), mapIds, labels, groundTruth, true);
 			_initProgressDialog->appendText("Updating the 3D map cloud... done.");
 		}
 		else
@@ -2957,7 +2984,6 @@ void MainWindow::applyPrefSettings(PreferencesDialog::PANEL_FLAGS flags)
 		{
 			this->updateMapCloud(
 					std::map<int, Transform>(_currentPosesMap),
-					Transform(),
 					std::multimap<int, Link>(_currentLinksMap),
 					std::map<int, int>(_currentMapIds),
 					std::map<int, std::string>(_currentLabels),
@@ -3219,7 +3245,10 @@ void MainWindow::updateSelectSourceMenu()
 			_preferencesDialog->getSourceDriver() == PreferencesDialog::kSrcDatabase ||
 			_preferencesDialog->getSourceDriver() == PreferencesDialog::kSrcImages ||
 			_preferencesDialog->getSourceDriver() == PreferencesDialog::kSrcVideo ||
-			_preferencesDialog->getSourceDriver() == PreferencesDialog::kSrcStereoImages);
+			_preferencesDialog->getSourceDriver() == PreferencesDialog::kSrcStereoImages ||
+			_preferencesDialog->getSourceDriver() == PreferencesDialog::kSrcStereoVideo ||
+			_preferencesDialog->getSourceDriver() == PreferencesDialog::kSrcRGBDImages
+	);
 
 	_ui->actionOpenNI_PCL->setChecked(_preferencesDialog->getSourceDriver() == PreferencesDialog::kSrcOpenNI_PCL);
 	_ui->actionOpenNI_PCL_ASUS->setChecked(_preferencesDialog->getSourceDriver() == PreferencesDialog::kSrcOpenNI_PCL);
@@ -3232,6 +3261,8 @@ void MainWindow::updateSelectSourceMenu()
 	_ui->actionFreenect2->setChecked(_preferencesDialog->getSourceDriver() == PreferencesDialog::kSrcFreenect2);
 	_ui->actionStereoDC1394->setChecked(_preferencesDialog->getSourceDriver() == PreferencesDialog::kSrcDC1394);
 	_ui->actionStereoFlyCapture2->setChecked(_preferencesDialog->getSourceDriver() == PreferencesDialog::kSrcFlyCapture2);
+	_ui->actionStereoZed->setChecked(_preferencesDialog->getSourceDriver() == PreferencesDialog::kSrcStereoZed);
+	_ui->actionStereoUsb->setChecked(_preferencesDialog->getSourceDriver() == PreferencesDialog::kSrcStereoUsb);
 }
 
 void MainWindow::changeImgRateSetting()
@@ -4289,7 +4320,6 @@ void MainWindow::postProcessing()
 	alignPosesToGroundTruth(optimizedPoses, _currentGTPosesMap);
 	this->updateMapCloud(
 			optimizedPoses,
-			Transform(),
 			std::multimap<int, Link>(_currentLinksMap),
 			std::map<int, int>(_currentMapIds),
 			std::map<int, std::string>(_currentLabels),
@@ -4429,7 +4459,15 @@ void MainWindow::selectStereoFlyCapture2()
 {
 	_preferencesDialog->selectSourceDriver(PreferencesDialog::kSrcFlyCapture2);
 }
+void MainWindow::selectStereoZed()
+{
+	_preferencesDialog->selectSourceDriver(PreferencesDialog::kSrcStereoZed);
+}
 
+void MainWindow::selectStereoUsb()
+{
+	_preferencesDialog->selectSourceDriver(PreferencesDialog::kSrcStereoUsb);
+}
 
 void MainWindow::dumpTheMemory()
 {
@@ -4652,7 +4690,6 @@ void MainWindow::anchorCloudsToGroundTruth()
 {
 	this->updateMapCloud(
 			std::map<int, Transform>(_currentPosesMap),
-			Transform(),
 			std::multimap<int, Link>(_currentLinksMap),
 			std::map<int, int>(_currentMapIds),
 			std::map<int, std::string>(_currentLabels),
@@ -5070,7 +5107,8 @@ void MainWindow::exportClouds()
 			_currentMapIds,
 			_cachedSignatures,
 			_createdClouds,
-			_preferencesDialog->getWorkingDirectory());
+			_preferencesDialog->getWorkingDirectory(),
+			_preferencesDialog->getAllParameters());
 }
 
 void MainWindow::viewClouds()
@@ -5085,7 +5123,8 @@ void MainWindow::viewClouds()
 			_currentMapIds,
 			_cachedSignatures,
 			_createdClouds,
-			_preferencesDialog->getWorkingDirectory());
+			_preferencesDialog->getWorkingDirectory(),
+			_preferencesDialog->getAllParameters());
 
 }
 

guilib/src/OdometryViewer.cpp

@@ -49,7 +49,14 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 
 namespace rtabmap {
 
-OdometryViewer::OdometryViewer(int maxClouds, int decimation, float voxelSize, float maxDepth, int qualityWarningThr, QWidget * parent) :
+OdometryViewer::OdometryViewer(
+	int maxClouds, 
+	int decimation, 
+	float voxelSize, 
+	float maxDepth, 
+	int qualityWarningThr, 
+	QWidget * parent,
+	const ParametersMap & parameters) :
 		QDialog(parent),
 		imageView_(new ImageView(this)),
 		cloudView_(new CloudViewer(this)),
@@ -59,7 +66,8 @@ OdometryViewer::OdometryViewer(int maxClouds, int decimation, float voxelSize, f
 		lastOdomPose_(Transform::getIdentity()),
 		qualityWarningThr_(qualityWarningThr),
 		id_(0),
-		validDecimationValue_(1)
+		validDecimationValue_(1),
+		parameters_(parameters)
 {
 
 	qRegisterMetaType<rtabmap::OdometryEvent>("rtabmap::OdometryEvent");
@@ -241,7 +249,8 @@ void OdometryViewer::processData(const rtabmap::OdometryEvent & odom)
 				validDecimationValue_,
 				0,
 				0,
-				validIndices.get());
+				validIndices.get(),
+				parameters_);
 
 		if(voxelSpin_->value())
 		{

guilib/src/PreferencesDialog.cpp

@@ -237,6 +237,11 @@ PreferencesDialog::PreferencesDialog(QWidget * parent) :
 	{
 		_ui->comboBox_cameraStereo->setItemData(1, 0, Qt::UserRole - 1);
 	}
+	if (!CameraStereoZed::available())
+	{
+		_ui->comboBox_cameraRGBD->setItemData(7, 0, Qt::UserRole - 1);
+		_ui->comboBox_cameraStereo->setItemData(4, 0, Qt::UserRole - 1);
+	}
 	_ui->openni2_exposure->setEnabled(CameraOpenNI2::exposureGainAvailable());
 	_ui->openni2_gain->setEnabled(CameraOpenNI2::exposureGainAvailable());
 
@@ -466,6 +471,8 @@ PreferencesDialog::PreferencesDialog(QWidget * parent) :
 	connect(_ui->lineEdit_cameraStereoVideo_path, SIGNAL(textChanged(const QString &)), this, SLOT(makeObsoleteSourcePanel()));
 	connect(_ui->checkBox_stereoVideo_rectify, SIGNAL(stateChanged(int)), this, SLOT(makeObsoleteSourcePanel()));
 
+	connect(_ui->checkBox_stereoZed_computeDisparity, SIGNAL(stateChanged(int)), this, SLOT(makeObsoleteSourcePanel()));
+
 	connect(_ui->checkbox_rgbd_colorOnly, SIGNAL(stateChanged(int)), this, SLOT(makeObsoleteSourcePanel()));
 	connect(_ui->spinBox_source_imageDecimation, SIGNAL(valueChanged(int)), this, SLOT(makeObsoleteSourcePanel()));
 	connect(_ui->checkbox_stereo_depthGenerated, SIGNAL(stateChanged(int)), this, SLOT(makeObsoleteSourcePanel()));
@@ -684,7 +691,6 @@ PreferencesDialog::PreferencesDialog(QWidget * parent) :
 	_ui->localDetection_pathFilteringRadius->setObjectName(Parameters::kRGBDProximityPathFilteringRadius().c_str());
 	_ui->localDetection_angle->setObjectName(Parameters::kRGBDProximityAngle().c_str());
 	_ui->checkBox_localSpacePathOdomPosesUsed->setObjectName(Parameters::kRGBDProximityPathRawPosesUsed().c_str());
-	_ui->checkBox_localSpaceAssembleScans->setObjectName(Parameters::kRGBDProximityPathScansMerged().c_str());
 	_ui->rgdb_localImmunizationRatio->setObjectName(Parameters::kRGBDLocalImmunizationRatio().c_str());
 	_ui->loopClosure_reextract->setObjectName(Parameters::kRGBDLoopClosureReextractFeatures().c_str());
 
@@ -1142,7 +1148,7 @@ void PreferencesDialog::resetSettings(QGroupBox * groupBox)
 		for(int i=0; i<2; ++i)
 		{
 			_3dRenderingShowClouds[i]->setChecked(true);
-			_3dRenderingDecimation[i]->setValue(8);
+			_3dRenderingDecimation[i]->setValue(4);
 			_3dRenderingMaxDepth[i]->setValue(0.0);
 			_3dRenderingMinDepth[i]->setValue(0.0);
 			_3dRenderingShowScans[i]->setChecked(true);
@@ -1272,6 +1278,7 @@ void PreferencesDialog::resetSettings(QGroupBox * groupBox)
 		_ui->checkBox_stereoImages_rectify->setChecked(false);
 		_ui->lineEdit_cameraStereoVideo_path->setText("");
 		_ui->checkBox_stereoVideo_rectify->setChecked(false);
+		_ui->checkBox_stereoZed_computeDisparity->setChecked(true);
 
 		_ui->checkBox_cameraImages_timestamps->setChecked(false);
 		_ui->checkBox_cameraImages_syncTimeStamps->setChecked(true);
@@ -1596,6 +1603,11 @@ void PreferencesDialog::readCameraSettings(const QString & filePath)
 	_ui->checkBox_stereoVideo_rectify->setChecked(settings.value("rectify",_ui->checkBox_stereoVideo_rectify->isChecked()).toBool());
 	settings.endGroup(); // StereoVideo
 
+	settings.beginGroup("StereoZed");
+	_ui->checkBox_stereoZed_computeDisparity->setChecked(settings.value("compute_disp", _ui->checkBox_stereoZed_computeDisparity->isChecked()).toBool());
+	settings.endGroup(); // StereoZed
+	
+
 	settings.beginGroup("Images");
 	_ui->source_images_lineEdit_path->setText(settings.value("path", _ui->source_images_lineEdit_path->text()).toString());
 	_ui->source_images_spinBox_startPos->setValue(settings.value("startPos",_ui->source_images_spinBox_startPos->value()).toInt());
@@ -1980,6 +1992,12 @@ void PreferencesDialog::writeCameraSettings(const QString & filePath) const
 	settings.setValue("rectify", 	    _ui->checkBox_stereoVideo_rectify->isChecked());
 	settings.endGroup(); // StereoVideo
 
+	settings.beginGroup("StereoZed");
+	settings.setValue("compute_disp", _ui->checkBox_stereoZed_computeDisparity->isChecked());
+	settings.endGroup(); // StereoZed
+
+	
+
 	settings.beginGroup("Images");
 	settings.setValue("path", 			_ui->source_images_lineEdit_path->text());
 	settings.setValue("startPos", 		_ui->source_images_spinBox_startPos->value());
@@ -2557,17 +2575,16 @@ void PreferencesDialog::selectSourceDriver(Src src)
 	else if(src >= kSrcStereo && src<kSrcRGB)
 	{
 		_ui->comboBox_sourceType->setCurrentIndex(1);
-		_ui->comboBox_cameraRGBD->setCurrentIndex(src - kSrcStereo);
+		_ui->comboBox_cameraStereo->setCurrentIndex(src - kSrcStereo);
 	}
 	else if(src >= kSrcRGB && src<kSrcDatabase)
 	{
 		_ui->comboBox_sourceType->setCurrentIndex(2);
-		_ui->comboBox_cameraRGBD->setCurrentIndex(src - kSrcRGB);
+		_ui->source_comboBox_image_type->setCurrentIndex(src - kSrcRGB);
 	}
 	else if(src >= kSrcDatabase)
 	{
 		_ui->comboBox_sourceType->setCurrentIndex(3);
-		_ui->comboBox_cameraRGBD->setCurrentIndex(src - kSrcDatabase);
 	}
 
 	if(validateForm())
@@ -3428,8 +3445,13 @@ void PreferencesDialog::updateSourceGrpVisibility()
 	_ui->groupBox_cameraRGBDImages->setVisible(_ui->comboBox_sourceType->currentIndex() == 0 && _ui->comboBox_cameraRGBD->currentIndex() == kSrcRGBDImages-kSrcRGBD);
 	_ui->groupBox_openni->setVisible(_ui->comboBox_sourceType->currentIndex() == 0 && _ui->comboBox_cameraRGBD->currentIndex() == kSrcOpenNI_PCL-kSrcRGBD);
 
-	_ui->stackedWidget_stereo->setVisible(_ui->comboBox_sourceType->currentIndex() == 1 && (_ui->comboBox_cameraStereo->currentIndex() == kSrcStereoVideo-kSrcStereo || _ui->comboBox_cameraStereo->currentIndex() == kSrcStereoImages-kSrcStereo));
+	_ui->stackedWidget_stereo->setVisible(_ui->comboBox_sourceType->currentIndex() == 1 && 
+		(_ui->comboBox_cameraStereo->currentIndex() == kSrcStereoVideo-kSrcStereo || 
+		 _ui->comboBox_cameraStereo->currentIndex() == kSrcStereoImages-kSrcStereo ||
+		 _ui->comboBox_cameraStereo->currentIndex() == kSrcStereoZed - kSrcStereo));
 	_ui->groupBox_cameraStereoImages->setVisible(_ui->comboBox_sourceType->currentIndex() == 1 && _ui->comboBox_cameraStereo->currentIndex() == kSrcStereoImages-kSrcStereo);
+	_ui->groupBox_cameraStereoVideo->setVisible(_ui->comboBox_sourceType->currentIndex() == 1 && _ui->comboBox_cameraStereo->currentIndex() == kSrcStereoVideo - kSrcStereo);
+	_ui->groupBox_cameraStereoZed->setVisible(_ui->comboBox_sourceType->currentIndex() == 1 && _ui->comboBox_cameraStereo->currentIndex() == kSrcStereoZed - kSrcStereo);
 
 	_ui->stackedWidget_image->setVisible(_ui->comboBox_sourceType->currentIndex() == 2 && (_ui->source_comboBox_image_type->currentIndex() == kSrcImages-kSrcRGB || _ui->source_comboBox_image_type->currentIndex() == kSrcVideo-kSrcRGB));
 	_ui->source_groupBox_images->setVisible(_ui->comboBox_sourceType->currentIndex() == 2 && _ui->source_comboBox_image_type->currentIndex() == kSrcImages-kSrcRGB);
@@ -3949,6 +3971,13 @@ Camera * PreferencesDialog::createCamera(bool useRawImages)
 				_ui->lineEdit_cameraImages_timestamps->text().toStdString(),
 				_ui->checkBox_cameraImages_syncTimeStamps->isChecked());
 	}
+	else if (driver == kSrcStereoUsb)
+	{
+		camera = new CameraStereoVideo(
+			this->getSourceDevice().isEmpty() ? 0 : atoi(this->getSourceDevice().toStdString().c_str()),
+			this->getGeneralInputRate(),
+			this->getSourceLocalTransform());
+	}
 	else if(driver == kSrcStereoVideo)
 	{
 		camera = new CameraStereoVideo(
@@ -3957,6 +3986,13 @@ Camera * PreferencesDialog::createCamera(bool useRawImages)
 			this->getGeneralInputRate(),
 			this->getSourceLocalTransform());
 	}
+	else if (driver == kSrcStereoZed)
+	{
+		camera = new CameraStereoZed(
+			_ui->checkBox_stereoZed_computeDisparity->isChecked(),
+			this->getGeneralInputRate(),
+			this->getSourceLocalTransform());
+	}
 	else if(driver == kSrcUsbDevice)
 	{
 		camera = new CameraVideo(
@@ -4220,7 +4256,8 @@ void PreferencesDialog::testOdometry()
 					0.0f,
 					_ui->doubleSpinBox_maxDepth_odom->value(),
 					this->getOdomQualityWarnThr(),
-					this);
+					this,
+					this->getAllParameters());
 	odomViewer->setWindowTitle(tr("Odometry viewer"));
 	odomViewer->resize(1280, 480+QPushButton().minimumHeight());
 	odomViewer->registerToEventsManager();
@@ -4270,7 +4307,7 @@ void PreferencesDialog::testOdometry()
 
 void PreferencesDialog::testCamera()
 {
-	CameraViewer * window = new CameraViewer(this);
+	CameraViewer * window = new CameraViewer(this, this->getAllParameters());
 	window->setWindowTitle(tr("Camera viewer"));
 	window->resize(1280, 480+QPushButton().minimumHeight());
 	window->registerToEventsManager();

guilib/src/ui/mainWindow.ui

@@ -7,7 +7,7 @@
     <x>0</x>
     <y>0</y>
     <width>1012</width>
-    <height>711</height>
+    <height>712</height>
    </rect>
   </property>
   <property name="windowTitle">
@@ -27,7 +27,7 @@
      <x>0</x>
      <y>0</y>
      <width>1012</width>
-     <height>25</height>
+     <height>21</height>
     </rect>
    </property>
    <widget class="QMenu" name="menuFile">
@@ -190,7 +190,19 @@
        <addaction name="actionStereoDC1394"/>
        <addaction name="actionStereoFlyCapture2"/>
       </widget>
+      <widget class="QMenu" name="menuZed_camera">
+       <property name="title">
+        <string>Zed camera</string>
+       </property>
+       <property name="icon">
+        <iconset resource="../GuiLib.qrc">
+         <normaloff>:/images/zed.png</normaloff>:/images/zed.png</iconset>
+       </property>
+       <addaction name="actionStereoZed"/>
+       <addaction name="actionStereoUsb"/>
+      </widget>
       <addaction name="menuBumblebee2_2"/>
+      <addaction name="menuZed_camera"/>
      </widget>
      <addaction name="menuRGB_D_camera"/>
      <addaction name="menuStereo_camera"/>
@@ -1325,7 +1337,7 @@
     <bool>true</bool>
    </property>
    <property name="text">
-    <string>More options...</string>
+    <string>More Options...</string>
    </property>
   </action>
   <action name="actionLabel_current_location">
@@ -1391,6 +1403,22 @@
     <string>Anchor clouds to ground truth</string>
    </property>
   </action>
+  <action name="actionStereoZed">
+   <property name="checkable">
+    <bool>true</bool>
+   </property>
+   <property name="text">
+    <string>Zed sdk</string>
+   </property>
+  </action>
+  <action name="actionStereoUsb">
+   <property name="checkable">
+    <bool>true</bool>
+   </property>
+   <property name="text">
+    <string>Stereo Usb Camera</string>
+   </property>
+  </action>
  </widget>
  <customwidgets>
   <customwidget>

guilib/src/ui/preferencesDialog.ui

@@ -63,16 +63,25 @@
           <property name="geometry">
            <rect>
             <x>0</x>
-            <y>-466</y>
-            <width>686</width>
-            <height>2023</height>
+            <y>0</y>
+            <width>685</width>
+            <height>1826</height>
            </rect>
           </property>
           <layout class="QVBoxLayout" name="verticalLayout_16">
            <property name="spacing">
             <number>0</number>
            </property>
-           <property name="margin">
+           <property name="leftMargin">
+            <number>0</number>
+           </property>
+           <property name="topMargin">
+            <number>0</number>
+           </property>
+           <property name="rightMargin">
+            <number>0</number>
+           </property>
+           <property name="bottomMargin">
             <number>0</number>
            </property>
            <item>
@@ -86,7 +95,7 @@
                 <enum>QFrame::Raised</enum>
                </property>
                <property name="currentIndex">
-                <number>1</number>
+                <number>3</number>
                </property>
                <widget class="QWidget" name="page_22">
                 <layout class="QVBoxLayout" name="verticalLayout_29" stretch="0,1">
@@ -2006,7 +2015,7 @@ when using the file type, logs are saved in LogRtabmap.txt (located in the worki
                     <item>
                      <widget class="QStackedWidget" name="stackedWidget_src">
                       <property name="currentIndex">
-                       <number>3</number>
+                       <number>1</number>
                       </property>
                       <widget class="QWidget" name="page_41">
                        <layout class="QVBoxLayout" name="verticalLayout_64">
@@ -2182,7 +2191,7 @@ when using the file type, logs are saved in LogRtabmap.txt (located in the worki
                                  <property name="sizeHint" stdset="0">
                                   <size>
                                    <width>20</width>
-                                   <height>306</height>
+                                   <height>0</height>
                                   </size>
                                  </property>
                                 </spacer>
@@ -2199,7 +2208,7 @@ when using the file type, logs are saved in LogRtabmap.txt (located in the worki
                                  <property name="sizeHint" stdset="0">
                                   <size>
                                    <width>20</width>
-                                   <height>306</height>
+                                   <height>0</height>
                                   </size>
                                  </property>
                                 </spacer>
@@ -2216,7 +2225,7 @@ when using the file type, logs are saved in LogRtabmap.txt (located in the worki
                                  <property name="sizeHint" stdset="0">
                                   <size>
                                    <width>20</width>
-                                   <height>306</height>
+                                   <height>0</height>
                                   </size>
                                  </property>
                                 </spacer>
@@ -2730,7 +2739,7 @@ when using the file type, logs are saved in LogRtabmap.txt (located in the worki
                            <item>
                             <widget class="QLabel" name="label_46">
                              <property name="text">
-                              <string>Grabber for stereo devices (i.e., Bumblebee2).</string>
+                              <string>Grabber for stereo devices (i.e., Bumblebee2, Zed camera).</string>
                              </property>
                              <property name="wordWrap">
                               <bool>true</bool>
@@ -2764,6 +2773,16 @@ when using the file type, logs are saved in LogRtabmap.txt (located in the worki
                                  <string>Video (Side-by-Side)</string>
                                 </property>
                                </item>
+                               <item>
+                                <property name="text">
+                                 <string>ZED sdk</string>
+                                </property>
+                               </item>
+                               <item>
+                                <property name="text">
+                                 <string>Usb camera (Side-by-Side)</string>
+                                </property>
+                               </item>
                               </widget>
                              </item>
                              <item row="0" column="2">
@@ -2798,7 +2817,7 @@ when using the file type, logs are saved in LogRtabmap.txt (located in the worki
                            <item>
                             <widget class="QStackedWidget" name="stackedWidget_stereo">
                              <property name="currentIndex">
-                              <number>3</number>
+                              <number>5</number>
                              </property>
                              <widget class="QWidget" name="page_49">
                               <layout class="QVBoxLayout" name="verticalLayout_91">
@@ -2810,7 +2829,7 @@ when using the file type, logs are saved in LogRtabmap.txt (located in the worki
                                  <property name="sizeHint" stdset="0">
                                   <size>
                                    <width>20</width>
-                                   <height>306</height>
+                                   <height>0</height>
                                   </size>
                                  </property>
                                 </spacer>
@@ -2827,7 +2846,7 @@ when using the file type, logs are saved in LogRtabmap.txt (located in the worki
                                  <property name="sizeHint" stdset="0">
                                   <size>
                                    <width>20</width>
-                                   <height>306</height>
+                                   <height>0</height>
                                   </size>
                                  </property>
                                 </spacer>
@@ -2943,7 +2962,7 @@ when using the file type, logs are saved in LogRtabmap.txt (located in the worki
                              <widget class="QWidget" name="page_50">
                               <layout class="QVBoxLayout" name="verticalLayout_78">
                                <item>
-                                <widget class="QGroupBox" name="groupBox_cameraStereoImages_2">
+                                <widget class="QGroupBox" name="groupBox_cameraStereoVideo">
                                  <property name="sizePolicy">
                                   <sizepolicy hsizetype="Ignored" vsizetype="Ignored">
                                    <horstretch>0</horstretch>
@@ -3006,6 +3025,79 @@ when using the file type, logs are saved in LogRtabmap.txt (located in the worki
                                </item>
                               </layout>
                              </widget>
+                             <widget class="QWidget" name="page_59">
+                              <layout class="QVBoxLayout" name="verticalLayout_101">
+                               <item>
+                                <widget class="QGroupBox" name="groupBox_cameraStereoZed">
+                                 <property name="sizePolicy">
+                                  <sizepolicy hsizetype="Ignored" vsizetype="Ignored">
+                                   <horstretch>0</horstretch>
+                                   <verstretch>0</verstretch>
+                                  </sizepolicy>
+                                 </property>
+                                 <property name="title">
+                                  <string>Zed sdk</string>
+                                 </property>
+                                 <layout class="QVBoxLayout" name="verticalLayout_102">
+                                  <item>
+                                   <layout class="QGridLayout" name="gridLayout_37" columnstretch="0,1">
+                                    <item row="0" column="0">
+                                     <widget class="QCheckBox" name="checkBox_stereoZed_computeDisparity">
+                                      <property name="text">
+                                       <string/>
+                                      </property>
+                                     </widget>
+                                    </item>
+                                    <item row="0" column="1">
+                                     <widget class="QLabel" name="label_272">
+                                      <property name="text">
+                                       <string>Compute disparity with the GPU (using Zed sdk approach). Note that &quot;Generate disparity image...&quot; above will be ignored if set.</string>
+                                      </property>
+                                      <property name="wordWrap">
+                                       <bool>true</bool>
+                                      </property>
+                                      <property name="textInteractionFlags">
+                                       <set>Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse</set>
+                                      </property>
+                                     </widget>
+                                    </item>
+                                   </layout>
+                                  </item>
+                                  <item>
+                                   <spacer name="verticalSpacer_54">
+                                    <property name="orientation">
+                                     <enum>Qt::Vertical</enum>
+                                    </property>
+                                    <property name="sizeHint" stdset="0">
+                                     <size>
+                                      <width>20</width>
+                                      <height>0</height>
+                                     </size>
+                                    </property>
+                                   </spacer>
+                                  </item>
+                                 </layout>
+                                </widget>
+                               </item>
+                              </layout>
+                             </widget>
+                             <widget class="QWidget" name="page_60">
+                              <layout class="QVBoxLayout" name="verticalLayout_103">
+                               <item>
+                                <spacer name="verticalSpacer_53">
+                                 <property name="orientation">
+                                  <enum>Qt::Vertical</enum>
+                                 </property>
+                                 <property name="sizeHint" stdset="0">
+                                  <size>
+                                   <width>20</width>
+                                   <height>217</height>
+                                  </size>
+                                 </property>
+                                </spacer>
+                               </item>
+                              </layout>
+                             </widget>
                             </widget>
                            </item>
                           </layout>
@@ -3069,7 +3161,7 @@ when using the file type, logs are saved in LogRtabmap.txt (located in the worki
                            <item>
                             <widget class="QStackedWidget" name="stackedWidget_image">
                              <property name="currentIndex">
-                              <number>1</number>
+                              <number>0</number>
                              </property>
                              <widget class="QWidget" name="page_7">
                               <layout class="QVBoxLayout" name="verticalLayout_30">
@@ -3422,7 +3514,16 @@ when using the file type, logs are saved in LogRtabmap.txt (located in the worki
                        <string>Directory of images (optional settings)</string>
                       </property>
                       <layout class="QVBoxLayout" name="verticalLayout_93">
-                       <property name="margin">
+                       <property name="leftMargin">
+                        <number>0</number>
+                       </property>
+                       <property name="topMargin">
+                        <number>0</number>
+                       </property>
+                       <property name="rightMargin">
+                        <number>0</number>
+                       </property>
+                       <property name="bottomMargin">
                         <number>0</number>
                        </property>
                        <item>
@@ -6683,7 +6784,20 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag
                        </item>
                        <item>
                         <layout class="QGridLayout" name="gridLayout_50" columnstretch="0,1">
-                         <item row="3" column="0">
+                         <item row="3" column="1">
+                          <widget class="QLabel" name="label_scanMatching_4">
+                           <property name="text">
+                            <string>When comparing to a local path, merge the laser scans using the odometry poses instead of the ones in the optimized local graph.</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="localDetection_pathFilteringRadius">
                            <property name="suffix">
                             <string> m</string>
@@ -6699,7 +6813,7 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag
                            </property>
                           </widget>
                          </item>
-                         <item row="3" column="1">
+                         <item row="2" column="1">
                           <widget class="QLabel" name="label_space3_3">
                            <property name="text">
                             <string>Path filtering radius to avoid merging laser scans which are close. 0 to ignore.</string>
@@ -6712,20 +6826,7 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag
                            </property>
                           </widget>
                          </item>
-                         <item row="4" column="1">
-                          <widget class="QLabel" name="label_scanMatching_4">
-                           <property name="text">
-                            <string>When comparing to a local path, merge the laser scans using the odometry poses instead of the ones in the optimized local graph.</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">
+                         <item row="3" column="0">
                           <widget class="QCheckBox" name="checkBox_localSpacePathOdomPosesUsed">
                            <property name="text">
                             <string/>
@@ -6755,7 +6856,7 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag
                            </property>
                           </widget>
                          </item>
-                         <item row="6" column="1">
+                         <item row="5" column="1">
                           <widget class="QLabel" name="label_scanMatching_6">
                            <property name="text">
                             <string>Save scan matching IDs in link's user data.</string>
@@ -6768,33 +6869,13 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag
                            </property>
                           </widget>
                          </item>
-                         <item row="6" column="0">
+                         <item row="5" column="0">
                           <widget class="QCheckBox" name="checkBox_localSpaceScanMatchingIDsSaved">
                            <property name="text">
                             <string/>
                            </property>
                           </widget>
                          </item>
-                         <item row="2" column="1">
-                          <widget class="QLabel" name="label_scanMatching_7">
-                           <property name="text">
-                            <string>Merge close laser scans on each path. If false, only the nearest laser scan on the path is used for ICP.</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="QCheckBox" name="checkBox_localSpaceAssembleScans">
-                           <property name="text">
-                            <string/>
-                           </property>
-                          </widget>
-                         </item>
                          <item row="1" column="1">
                           <widget class="QLabel" name="label_scanMatching_8">
                            <property name="text">
@@ -8599,7 +8680,16 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag
                       </property>
                       <widget class="QWidget" name="page_54">
                        <layout class="QVBoxLayout" name="verticalLayout_85">
-                        <property name="margin">
+                        <property name="leftMargin">
+                         <number>0</number>
+                        </property>
+                        <property name="topMargin">
+                         <number>0</number>
+                        </property>
+                        <property name="rightMargin">
+                         <number>0</number>
+                        </property>
+                        <property name="bottomMargin">
                          <number>0</number>
                         </property>
                         <item>
@@ -8739,7 +8829,16 @@ Lower the ratio -&gt; higher the precision. 0 means disabled, matching the neare
                       </widget>
                       <widget class="QWidget" name="page_55">
                        <layout class="QVBoxLayout" name="verticalLayout_86">
-                        <property name="margin">
+                        <property name="leftMargin">
+                         <number>0</number>
+                        </property>
+                        <property name="topMargin">
+                         <number>0</number>
+                        </property>
+                        <property name="rightMargin">
+                         <number>0</number>
+                        </property>
+                        <property name="bottomMargin">
                          <number>0</number>
                         </property>
                         <item>
@@ -8897,7 +8996,16 @@ Lower the ratio -&gt; higher the precision. 0 means disabled, matching the neare
                         <property name="spacing">
                          <number>0</number>
                         </property>
-                        <property name="margin">
+                        <property name="leftMargin">
+                         <number>0</number>
+                        </property>
+                        <property name="topMargin">
+                         <number>0</number>
+                        </property>
+                        <property name="rightMargin">
+                         <number>0</number>
+                        </property>
+                        <property name="bottomMargin">
                          <number>0</number>
                         </property>
                         <item>
@@ -8977,7 +9085,16 @@ Lower the ratio -&gt; higher the precision. 0 means disabled, matching the neare
                         <property name="spacing">
                          <number>0</number>
                         </property>
-                        <property name="margin">
+                        <property name="leftMargin">
+                         <number>0</number>
+                        </property>
+                        <property name="topMargin">
+                         <number>0</number>
+                        </property>
+                        <property name="rightMargin">
+                         <number>0</number>
+                        </property>
+                        <property name="bottomMargin">
                          <number>0</number>
                         </property>
                         <item>
@@ -9089,7 +9206,16 @@ Lower the ratio -&gt; higher the precision. 0 means disabled, matching the neare
                         <property name="spacing">
                          <number>0</number>
                         </property>
-                        <property name="margin">
+                        <property name="leftMargin">
+                         <number>0</number>
+                        </property>
+                        <property name="topMargin">
+                         <number>0</number>
+                        </property>
+                        <property name="rightMargin">
+                         <number>0</number>
+                        </property>
+                        <property name="bottomMargin">
                          <number>0</number>
                         </property>
                         <item>

package.xml

@@ -1,7 +1,7 @@
 <?xml version="1.0"?>
 <package>
   <name>rtabmap</name>
-  <version>0.11.4</version>
+  <version>0.11.7</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>
@@ -12,7 +12,7 @@
 
   <buildtool_depend>cmake</buildtool_depend>
   
-  <build_depend>qt_gui_core</build_depend> <!-- libqt4-dev or libqt5-dev -->
+  <build_depend>qt_gui_cpp</build_depend> <!-- libqt4-dev or libqt5-dev -->
   <build_depend>libpcl-all-dev</build_depend>
   <build_depend>libsqlite3-dev</build_depend>
   <build_depend>zlib</build_depend>
@@ -24,7 +24,7 @@
   <!-- libproj-dev needed due to error in vtk6 (kinetic)-->
   <build_depend>proj</build_depend>
   
-  <run_depend>qt_gui_core</run_depend>
+  <run_depend>qt_gui_cpp</run_depend>
   <run_depend>libpcl-all-dev</run_depend>
   <run_depend>libsqlite3-dev</run_depend>
   <run_depend>zlib</run_depend>

tools/CameraRGBD/main.cpp

@@ -53,6 +53,7 @@ void showUsage()
 			"                                     5=Freenect2  (Kinect v2)\n"
 			"                                     6=DC1394     (Bumblebee2)\n"
 			"                                     7=FlyCapture2 (Bumblebee2)\n"
+			"                                     8=ZED stereo\n"
 			"  Options:\n"
 			"      -rate #.#                      Input rate Hz (default 0=inf)\n"
 			"      -save_stereo \"path\"            Save stereo images in a folder or a video file (side by side *.avi).\n"
@@ -149,9 +150,9 @@ int main(int argc, char * argv[])
 
 			// last
 			driver = atoi(argv[i]);
-			if(driver < 0 || driver > 7)
+			if(driver < 0 || driver > 8)
 			{
-				UERROR("driver should be between 0 and 6.");
+				UERROR("driver should be between 0 and 8.");
 				showUsage();
 			}
 		}
@@ -235,6 +236,15 @@ int main(int argc, char * argv[])
 		}
 		camera = new rtabmap::CameraStereoFlyCapture2();
 	}
+	else if(driver == 8)
+	{
+		if(!rtabmap::CameraStereoZed::available())
+		{
+			UERROR("Not built with ZED sdk support...");
+			exit(-1);
+		}
+		camera = new rtabmap::CameraStereoZed(true);
+	}
 	else
 	{
 		UFATAL("");