Update package.xml

Bump package version

.appveyor.yml

@@ -90,7 +90,7 @@ before_build:
   - cd c:\projects\rtabmap\build
   - ECHO %PROGRAMFILES%
   - ECHO %PATH%
-  - cmake -G "Visual Studio 14 2015 Win64" -DOpenCV_DIR="C:\Program Files\opencv\build" -DPCL_DIR="C:\Program Files\PCL\cmake" -DZLIB_ROOT="C:\Program Files\zlib" ..
+  - cmake -G "Visual Studio 14 2015 Win64" -DOpenCV_DIR="C:\Program Files\opencv\build" -DPCL_DIR="C:\Program Files\PCL\cmake" -DZLIB_ROOT="C:\Program Files\zlib" -DBUILD_AS_BUNDLE=ON ..
   
 after_build :
   - cmake --build . --config Release --target package

.travis.yml

@@ -20,6 +20,7 @@ install:
     - sudo sh -c 'echo "deb http://packages.ros.org/ros/ubuntu trusty main" > /etc/apt/sources.list.d/ros-latest.list'
     - wget http://packages.ros.org/ros.key -O - | sudo apt-key add -
     - sudo apt-get update
+    - sudo apt-get update && sudo apt-get install dpkg
     - sudo apt-get -y install libpcl-1.7-all libfreenect-dev ros-indigo-libg2o ros-indigo-octomap libopenni2-dev
 
 script: 

CMakeLists.txt

@@ -21,7 +21,7 @@ SET(CMAKE_MODULE_PATH "${PROJECT_SOURCE_DIR}/cmake_modules")
 #######################
 SET(RTABMAP_MAJOR_VERSION 0)
 SET(RTABMAP_MINOR_VERSION 17)
-SET(RTABMAP_PATCH_VERSION 4)
+SET(RTABMAP_PATCH_VERSION 6)
 SET(RTABMAP_VERSION
   ${RTABMAP_MAJOR_VERSION}.${RTABMAP_MINOR_VERSION}.${RTABMAP_PATCH_VERSION})
   
@@ -131,9 +131,9 @@ IF(ANDROID_PREBUILD)
    return()
 ENDIF(ANDROID_PREBUILD)
 
-IF(APPLE)
-    OPTION(BUILD_AS_BUNDLE "Set to ON to build as bundle (DragNDrop)" OFF)
-ENDIF(APPLE)
+IF(APPLE OR WIN32)
+    OPTION(BUILD_AS_BUNDLE "Set to ON to build as bundle with all embedded dependencies (DragNDrop for Mac, installer for Windows)" OFF)
+ENDIF(APPLE OR WIN32)
 OPTION(BUILD_APP "Build main application" ON)
 OPTION(BUILD_TOOLS "Build tools" ON)
 OPTION(BUILD_EXAMPLES "Build examples" ON)
@@ -155,7 +155,7 @@ option(WITH_TORO          "Include TORO support"                 ON)
 option(WITH_VERTIGO       "Include Vertigo support"              ON)
 option(WITH_CVSBA         "Include cvsba support"                ON)
 option(WITH_POINTMATCHER  "Include libpointmatcher support"      ON)
-option(WITH_LOAM          "Include LOAM support"                 OFF)
+option(WITH_LOAM          "Include LOAM support"                 ON)
 option(WITH_FLYCAPTURE2   "Include FlyCapture2/Triclops support" ON)
 option(WITH_ZED           "Include ZED sdk support"              ON)
 option(WITH_REALSENSE     "Include RealSense support"            ON)
@@ -187,6 +187,11 @@ endif()
 
 FIND_PACKAGE(ZLIB REQUIRED QUIET)
 
+FIND_PACKAGE(Sqlite3 QUIET)
+IF(Sqlite3_FOUND)
+   MESSAGE(STATUS "Found Sqlite3: ${Sqlite3_INCLUDE_DIRS} ${Sqlite3_LIBRARIES}")
+ENDIF(Sqlite3_FOUND)
+
 if(NOT "${PCL_LIBRARIES}" STREQUAL "")
    # fix libproj.so not found on Xenial
    list(REMOVE_ITEM PCL_LIBRARIES "vtkproj4")
@@ -676,9 +681,9 @@ IF(VTK_USE_QVTK)
    SET(CONF_DEPENDENCIES ${CONF_DEPENDENCIES} ${QVTK_LIBRARY}) 
 ENDIF(VTK_USE_QVTK)
 
-IF(NOT (OpenCV_FOUND AND OpenCV_VERSION_MAJOR EQUAL 3))
+IF(NOT (OpenCV_FOUND AND NOT (OpenCV_VERSION_MAJOR LESS 3)))
    SET(OPENCV3 "//")
-ENDIF(NOT (OpenCV_FOUND AND OpenCV_VERSION_MAJOR EQUAL 3))
+ENDIF(NOT (OpenCV_FOUND AND NOT (OpenCV_VERSION_MAJOR LESS 3)))
 CONFIGURE_FILE(Version.h.in ${PROJECT_SOURCE_DIR}/corelib/include/${PROJECT_PREFIX}/core/Version.h)
 
 ADD_SUBDIRECTORY( utilite )
@@ -798,7 +803,11 @@ IF(WIN32)
   ELSE() 
     SET(CPACK_NSIS_INSTALL_ROOT "$PROGRAMFILES") 
   ENDIF()
-  SET(CPACK_GENERATOR "ZIP;NSIS")
+  IF(BUILD_AS_BUNDLE)
+    SET(CPACK_GENERATOR "ZIP;NSIS")
+  ELSE()
+    SET(CPACK_GENERATOR "ZIP")
+  ENDIF()
   SET(CPACK_SOURCE_GENERATOR "ZIP")
   SET(CPACK_NSIS_PACKAGE_NAME "${PROJECT_NAME}")
   SET(ICON_PATH "${PROJECT_SOURCE_DIR}/app/src/${PROJECT_NAME}.ico")
@@ -853,28 +862,35 @@ IF(NOT WIN32)
     # see comment above for the BUILD_SHARED_LIBS option on Windows
     MESSAGE(STATUS "  BUILD_SHARED_LIBS =    ${BUILD_SHARED_LIBS}")
 ENDIF(NOT WIN32)
-IF(APPLE)
+IF(APPLE OR WIN32)
 MESSAGE(STATUS "  BUILD_AS_BUNDLE =      ${BUILD_AS_BUNDLE}")
-ENDIF(APPLE)
+ENDIF(APPLE OR WIN32)
 MESSAGE(STATUS "  CMAKE_CXX_FLAGS = ${CMAKE_CXX_FLAGS}")
 MESSAGE(STATUS "  PCL_DEFINITIONS = ${PCL_DEFINITIONS}")
 
+MESSAGE(STATUS "Optional dependencies ('*' affects some default parameters) :")
 IF(OpenCV_FOUND)
   IF(OpenCV_VERSION_MAJOR EQUAL 2)
     IF(OPENCV_NONFREE_FOUND)
-      MESSAGE(STATUS "  With OpenCV 2 nonfree module (SIFT/SURF) = YES (License: Non commercial)")
+      MESSAGE(STATUS " *With OpenCV 2 nonfree module (SIFT/SURF) = YES (License: Non commercial)")
     ELSE()
-      MESSAGE(STATUS "  With OpenCV 2 nonfree module (SIFT/SURF) = NO (not found, License: BSD)")
+      MESSAGE(STATUS " *With OpenCV 2 nonfree module (SIFT/SURF) = NO (not found, License: BSD)")
     ENDIF()
   ELSE()
     IF(OPENCV_XFEATURES2D_FOUND)
-      MESSAGE(STATUS "  With OpenCV 3 xfeatures2d module (SIFT/SURF/BRIEF/FREAK) = YES (License: Non commercial)")
+      MESSAGE(STATUS " *With OpenCV 3 xfeatures2d module (SIFT/SURF/BRIEF/FREAK) = YES (License: Non commercial)")
     ELSE()
-      MESSAGE(STATUS "  With OpenCV 3 xfeatures2d module (SIFT/SURF/BRIEF/FREAK) = NO (not found, License: BSD)")
+      MESSAGE(STATUS " *With OpenCV 3 xfeatures2d module (SIFT/SURF/BRIEF/FREAK) = NO (not found, License: BSD)")
     ENDIF()
   ENDIF()
 ENDIF(OpenCV_FOUND)
 
+IF(Sqlite3_FOUND)
+MESSAGE(STATUS "  With external SQLite3     = YES (License: Public Domain)")
+ELSE()
+MESSAGE(STATUS "  With external SQLite3     = NO (sqlite3 not found, internal version is used for convenience)")
+ENDIF()
+
 IF(Freenect_FOUND)
 MESSAGE(STATUS "  With Freenect             = YES (License: Apache v2 and/or GPLv2)")
 ELSEIF(NOT WITH_FREENECT)
@@ -930,19 +946,19 @@ MESSAGE(STATUS "  With TORO                 = NO (WITH_TORO=OFF)")
 ENDIF()
 
 IF(G2O_FOUND)
-MESSAGE(STATUS "  With g2o                  = YES (License: BSD)")
+MESSAGE(STATUS " *With g2o                  = YES (License: BSD)")
 ELSEIF(NOT WITH_G2O)
-MESSAGE(STATUS "  With g2o                  = NO (WITH_G2O=OFF)")
+MESSAGE(STATUS " *With g2o                  = NO (WITH_G2O=OFF)")
 ELSE()
-MESSAGE(STATUS "  With g2o                  = NO (g2o not found)")
+MESSAGE(STATUS " *With g2o                  = NO (g2o not found)")
 ENDIF()
 
 IF(GTSAM_FOUND)
-MESSAGE(STATUS "  With GTSAM                = YES (License: BSD)")
+MESSAGE(STATUS " *With GTSAM                = YES (License: BSD)")
 ELSEIF(NOT WITH_GTSAM)
-MESSAGE(STATUS "  With GTSAM                = NO (WITH_GTSAM=OFF)")
+MESSAGE(STATUS " *With GTSAM                = NO (WITH_GTSAM=OFF)")
 ELSE()
-MESSAGE(STATUS "  With GTSAM                = NO (GTSAM not found)")
+MESSAGE(STATUS " *With GTSAM                = NO (GTSAM not found)")
 ENDIF()
 
 IF(G2O_FOUND OR GTSAM_FOUND)
@@ -964,16 +980,16 @@ MESSAGE(STATUS "  With cvsba                = NO (cvsba not found)")
 ENDIF()
 
 IF(libpointmatcher_FOUND)
-MESSAGE(STATUS "  With libpointmatcher      = YES (License: BSD)")
+MESSAGE(STATUS " *With libpointmatcher      = YES (License: BSD)")
 ELSEIF(NOT WITH_POINTMATCHER)
-MESSAGE(STATUS "  With libpointmatcher      = NO (WITH_POINTMATCHER=OFF)")
+MESSAGE(STATUS " *With libpointmatcher      = NO (WITH_POINTMATCHER=OFF)")
 ELSE()
-MESSAGE(STATUS "  With libpointmatcher      = NO (libpointmatcher not found)")
+MESSAGE(STATUS " *With libpointmatcher      = NO (libpointmatcher not found)")
 ENDIF()
 
 IF(loam_velodyne_FOUND)
 MESSAGE(STATUS "  With loam_velodyne        = YES (License: BSD)")
-ELSEIF(NOT WITH_POINTMATCHER)
+ELSEIF(NOT WITH_LOAM)
 MESSAGE(STATUS "  With loam_velodyne        = NO (WITH_LOAM=OFF)")
 ELSE()
 MESSAGE(STATUS "  With loam_velodyne        = NO (loam_velodyne not found)")

app/src/CMakeLists.txt

@@ -104,7 +104,7 @@ INSTALL(CODE "execute_process(COMMAND ln -s \"../MacOS/${CMAKE_BUNDLE_NAME}\" ${
         WORKING_DIRECTORY \$ENV{DESTDIR}\${CMAKE_INSTALL_PREFIX}/bin)")
 ENDIF(APPLE AND BUILD_AS_BUNDLE)
        
-IF((APPLE AND BUILD_AS_BUNDLE) OR WIN32)
+IF(BUILD_AS_BUNDLE AND (APPLE OR WIN32))
   SET(APPS "\$ENV{DESTDIR}\${CMAKE_INSTALL_PREFIX}/bin/${PROJECT_NAME}${CMAKE_EXECUTABLE_SUFFIX}")
   SET(plugin_dest_dir bin)
   SET(qtconf_dest_dir bin)
@@ -189,5 +189,5 @@ IF((APPLE AND BUILD_AS_BUNDLE) OR WIN32)
     include(\"BundleUtilities\")
     fixup_bundle(\"${APPS}\" \"\${QTPLUGINS}\" \"${DIRS}\")
   " COMPONENT runtime)
-ENDIF((APPLE AND BUILD_AS_BUNDLE) OR WIN32)
+ENDIF(BUILD_AS_BUNDLE AND (APPLE OR WIN32))
   

app/src/main.cpp

@@ -49,7 +49,7 @@ int main(int argc, char* argv[])
 	QApplication * app = new QApplication(argc, argv);
 	app->setStyleSheet("QMessageBox { messagebox-text-interaction-flags: 5; }"); // selectable message box
 
-	ParametersMap parameters = Parameters::parseArguments(argc, argv, true);
+	ParametersMap parameters = Parameters::parseArguments(argc, argv, false);
 	MainWindow * mainWindow = new MainWindow();
     app->installEventFilter(mainWindow); // to catch FileOpen events.
     
@@ -85,9 +85,9 @@ int main(int argc, char* argv[])
     
     if(!database.empty())
     {
-    	mainWindow->openDatabase(database.c_str());
+    	mainWindow->openDatabase(database.c_str(), parameters);
     }
-    if(parameters.size())
+    else if(parameters.size())
     {
     	mainWindow->updateParameters(parameters);
     }

cmake_modules/FindSqlite3.cmake

@@ -2,46 +2,29 @@
 # This module finds an installed Sqlite3 package.
 #
 # It sets the following variables:
-#  SQLITE3_FOUND       - Set to false, or undefined, if Sqlite3 isn't found.
-#  SQLITE3_INCLUDE_DIR - The Sqlite3 include directory.
-#  SQLITE3_LIBRARY     - The Sqlite3 library to link against.
+#  Sqlite3_FOUND       - Set to false, or undefined, if Sqlite3 isn't found.
+#  Sqlite3_INCLUDE_DIR - The Sqlite3 include directory.
+#  Sqlite3_LIBRARY     - The Sqlite3 library to link against.
 
-SET(SQLITE3_VERSION_REQUIRED "3.6.0")
+FIND_PATH(Sqlite3_INCLUDE_DIR sqlite3.h PATHS $ENV{Sqlite3_ROOT_DIR}/include $ENV{Sqlite3_ROOT_DIR})
 
-IF(UNIX)
-	FIND_PROGRAM(SQLITE3_EXEC NAME sqlite3 PATHS)  
-	IF(SQLITE3_EXEC)  
-        MESSAGE(STATUS "Found Sqlite3 executable : ${SQLITE3_EXEC}")
-		EXECUTE_PROCESS(COMMAND ${SQLITE3_EXEC} --version 
-				   	    OUTPUT_VARIABLE SQLITE3_VERSION
-						OUTPUT_STRIP_TRAILING_WHITESPACE
-						WORKING_DIRECTORY "./"
-   		)
-		IF(SQLITE3_VERSION VERSION_LESS SQLITE3_VERSION_REQUIRED)
-		    MESSAGE(FATAL_ERROR "Sqlite ${SQLITE3_VERSION} found, but version ${SQLITE3_VERSION_REQUIRED} minimum is required")
-		ENDIF(SQLITE3_VERSION VERSION_LESS SQLITE3_VERSION_REQUIRED)
-	ELSE(SQLITE3_EXEC)
-		MESSAGE(FATAL_ERROR "Could not find Sqlite3 executable")
-	ENDIF(SQLITE3_EXEC)
-ENDIF(UNIX)
+FIND_LIBRARY(Sqlite3_LIBRARY NAMES sqlite3 PATHS $ENV{Sqlite3_ROOT_DIR}/lib $ENV{Sqlite3_ROOT_DIR})
 
-FIND_PATH(SQLITE3_INCLUDE_DIR sqlite3.h)
+IF (Sqlite3_INCLUDE_DIR AND Sqlite3_LIBRARY)
+   SET(Sqlite3_FOUND TRUE)
+   SET(Sqlite3_INCLUDE_DIRS ${Sqlite3_INCLUDE_DIR})
+   SET(Sqlite3_LIBRARIES ${Sqlite3_LIBRARY})
+ENDIF (Sqlite3_INCLUDE_DIR AND Sqlite3_LIBRARY)
 
-FIND_LIBRARY(SQLITE3_LIBRARY NAMES sqlite3.dll sqlite3)
-
-IF (SQLITE3_INCLUDE_DIR AND SQLITE3_LIBRARY)
-   SET(SQLITE3_FOUND TRUE)
-ENDIF (SQLITE3_INCLUDE_DIR AND SQLITE3_LIBRARY)
-
-IF (SQLITE3_FOUND)
+IF (Sqlite3_FOUND)
    # show which Sqlite3 was found only if not quiet
    IF (NOT Sqlite3_FIND_QUIETLY)
-      MESSAGE(STATUS "Found Sqlite3")
+      MESSAGE(STATUS "Found Sqlite3: ${Sqlite3_INCLUDE_DIRS} ${Sqlite3_LIBRARIES}")
    ENDIF (NOT Sqlite3_FIND_QUIETLY)
-ELSE (SQLITE3_FOUND)
+ELSE (Sqlite3_FOUND)
    # fatal error if Sqlite3 is required but not found
    IF (Sqlite3_FIND_REQUIRED)
       MESSAGE(FATAL_ERROR "Could not find Sqlite3")
    ENDIF (Sqlite3_FIND_REQUIRED)
-ENDIF (SQLITE3_FOUND)
+ENDIF (Sqlite3_FOUND)
 

corelib/include/rtabmap/core/CameraModel.h

@@ -75,6 +75,7 @@ public:
 	virtual ~CameraModel() {}
 
 	void initRectificationMap();
+	bool isRectificationMapInitialized() {return !mapX_.empty() && !mapY_.empty();}
 
 	bool isValidForProjection() const {return fx()>0.0 && fy()>0.0 && cx()>0.0 && cy()>0.0;}
 	bool isValidForReprojection() const {return fx()>0.0 && fy()>0.0 && cx()>0.0 && cy()>0.0 && imageWidth()>0 && imageHeight()>0;}

corelib/include/rtabmap/core/CameraRGBD.h

@@ -379,6 +379,7 @@ class RTABMAP_EXP CameraRealSense :
 {
 public:
 	static bool available();
+	enum RGBSource {kColor, kInfrared, kFishEye};
 
 public:
 	// default local transform z in, x right, y down));
@@ -391,11 +392,8 @@ public:
 		const Transform & localTransform = Transform::getIdentity());
 	virtual ~CameraRealSense();
 
-	void setDepthScaledToRGBSize(bool enabled) {
-#ifdef RTABMAP_REALSENSE
-		depthScaledToRGBSize_ = enabled;
-#endif
-		}
+	void setDepthScaledToRGBSize(bool enabled);
+	void setRGBSource(RGBSource source);
 	virtual bool init(const std::string & calibrationFolder = ".", const std::string & cameraName = "");
 	virtual bool isCalibrated() const;
 	virtual std::string getSerial() const;
@@ -413,6 +411,9 @@ private:
 	int presetDepth_;
 	bool computeOdometry_;
 	bool depthScaledToRGBSize_;
+	RGBSource rgbSource_;
+	CameraModel cameraModel_;
+	std::vector<int> rsRectificationTable_;
 
 	int motionSeq_[2];
 	rs::slam::slam * slam_;

corelib/include/rtabmap/core/DBDriver.h

@@ -179,7 +179,6 @@ public:
 protected:
 	DBDriver(const ParametersMap & parameters = ParametersMap());
 
-private:
 	virtual bool connectDatabaseQuery(const std::string & url, bool overwritten = false) = 0;
 	virtual void disconnectDatabaseQuery(bool save = true, const std::string & outputUrl = "") = 0;
 	virtual bool isConnectedQuery() const = 0;

corelib/include/rtabmap/core/DBDriverSqlite3.h

@@ -31,7 +31,9 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #include "rtabmap/core/RtabmapExp.h" // DLL export/import defines
 #include "rtabmap/core/DBDriver.h"
 #include <opencv2/features2d/features2d.hpp>
-#include "sqlite3/sqlite3.h"
+
+typedef struct sqlite3_stmt sqlite3_stmt;
+typedef struct sqlite3 sqlite3;
 
 namespace rtabmap {
 
@@ -48,7 +50,7 @@ public:
 	void setSynchronous(int synchronous);
 	void setTempStore(int tempStore);
 
-private:
+protected:
 	virtual bool connectDatabaseQuery(const std::string & url, bool overwritten = false);
 	virtual void disconnectDatabaseQuery(bool save = true, const std::string & outputUrl = "");
 	virtual bool isConnectedQuery() const;
@@ -175,10 +177,12 @@ private:
 	void loadLinksQuery(std::list<Signature *> & signatures) const;
 	int loadOrSaveDb(sqlite3 *pInMemory, const std::string & fileName, int isSave) const;
 
-private:
+protected:
 	sqlite3 * _ppDb;
-	long _memoryUsedEstimate;
 	std::string _version;
+
+private:
+	long _memoryUsedEstimate;
 	bool _dbInMemory;
 	unsigned int _cacheSize;
 	int _journalMode;

corelib/include/rtabmap/core/Memory.h

@@ -291,7 +291,7 @@ private:
 	float _laserScanDownsampleStepSize;
 	float _laserScanVoxelSize;
 	int _laserScanNormalK;
-	int _laserScanNormalRadius;
+	float _laserScanNormalRadius;
 	bool _reextractLoopClosureFeatures;
 	bool _localBundleOnLoopClosure;
 	float _rehearsalMaxDistance;
@@ -302,6 +302,8 @@ private:
 	int _visMaxFeatures;
 	int _visCorType;
 	bool _imagesAlreadyRectified;
+	bool _rectifyOnlyFeatures;
+	bool _covOffDiagonalIgnored;
 
 	int _idCount;
 	int _idMapCount;
@@ -311,6 +313,8 @@ private:
 	bool _linksChanged; // False by default, become true when links are modified.
 	int _signaturesAdded;
 	GPS _gpsOrigin;
+	std::vector<CameraModel> _rectCameraModels;
+	StereoCameraModel _rectStereoCameraModel;
 
 	std::map<int, Signature *> _signatures; // TODO : check if a signature is already added? although it is not supposed to occur...
 	std::set<int> _stMem; // id

corelib/include/rtabmap/core/OccupancyGrid.h

@@ -39,6 +39,17 @@ namespace rtabmap {
 
 class RTABMAP_EXP OccupancyGrid
 {
+public:
+	inline static float logodds(double probability)
+	{
+		return (float) log(probability/(1-probability));
+	}
+
+	inline static double probability(double logodds)
+	{
+		return 1. - ( 1. / (1. + exp(logodds)));
+	}
+
 public:
 	OccupancyGrid(const ParametersMap & parameters = ParametersMap());
 	void parseParameters(const ParametersMap & parameters);
@@ -53,6 +64,7 @@ public:
 	bool isMapFrameProjection() const {return projMapFrame_;}
 	const std::map<int, Transform> & addedNodes() const {return addedNodes_;}
 	int cacheSize() const {return (int)cache_.size();}
+	const std::map<int, std::pair<std::pair<cv::Mat, cv::Mat>, cv::Mat> > & getCache() const {return cache_;}
 
 	template<typename PointT>
 	typename pcl::PointCloud<PointT>::Ptr segmentCloud(
@@ -87,6 +99,7 @@ public:
 			const cv::Mat & empty);
 	void update(const std::map<int, Transform> & poses);
 	cv::Mat getMap(float & xMin, float & yMin) const;
+	cv::Mat getProbMap(float & xMin, float & yMin) const;
 	const pcl::PointCloud<pcl::PointXYZRGB>::Ptr & getMapGround() const {return assembledGround_;}
 	const pcl::PointCloud<pcl::PointXYZRGB>::Ptr & getMapObstacles() const {return assembledObstacles_;}
 	const pcl::PointCloud<pcl::PointXYZRGB>::Ptr & getMapEmptyCells() const {return assembledEmptyCells_;}
@@ -125,6 +138,11 @@ private:
 	bool erode_;
 	float footprintRadius_;
 	float updateError_;
+	float occupancyThr_;
+	float probHit_;
+	float probMiss_;
+	float probClampingMin_;
+	float probClampingMax_;
 
 	std::map<int, std::pair<std::pair<cv::Mat, cv::Mat>, cv::Mat> > cache_; //<node id, < <ground, obstacles>, empty> >
 	cv::Mat map_;

corelib/include/rtabmap/core/OdometryOkvis.h

@@ -55,12 +55,12 @@ private:
 #ifdef RTABMAP_OKVIS
 	OkvisCallbackHandler * okvisCallbackHandler_;
 	okvis::ThreadedKFVio * okvisEstimator_;
+	int imagesProcessed_;
+	bool initGravity_;
 #endif
 	ParametersMap okvisParameters_;
 	IMU lastImu_; // only used for initialization
-	int imagesProcessed_;
 	Transform previousPose_;
-	bool initGravity_;
 };
 
 }

corelib/include/rtabmap/core/Parameters.h

@@ -175,8 +175,8 @@ class RTABMAP_EXP Parameters
     RTABMAP_PARAM(Rtabmap, PublishRAMUsage,              bool, false, "Publishing RAM usage in statistics (may add a small overhead to get info from the system).");
     RTABMAP_PARAM(Rtabmap, ComputeRMSE,                  bool, true,  "Compute root mean square error (RMSE) and publish it in statistics, if ground truth is provided.");
     RTABMAP_PARAM(Rtabmap, SaveWMState,                  bool, false, "Save working memory state after each update in statistics.");
-    RTABMAP_PARAM(Rtabmap, TimeThr,                      float, 0,    "Maximum time allowed for the detector (ms) (0 means infinity).");
-    RTABMAP_PARAM(Rtabmap, MemoryThr,                    int, 0,      "Maximum signatures in the Working Memory (ms) (0 means infinity).");
+    RTABMAP_PARAM(Rtabmap, TimeThr,                      float, 0,    "Maximum time allowed for map update (ms) (0 means infinity). When map update time exceeds this fixed time threshold, some nodes in Working Memory (WM) are transferred to Long-Term Memory to limit the size of the WM and decrease the update time.");
+    RTABMAP_PARAM(Rtabmap, MemoryThr,                    int, 0,      uFormat("Maximum nodes in the Working Memory (0 means infinity). Similar to \"%s\", when the number of nodes in Working Memory (WM) exceeds this treshold, some nodes are transferred to Long-Term Memory to keep WM size fixed.", kRtabmapTimeThr().c_str()));
     RTABMAP_PARAM(Rtabmap, DetectionRate,                float, 1,    "Detection rate (Hz). RTAB-Map will filter input images to satisfy this rate.");
     RTABMAP_PARAM(Rtabmap, ImageBufferSize,          unsigned int, 1, "Data buffer size (0 min inf).");
     RTABMAP_PARAM(Rtabmap, CreateIntermediateNodes,      bool, false, uFormat("Create intermediate nodes between loop closure detection. Only used when %s>0.", kRtabmapDetectionRate().c_str()));
@@ -188,6 +188,7 @@ class RTABMAP_EXP Parameters
     RTABMAP_PARAM(Rtabmap, StartNewMapOnLoopClosure,     bool, false, "Start a new map only if there is a global loop closure with a previous map.");
     RTABMAP_PARAM(Rtabmap, StartNewMapOnGoodSignature,   bool, false, uFormat("Start a new map only if the first signature is not bad (i.e., has enough features, see %s).", kKpBadSignRatio().c_str()));
     RTABMAP_PARAM(Rtabmap, ImagesAlreadyRectified,       bool, true,  "Images are already rectified. By default RTAB-Map assumes that received images are rectified. If they are not, they can be rectified by RTAB-Map if this parameter is false.");
+    RTABMAP_PARAM(Rtabmap, RectifyOnlyFeatures,          bool, false,  uFormat("If \"%s\" is false and this parameter is true, the whole RGB image will not be rectified, only the features. Warning: As projection of RGB-D image to point cloud is assuming that images are rectified, the generated point cloud map will have wrong colors if this parameter is true.", kRtabmapImagesAlreadyRectified().c_str()));
 
     // Hypotheses selection
     RTABMAP_PARAM(Rtabmap, LoopThr,           float, 0.11,      "Loop closing threshold.");
@@ -219,8 +220,9 @@ class RTABMAP_EXP Parameters
     RTABMAP_PARAM(Mem, LaserScanDownsampleStepSize, int, 1,         "If > 1, downsample the laser scans when creating a signature.");
     RTABMAP_PARAM(Mem, LaserScanVoxelSize,          float, 0.0,     uFormat("If > 0 m, voxel filtering is done on laser scans when creating a signature. If the laser scan had normals, they will be removed. To recompute the normals, make sure to use \"%s\" or \"%s\" parameters.", kMemLaserScanNormalK().c_str(), kMemLaserScanNormalRadius().c_str()).c_str());
     RTABMAP_PARAM(Mem, LaserScanNormalK,            int, 0,         "If > 0 and laser scans don't have normals, normals will be computed with K search neighbors when creating a signature.");
-    RTABMAP_PARAM(Mem, LaserScanNormalRadius,       int, 0,         "If > 0 m and laser scans don't have normals, normals will be computed with radius search neighbors when creating a signature.");
+    RTABMAP_PARAM(Mem, LaserScanNormalRadius,       float, 0.0,     "If > 0 m and laser scans don't have normals, normals will be computed with radius search neighbors when creating a signature.");
     RTABMAP_PARAM(Mem, UseOdomFeatures,             bool, true,     "Use odometry features.");
+    RTABMAP_PARAM(Mem, CovOffDiagIgnored,           bool, true,     "Ignore off diagonal values of the covariance matrix.");
 
     // KeypointMemory (Keypoint-based)
     RTABMAP_PARAM(Kp, NNStrategy,               int, 1,       "kNNFlannNaive=0, kNNFlannKdTree=1, kNNFlannLSH=2, kNNBruteForce=3, kNNBruteForceGPU=4");
@@ -283,8 +285,8 @@ class RTABMAP_EXP Parameters
     RTABMAP_PARAM(FAST, GpuKeypointsRatio,  double, 0.05, "Used with FAST GPU.");
     RTABMAP_PARAM(FAST, MinThreshold,       int, 7,       "Minimum threshold. Used only when FAST/GridRows and FAST/GridCols are set.");
     RTABMAP_PARAM(FAST, MaxThreshold,       int, 200,     "Maximum threshold. Used only when FAST/GridRows and FAST/GridCols are set.");
-    RTABMAP_PARAM(FAST, GridRows,           int, 4,       "Grid rows (0 to disable). Adapts the detector to partition the source image into a grid and detect points in each cell.");
-    RTABMAP_PARAM(FAST, GridCols,           int, 4,       "Grid cols (0 to disable). Adapts the detector to partition the source image into a grid and detect points in each cell.");
+    RTABMAP_PARAM(FAST, GridRows,           int, 0,       "Grid rows (0 to disable). Adapts the detector to partition the source image into a grid and detect points in each cell.");
+    RTABMAP_PARAM(FAST, GridCols,           int, 0,       "Grid cols (0 to disable). Adapts the detector to partition the source image into a grid and detect points in each cell.");
 
     RTABMAP_PARAM(GFTT, QualityLevel,      double, 0.001, "");
     RTABMAP_PARAM(GFTT, MinDistance,       double, 3,    "");
@@ -677,7 +679,11 @@ class RTABMAP_EXP Parameters
     RTABMAP_PARAM(GridGlobal, MinSize,              float,  0.0,     "Minimum map size (m).");
     RTABMAP_PARAM(GridGlobal, Eroded,               bool,   false,   "Erode obstacle cells.");
     RTABMAP_PARAM(GridGlobal, MaxNodes,             int,    0,       "Maximum nodes assembled in the map starting from the last node (0=unlimited).");
-    RTABMAP_PARAM(GridGlobal, OctoMapOccupancyThr,  float,  0.5,     "OctoMap occupancy threshold (value between 0 and 1).");
+    RTABMAP_PARAM(GridGlobal, OccupancyThr,         float,  0.5,     "Occupancy threshold (value between 0 and 1).");
+    RTABMAP_PARAM(GridGlobal, ProbHit,              float,  0.7,     "Probability of a hit (value between 0.5 and 1).");
+    RTABMAP_PARAM(GridGlobal, ProbMiss,             float,  0.4,     "Probability of a miss (value between 0 and 0.5).");
+    RTABMAP_PARAM(GridGlobal, ProbClampingMin,      float,  0.1192,  "Probability clamping minimum (value between 0 and 1).");
+    RTABMAP_PARAM(GridGlobal, ProbClampingMax,      float,  0.971,   "Probability clamping maximum (value between 0 and 1).");
 
 public:
     virtual ~Parameters();

corelib/include/rtabmap/core/Rtabmap.h

@@ -153,7 +153,8 @@ public:
 	void parseParameters(const ParametersMap & parameters);
 	const ParametersMap & getParameters() const {return _parameters;}
 	void setWorkingDirectory(std::string path);
-	void rejectLoopClosure(int oldId, int newId);
+	void rejectLastLoopClosure();
+	void deleteLastLocation();
 	void setOptimizedPoses(const std::map<int, Transform> & poses);
 	void get3DMap(std::map<int, Signature> & signatures,
 			std::map<int, Transform> & poses,
@@ -249,7 +250,7 @@ private:
 	float _proximityAngle;
 	std::string _databasePath;
 	bool _optimizeFromGraphEnd;
-	float _optimizationMaxLinearError;
+	float _optimizationMaxError;
 	bool _startNewMapOnLoopClosure;
 	bool _startNewMapOnGoodSignature;
 	float _goalReachedRadius; // meters

corelib/include/rtabmap/core/Statistics.h

@@ -67,6 +67,8 @@ class RTABMAP_EXP Statistics
 	RTABMAP_STATS(Loop, Optimization_max_error_ratio, );
 	RTABMAP_STATS(Loop, Optimization_error, );
 	RTABMAP_STATS(Loop, Optimization_iterations, );
+	RTABMAP_STATS(Loop, Linear_variance,);
+	RTABMAP_STATS(Loop, Angular_variance,);
 
 	RTABMAP_STATS(Proximity, Time_detections,);
 	RTABMAP_STATS(Proximity, Space_last_detection_id,);
@@ -104,6 +106,7 @@ class RTABMAP_EXP Statistics
 	RTABMAP_STATS(Memory, Odometry_variance_lin,);
 	RTABMAP_STATS(Memory, Distance_travelled, m);
 	RTABMAP_STATS(Memory, RAM_usage, MB);
+	RTABMAP_STATS(Memory, Triangulated_points, );
 
 	RTABMAP_STATS(Timing, Memory_update, ms);
 	RTABMAP_STATS(Timing, Neighbor_link_refining, ms);
@@ -134,6 +137,7 @@ class RTABMAP_EXP Statistics
 	RTABMAP_STATS(TimingMem, Descriptors_extraction, ms);
 	RTABMAP_STATS(TimingMem, Rectification, ms);
 	RTABMAP_STATS(TimingMem, Keypoints_3D, ms);
+	RTABMAP_STATS(TimingMem, Keypoints_3D_motion, ms);
 	RTABMAP_STATS(TimingMem, Joining_dictionary_update, ms);
 	RTABMAP_STATS(TimingMem, Add_new_words, ms);
 	RTABMAP_STATS(TimingMem, Compressing_data, ms);

corelib/include/rtabmap/core/StereoCameraModel.h

@@ -86,6 +86,7 @@ public:
 	bool isValidForRectification() const {return left_.isValidForRectification() && right_.isValidForRectification();}
 
 	void initRectificationMap() {left_.initRectificationMap(); right_.initRectificationMap();}
+	bool isRectificationMapInitialized() {return left_.isRectificationMapInitialized() && right_.isRectificationMapInitialized();}
 
 	void setName(const std::string & name, const std::string & leftSuffix = "left", const std::string & rightSuffix = "right");
 	const std::string & name() const {return name_;}

corelib/src/CMakeLists.txt

@@ -83,9 +83,7 @@ SET(SRC_FILES
 		
 	rtflann/ext/lz4.c
 	rtflann/ext/lz4hc.c
-	FlannIndex.cpp
-	
-	sqlite3/sqlite3.c	
+	FlannIndex.cpp	
 	
 	#clams stuff
 	clams/discrete_depth_distortion_model_helpers.cpp
@@ -129,6 +127,26 @@ SET(LIBRARIES
 	${ZLIB_LIBRARIES} 
 )
 
+IF(Sqlite3_FOUND)
+    SET(INCLUDE_DIRS
+		${INCLUDE_DIRS}
+		${Sqlite3_INCLUDE_DIRS}
+	)
+	SET(LIBRARIES
+		${LIBRARIES}
+		${Sqlite3_LIBRARIES}
+	)
+ELSE()
+	SET(SRC_FILES
+		${SRC_FILES}
+		sqlite3/sqlite3.c
+	)
+	 SET(INCLUDE_DIRS
+	    ${CMAKE_CURRENT_SOURCE_DIR}/sqlite3
+		${INCLUDE_DIRS}
+	)
+ENDIF()
+
 IF(Freenect_FOUND)
 	IF(Freenect_DASH_INCLUDES)
 	   ADD_DEFINITIONS("-DFREENECT_DASH_INCLUDES")

corelib/src/CameraModel.cpp

@@ -31,6 +31,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #include <rtabmap/utilite/UFile.h>
 #include <rtabmap/utilite/UConversion.h>
 #include <rtabmap/utilite/UMath.h>
+#include <rtabmap/utilite/UStl.h>
 #include <opencv2/imgproc/imgproc.hpp>
 
 namespace rtabmap {
@@ -57,7 +58,7 @@ CameraModel::CameraModel(
 		localTransform_(localTransform)
 {
 	UASSERT(K_.empty() || (K_.rows == 3 && K_.cols == 3 && K_.type() == CV_64FC1));
-	UASSERT(D_.empty() || (D_.rows == 1 && (D_.cols == 4 || D_.cols == 5 || D_.cols == 8) && D_.type() == CV_64FC1));
+	UASSERT(D_.empty() || (D_.rows == 1 && (D_.cols == 4 || D_.cols == 5 || D_.cols == 6 || D_.cols == 8) && D_.type() == CV_64FC1));
 	UASSERT(R_.empty() || (R_.rows == 3 && R_.cols == 3 && R_.type() == CV_64FC1));
 	UASSERT(P_.empty() || (P_.rows == 3 && P_.cols == 4 && P_.type() == CV_64FC1));
 }
@@ -153,12 +154,33 @@ CameraModel::CameraModel(
 void CameraModel::initRectificationMap()
 {
 	UASSERT(imageSize_.height > 0 && imageSize_.width > 0);
-	UASSERT(D_.rows == 1 && (D_.cols == 4 || D_.cols == 5 || D_.cols == 8));
+	UASSERT(D_.rows == 1 && (D_.cols == 4 || D_.cols == 5 || D_.cols == 6 || D_.cols == 8));
 	UASSERT(R_.rows == 3 && R_.cols == 3);
 	UASSERT(P_.rows == 3 && P_.cols == 4);
 	// init rectification map
 	UINFO("Initialize rectify map");
-	cv::initUndistortRectifyMap(K_, D_, R_, P_, imageSize_, CV_32FC1, mapX_, mapY_);
+	if(D_.cols == 6)
+	{
+#if CV_MAJOR_VERSION > 2 or (CV_MAJOR_VERSION == 2 and (CV_MINOR_VERSION >4 or (CV_MINOR_VERSION == 4 and CV_SUBMINOR_VERSION >=10)))
+		// Equidistant / FishEye
+		// get only k parameters (k1,k2,p1,p2,k3,k4)
+		cv::Mat D(1, 4, CV_64FC1);
+		D.at<double>(0,0) = D_.at<double>(0,1);
+		D.at<double>(0,1) = D_.at<double>(0,2);
+		D.at<double>(0,2) = D_.at<double>(0,4);
+		D.at<double>(0,3) = D_.at<double>(0,5);
+		cv::fisheye::initUndistortRectifyMap(K_, D, R_, P_, imageSize_, CV_32FC1, mapX_, mapY_);
+	}
+	else
+#else
+		UWARN("Too old opencv version (%d,%d,%d) to support fisheye model (min 2.4.10 required)!",
+				CV_MAJOR_VERSION, CV_MINOR_VERSION, CV_SUBMINOR_VERSION);
+	}
+#endif
+	{
+		// RadialTangential
+		cv::initUndistortRectifyMap(K_, D_, R_, P_, imageSize_, CV_32FC1, mapX_, mapY_);
+	}
 }
 
 void CameraModel::setImageSize(const cv::Size & size)
@@ -263,6 +285,27 @@ bool CameraModel::load(const std::string & directory, const std::string & camera
 				UWARN("Missing \"distorsion_coefficients\" field in \"%s\"", filePath.c_str());
 			}
 
+			n = fs["distortion_model"];
+			if(n.type() != cv::FileNode::NONE)
+			{
+				std::string distortionModel = (std::string)n;
+				if(D_.cols>=4 &&
+				  (uStrContains(distortionModel, "fisheye") ||
+				   uStrContains(distortionModel, "equidistant")))
+				{
+					cv::Mat D = cv::Mat::zeros(1,6,CV_64FC1);
+					D.at<double>(0,0) = D_.at<double>(0,0);
+					D.at<double>(0,1) = D_.at<double>(0,1);
+					D.at<double>(0,4) = D_.at<double>(0,2);
+					D.at<double>(0,5) = D_.at<double>(0,3);
+					D_ = D;
+				}
+			}
+			else
+			{
+				UWARN("Missing \"distortion_model\" field in \"%s\"", filePath.c_str());
+			}
+
 			n = fs["rectification_matrix"];
 			if(n.type() != cv::FileNode::NONE)
 			{
@@ -347,20 +390,33 @@ bool CameraModel::save(const std::string & directory) const
 
 		if(!D_.empty())
 		{
+			cv::Mat D = D_;
+			if(D_.cols == 6)
+			{
+				D = cv::Mat(1,4,CV_64FC1);
+				D.at<double>(0,0) = D_.at<double>(0,0);
+				D.at<double>(0,1) = D_.at<double>(0,1);
+				D.at<double>(0,2) = D_.at<double>(0,4);
+				D.at<double>(0,3) = D_.at<double>(0,5);
+			}
 			fs << "distortion_coefficients" << "{";
-			fs << "rows" << D_.rows;
-			fs << "cols" << D_.cols;
-			fs << "data" << std::vector<double>((double*)D_.data, ((double*)D_.data)+(D_.rows*D_.cols));
+			fs << "rows" << D.rows;
+			fs << "cols" << D.cols;
+			fs << "data" << std::vector<double>((double*)D.data, ((double*)D.data)+(D.rows*D.cols));
 			fs << "}";
 
 			// compaibility with ROS
-			if(D_.cols > 5)
+			if(D_.cols == 6)
 			{
-				fs << "distortion_model" << "rational_polynomial";
+				fs << "distortion_model" << "equidistant"; // equidistant, fisheye
+			}
+			else if(D.cols > 5)
+			{
+				fs << "distortion_model" << "rational_polynomial"; // rad tan
 			}
 			else
 			{
-				fs << "distortion_model" << "plumb_bob";
+				fs << "distortion_model" << "plumb_bob"; // rad tan
 			}
 		}
 

corelib/src/CameraRGB.cpp

@@ -37,6 +37,10 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #include <rtabmap/utilite/UTimer.h>
 
 #include <opencv2/imgproc/imgproc.hpp>
+#include <opencv2/imgproc/types_c.h>
+#if CV_MAJOR_VERSION >= 3
+#include <opencv2/videoio/videoio_c.h>
+#endif
 
 #include <rtabmap/core/util3d.h>
 #include <rtabmap/core/util3d_filtering.h>
@@ -120,14 +124,8 @@ CameraImages::CameraImages(const std::string & path,
 CameraImages::~CameraImages()
 {
 	UDEBUG("");
-	if(_dir)
-	{
-		delete _dir;
-	}
-	if(_scanDir)
-	{
-		delete _scanDir;
-	}
+	delete _dir;
+	delete _scanDir;
 }
 
 bool CameraImages::init(const std::string & calibrationFolder, const std::string & cameraName)

corelib/src/CameraRGBD.cpp

@@ -39,6 +39,11 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #include <rtabmap/utilite/UTimer.h>
 #include <rtabmap/utilite/UMath.h>
 
+#include <opencv2/imgproc/types_c.h>
+#if CV_MAJOR_VERSION >= 3
+#include <opencv2/videoio/videoio_c.h>
+#endif
+
 #ifdef RTABMAP_OPENNI
 #include <pcl/io/openni_grabber.h>
 #include <pcl/io/oni_grabber.h>
@@ -1422,21 +1427,9 @@ CameraFreenect2::~CameraFreenect2()
 		dev_->close();
 		//deleted in freenect2_ destructor (Freeenect2Impl::clearDevices())
 	}
-	if(listener_)
-	{
-		delete listener_;
-	}
-
-	if(reg_)
-	{
-		delete reg_;
-		reg_ = 0;
-	}
-
-	if(freenect2_)
-	{
-		delete freenect2_;
-	}
+	delete listener_;
+	delete reg_;
+	delete freenect2_;
 	UDEBUG("");
 #endif
 }
@@ -2625,6 +2618,7 @@ CameraRealSense::CameraRealSense(
 	presetDepth_(presetDepth),
 	computeOdometry_(computeOdometry),
 	depthScaledToRGBSize_(false),
+	rgbSource_(kColor),
 	slam_(0)
 #endif
 {
@@ -2638,14 +2632,18 @@ CameraRealSense::~CameraRealSense()
 	UDEBUG("");
 	if(dev_)
 	{
-		if(slam_!=0)
+		try
 		{
-			dev_->stop(rs::source::all_sources);
-		}
-		else
-		{
-			dev_->stop();
+			if(slam_!=0)
+			{
+				dev_->stop(rs::source::all_sources);
+			}
+			else
+			{
+				dev_->stop();
+			}
 		}
+		catch(const rs::error & error){UWARN("%s", error.what());}
 		dev_ = 0;
 	}
 	UDEBUG("");
@@ -2692,6 +2690,118 @@ bool setStreamConfigIntrin(
 }
 #endif
 
+void CameraRealSense::setDepthScaledToRGBSize(bool enabled) {
+#ifdef RTABMAP_REALSENSE
+	depthScaledToRGBSize_ = enabled;
+#endif
+}
+void CameraRealSense::setRGBSource(RGBSource source)
+{
+#ifdef RTABMAP_REALSENSE
+	rgbSource_ = source;
+#endif
+}
+
+#ifdef RTABMAP_REALSENSE
+template<class GET_DEPTH, class TRANSFER_PIXEL> void align_images(const rs_intrinsics & depth_intrin, const rs_extrinsics & depth_to_other, const rs_intrinsics & other_intrin, GET_DEPTH get_depth, TRANSFER_PIXEL transfer_pixel)
+{
+	// Iterate over the pixels of the depth image
+#pragma omp parallel for schedule(dynamic)
+	for(int depth_y = 0; depth_y < depth_intrin.height; ++depth_y)
+	{
+		int depth_pixel_index = depth_y * depth_intrin.width;
+		for(int depth_x = 0; depth_x < depth_intrin.width; ++depth_x, ++depth_pixel_index)
+		{
+			// Skip over depth pixels with the value of zero, we have no depth data so we will not write anything into our aligned images
+			if(float depth = get_depth(depth_pixel_index))
+			{
+				// Map the top-left corner of the depth pixel onto the other image
+				float depth_pixel[2] = {depth_x-0.5f, depth_y-0.5f}, depth_point[3], other_point[3], other_pixel[2];
+				rs_deproject_pixel_to_point(depth_point, &depth_intrin, depth_pixel, depth);
+				rs_transform_point_to_point(other_point, &depth_to_other, depth_point);
+				rs_project_point_to_pixel(other_pixel, &other_intrin, other_point);
+				const int other_x0 = static_cast<int>(other_pixel[0] + 0.5f);
+				const int other_y0 = static_cast<int>(other_pixel[1] + 0.5f);
+
+				// Map the bottom-right corner of the depth pixel onto the other image
+				depth_pixel[0] = depth_x+0.5f; depth_pixel[1] = depth_y+0.5f;
+				rs_deproject_pixel_to_point(depth_point, &depth_intrin, depth_pixel, depth);
+				rs_transform_point_to_point(other_point, &depth_to_other, depth_point);
+				rs_project_point_to_pixel(other_pixel, &other_intrin, other_point);
+				const int other_x1 = static_cast<int>(other_pixel[0] + 0.5f);
+				const int other_y1 = static_cast<int>(other_pixel[1] + 0.5f);
+
+				if(other_x0 < 0 || other_y0 < 0 || other_x1 >= other_intrin.width || other_y1 >= other_intrin.height) continue;
+
+				// Transfer between the depth pixels and the pixels inside the rectangle on the other image
+				for(int y=other_y0; y<=other_y1; ++y) for(int x=other_x0; x<=other_x1; ++x) transfer_pixel(depth_pixel_index, y * other_intrin.width + x);
+			}
+		}
+	}
+}
+typedef uint8_t byte;
+
+void align_z_to_other(byte * z_aligned_to_other, const uint16_t * z_pixels, float z_scale, const rs_intrinsics & z_intrin, const rs_extrinsics & z_to_other, const rs_intrinsics & other_intrin)
+{
+	auto out_z = (uint16_t *)(z_aligned_to_other);
+	align_images(z_intrin, z_to_other, other_intrin,
+		[z_pixels, z_scale](int z_pixel_index) { return z_scale * z_pixels[z_pixel_index]; },
+		[out_z, z_pixels](int z_pixel_index, int other_pixel_index) { out_z[other_pixel_index] = out_z[other_pixel_index] ? std::min(out_z[other_pixel_index],z_pixels[z_pixel_index]) : z_pixels[z_pixel_index]; });
+}
+
+void align_disparity_to_other(byte * disparity_aligned_to_other, const uint16_t * disparity_pixels, float disparity_scale, const rs_intrinsics & disparity_intrin, const rs_extrinsics & disparity_to_other, const rs_intrinsics & other_intrin)
+{
+	auto out_disparity = (uint16_t *)(disparity_aligned_to_other);
+	align_images(disparity_intrin, disparity_to_other, other_intrin,
+		[disparity_pixels, disparity_scale](int disparity_pixel_index) { return disparity_scale / disparity_pixels[disparity_pixel_index]; },
+		[out_disparity, disparity_pixels](int disparity_pixel_index, int other_pixel_index) { out_disparity[other_pixel_index] = disparity_pixels[disparity_pixel_index]; });
+}
+
+template<int N> struct bytes { char b[N]; };
+template<int N, class GET_DEPTH> void align_other_to_depth_bytes(byte * other_aligned_to_depth, GET_DEPTH get_depth, const rs_intrinsics & depth_intrin, const rs_extrinsics & depth_to_other, const rs_intrinsics & other_intrin, const byte * other_pixels)
+{
+	auto in_other = (const bytes<N> *)(other_pixels);
+	auto out_other = (bytes<N> *)(other_aligned_to_depth);
+	align_images(depth_intrin, depth_to_other, other_intrin, get_depth,
+		[out_other, in_other](int depth_pixel_index, int other_pixel_index) { out_other[depth_pixel_index] = in_other[other_pixel_index]; });
+}
+
+/////////////////////////
+// Image rectification //
+/////////////////////////
+
+std::vector<int> compute_rectification_table(const rs_intrinsics & rect_intrin, const rs_extrinsics & rect_to_unrect, const rs_intrinsics & unrect_intrin)
+{
+	std::vector<int> rectification_table;
+	rectification_table.resize(rect_intrin.width * rect_intrin.height);
+	align_images(rect_intrin, rect_to_unrect, unrect_intrin, [](int) { return 1.0f; },
+		[&rectification_table](int rect_pixel_index, int unrect_pixel_index) { rectification_table[rect_pixel_index] = unrect_pixel_index; });
+	return rectification_table;
+}
+
+template<class T> void rectify_image_pixels(T * rect_pixels, const std::vector<int> & rectification_table, const T * unrect_pixels)
+{
+	for(auto entry : rectification_table) *rect_pixels++ = unrect_pixels[entry];
+}
+
+void rectify_image(uint8_t * rect_pixels, const std::vector<int> & rectification_table, const uint8_t * unrect_pixels, rs_format format)
+{
+	switch(format)
+	{
+	case RS_FORMAT_Y8:
+		return rectify_image_pixels((bytes<1> *)rect_pixels, rectification_table, (const bytes<1> *)unrect_pixels);
+	case RS_FORMAT_Y16: case RS_FORMAT_Z16:
+		return rectify_image_pixels((bytes<2> *)rect_pixels, rectification_table, (const bytes<2> *)unrect_pixels);
+	case RS_FORMAT_RGB8: case RS_FORMAT_BGR8:
+		return rectify_image_pixels((bytes<3> *)rect_pixels, rectification_table, (const bytes<3> *)unrect_pixels);
+	case RS_FORMAT_RGBA8: case RS_FORMAT_BGRA8:
+		return rectify_image_pixels((bytes<4> *)rect_pixels, rectification_table, (const bytes<4> *)unrect_pixels);
+	default:
+		assert(false); // NOTE: rectify_image_pixels(...) is not appropriate for RS_FORMAT_YUYV images, no logic prevents U/V channels from being written to one another
+	}
+}
+#endif
+
 bool CameraRealSense::init(const std::string & calibrationFolder, const std::string & cameraName)
 {
 	UDEBUG("");
@@ -2699,10 +2809,22 @@ bool CameraRealSense::init(const std::string & calibrationFolder, const std::str
 
 	if(dev_)
 	{
-		dev_->stop(rs::source::all_sources);
+		try
+		{
+			if(slam_!=0)
+			{
+				dev_->stop(rs::source::all_sources);
+			}
+			else
+			{
+				dev_->stop();
+			}
+		}
+		catch(const rs::error & error){UWARN("%s", error.what());}
 		dev_ = 0;
 	}
 	bufferedFrames_.clear();
+	rsRectificationTable_.clear();
 
 #ifdef RTABMAP_REALSENSE_SLAM
 	motionSeq_[0] = motionSeq_[1] = 0;
@@ -2742,29 +2864,78 @@ bool CameraRealSense::init(const std::string & calibrationFolder, const std::str
 	UINFO("    Preset RGB: %d", presetRGB_);
 	UINFO("    Preset Depth: %d", presetDepth_);
 
-	bool computeOdometry = false;
-#ifdef RTABMAP_REALSENSE_SLAM
-	if (name.find("ZR300") != std::string::npos && computeOdometry_)
-	{
-		// Only enable ZR300 functionality if fisheye stream is enabled.
-		// Accel/Gyro automatically enabled when fisheye requested
-		computeOdometry = true;
-	}
+#ifndef RTABMAP_REALSENSE_SLAM
+	computeOdometry_ = false;
 #endif
 
+	if (name.find("ZR300") == std::string::npos)
+	{
+		// Only enable ZR300 functionality odometry if fisheye stream is enabled.
+		// Accel/Gyro automatically enabled when fisheye requested
+		computeOdometry_ = false;
+		// Only ZR300 has fisheye
+		if(rgbSource_ == kFishEye)
+		{
+			UWARN("Fisheye cannot be used with %s camera, using color instead...", name.c_str());
+			rgbSource_ = kColor;
+		}
+	}
+
+	rs::intrinsics depth_intrin;
+	rs::intrinsics fisheye_intrin;
+	rs::intrinsics color_intrin;
 	// Configure depth and color to run with the device's preferred settings
 	UINFO("Enabling streams...");
 	// R200: 
 	//    0=640x480   vs 480x360
 	//    1=1920x1080 vs 640x480
 	//    2=640x480   vs 320x240
-	dev_->enable_stream(rs::stream::depth, (rs::preset)presetDepth_);
-	dev_->enable_stream(rs::stream::color, (rs::preset)presetRGB_);
+	try {
 
-	rs::intrinsics depth_intrin = dev_->get_stream_intrinsics(rs::stream::depth);
-	rs::intrinsics color_intrin = dev_->get_stream_intrinsics(rs::stream::color);
-	UINFO("    RGB:   %dx%d", color_intrin.width, color_intrin.height);
-	UINFO("    Depth: %dx%d", depth_intrin.width, depth_intrin.height);
+		// left/rgb stream
+		if(rgbSource_==kFishEye || computeOdometry_)
+		{
+			dev_->enable_stream(rs::stream::fisheye, 640, 480, rs::format::raw8, 30);
+			if(computeOdometry_)
+			{
+				// Needed to align image timestamps to common clock-domain with the motion events
+				dev_->set_option(rs::option::fisheye_strobe, 1);
+			}
+			// This option causes the fisheye image to be acquired in-sync with the depth image.
+			dev_->set_option(rs::option::fisheye_external_trigger, 1);
+			dev_->set_option(rs::option::fisheye_color_auto_exposure, 1);
+			fisheye_intrin = dev_->get_stream_intrinsics(rs::stream::fisheye);
+			UINFO("    Fisheye:   %dx%d", fisheye_intrin.width, fisheye_intrin.height);
+			if(rgbSource_==kFishEye)
+			{
+				color_intrin = fisheye_intrin; // not rectified
+			}
+		}
+		if(rgbSource_!=kFishEye)
+		{
+			dev_->enable_stream(rs::stream::color, (rs::preset)presetRGB_);
+			color_intrin = dev_->get_stream_intrinsics(rs::stream::rectified_color); // rectified
+			UINFO("    RGB:   %dx%d", color_intrin.width, color_intrin.height);
+
+			if(rgbSource_==kInfrared)
+			{
+				dev_->enable_stream(rs::stream::infrared, (rs::preset)presetRGB_);
+				color_intrin = dev_->get_stream_intrinsics(rs::stream::infrared); // rectified
+				UINFO("    IR left:   %dx%d", color_intrin.width, color_intrin.height);
+			}
+		}
+
+		dev_->enable_stream(rs::stream::depth, (rs::preset)presetDepth_);
+		depth_intrin = dev_->get_stream_intrinsics(rs::stream::depth); // rectified
+		UINFO("    Depth: %dx%d", depth_intrin.width, depth_intrin.height);
+	}
+	catch(const rs::error & error)
+	{
+		UERROR("Failed starting the streams: %s", error.what());
+		return false;
+	}
+
+	Transform imu2Camera = Transform::getIdentity();
 
 #ifdef RTABMAP_REALSENSE_SLAM
 	UDEBUG("Setup frame callback");
@@ -2794,9 +2965,50 @@ bool CameraRealSense::init(const std::string & calibrationFolder, const std::str
 		  frame.get_stride()
 		};
 		cv::Mat image;
-		if(frame.get_format() == rs::format::raw8)
+		if(frame.get_format() == rs::format::raw8 || frame.get_format() == rs::format::y8)
 		{
 			image = cv::Mat(height, width, CV_8UC1, (unsigned char*)frame.get_data());
+			if(frame.get_stream_type() == rs::stream::fisheye)
+			{
+				// fisheye always received just after the depth image (doesn't have exact timestamp with depth)
+				if(bufferedFrames_.size())
+				{
+					bufferedFrames_.rbegin()->second.first = image.clone();
+					UScopeMutex lock(dataMutex_);
+					bool notify = lastSyncFrames_.first.empty();
+					lastSyncFrames_ = bufferedFrames_.rbegin()->second;
+					if(notify)
+					{
+						dataReady_.release();
+					}
+					bufferedFrames_.clear();
+				}
+			}
+			else if(frame.get_stream_type() == rs::stream::infrared) // infrared (does have exact timestamp with depth)
+			{
+				if(bufferedFrames_.find(frame.get_timestamp()) != bufferedFrames_.end())
+				{
+					bufferedFrames_.find(frame.get_timestamp())->second.first = image.clone();
+					UScopeMutex lock(dataMutex_);
+					bool notify = lastSyncFrames_.first.empty();
+					lastSyncFrames_ = bufferedFrames_.find(frame.get_timestamp())->second;
+					if(notify)
+					{
+						dataReady_.release();
+					}
+					bufferedFrames_.erase(frame.get_timestamp());
+				}
+				else
+				{
+					bufferedFrames_.insert(std::make_pair(frame.get_timestamp(), std::make_pair(image.clone(), cv::Mat())));
+				}
+				if(bufferedFrames_.size()>5)
+				{
+					UWARN("Frames cannot be synchronized!");
+					bufferedFrames_.clear();
+				}
+				return;
+			}
 		}
 		else if(frame.get_format() == rs::format::z16)
 		{
@@ -2825,7 +3037,15 @@ bool CameraRealSense::init(const std::string & calibrationFolder, const std::str
 		}
 		else if(frame.get_format() == rs::format::rgb8)
 		{
-			image = cv::Mat(height, width, CV_8UC3, (unsigned char*)frame.get_data());
+			if(rsRectificationTable_.size())
+			{
+				image = cv::Mat(height, width, CV_8UC3);
+				rectify_image(image.data, rsRectificationTable_, (unsigned char*)frame.get_data(), (rs_format)frame.get_format());
+			}
+			else
+			{
+				image = cv::Mat(height, width, CV_8UC3, (unsigned char*)frame.get_data());
+			}
 			if(bufferedFrames_.find(frame.get_timestamp()) != bufferedFrames_.end())
 			{
 				bufferedFrames_.find(frame.get_timestamp())->second.first = image.clone();
@@ -2875,26 +3095,27 @@ bool CameraRealSense::init(const std::string & calibrationFolder, const std::str
 		}
 	};
 
+	UDEBUG("");
 	// Setup stream callback for stream
-	if(computeOdometry)
+	if(computeOdometry_ || rgbSource_ == kFishEye)
 	{
 		dev_->set_frame_callback(rs::stream::fisheye, frameCallback);
 	}
-	dev_->set_frame_callback(rs::stream::depth, frameCallback);
-	dev_->set_frame_callback(rs::stream::color, frameCallback);
-
-	if (computeOdometry)
+	if(rgbSource_ == kInfrared)
 	{
-		dev_->enable_stream(rs::stream::fisheye, 640, 480, rs::format::raw8, 30);
-		rs::intrinsics fisheye_intrin = dev_->get_stream_intrinsics(rs::stream::fisheye);
-		UINFO("    Fish: %dx%d", fisheye_intrin.width, fisheye_intrin.height);
+		dev_->set_frame_callback(rs::stream::infrared, frameCallback);
+	}
+	else if(rgbSource_ == kColor)
+	{
+		dev_->set_frame_callback(rs::stream::color, frameCallback);
+	}
 
-		// Needed to align image timestamps to common clock-domain with the motion events
-		dev_->set_option(rs::option::fisheye_strobe, 1);
-		// This option causes the fisheye image to be aquired in-sync with the depth image.
-		dev_->set_option(rs::option::fisheye_external_trigger, 1);
-		dev_->set_option(rs::option::fisheye_color_auto_exposure, 1);
+	dev_->set_frame_callback(rs::stream::depth, frameCallback);
 
+	UDEBUG("");
+
+	if (computeOdometry_)
+	{
 		UDEBUG("Setup motion callback");
 		//define callback to the motion events and set it.
 		std::function<void(rs::motion_data)> motion_callback;
@@ -3002,23 +3223,137 @@ bool CameraRealSense::init(const std::string & calibrationFolder, const std::str
 			return false;
 		}
 
-		dev_->start(rs::source::all_sources);
+		rs::extrinsics fisheye2imu = dev_->get_motion_extrinsics_from(rs::stream::fisheye);
+		imu2Camera = Transform(
+				fisheye2imu.rotation[0], fisheye2imu.rotation[1], fisheye2imu.rotation[2], fisheye2imu.translation[0],
+				fisheye2imu.rotation[3], fisheye2imu.rotation[4], fisheye2imu.rotation[5], fisheye2imu.translation[1],
+				fisheye2imu.rotation[6], fisheye2imu.rotation[7], fisheye2imu.rotation[8], fisheye2imu.translation[2]).inverse();
+
+		if(rgbSource_ == kInfrared)
+		{
+			rs::extrinsics color2Fisheye = dev_->get_extrinsics(rs::stream::fisheye, rs::stream::infrared);
+			Transform fisheye2Color = Transform(
+					color2Fisheye.rotation[0], color2Fisheye.rotation[1], color2Fisheye.rotation[2], color2Fisheye.translation[0],
+					color2Fisheye.rotation[3], color2Fisheye.rotation[4], color2Fisheye.rotation[5], color2Fisheye.translation[1],
+					color2Fisheye.rotation[6], color2Fisheye.rotation[7], color2Fisheye.rotation[8], color2Fisheye.translation[2]).inverse();
+			imu2Camera *= fisheye2Color;
+		}
+		else if(rgbSource_ == kColor)
+		{
+			rs::extrinsics color2Fisheye = dev_->get_extrinsics(rs::stream::fisheye, rs::stream::rectified_color);
+			Transform fisheye2Color = Transform(
+					color2Fisheye.rotation[0], color2Fisheye.rotation[1], color2Fisheye.rotation[2], color2Fisheye.translation[0],
+					color2Fisheye.rotation[3], color2Fisheye.rotation[4], color2Fisheye.rotation[5], color2Fisheye.translation[1],
+					color2Fisheye.rotation[6], color2Fisheye.rotation[7], color2Fisheye.rotation[8], color2Fisheye.translation[2]).inverse();
+			imu2Camera *= fisheye2Color;
+		}
+
+		UDEBUG("start device!");
+		try
+		{
+			dev_->start(rs::source::all_sources);
+		}
+		catch(const rs::error & error)
+		{
+			UERROR("Failed starting the device: %s (try to unplug/plug the camera)", error.what());
+			return false;
+		}
 	}
 	else
 	{
-		dev_->start();
+		UDEBUG("start device!");
+		try
+		{
+			dev_->start();
+		}
+		catch(const rs::error & error)
+		{
+			UERROR("Failed starting the device: %s (try to unplug/plug the camera)", error.what());
+			return false;
+		}
 	}
 #else
-	dev_->start();
 	try {
+		dev_->start();
 		dev_->wait_for_frames();
 	}
 	catch (const rs::error & e)
 	{
-		UERROR("Exception: %s", e.what());
+		UERROR("Exception: %s (try to unplug/plug the camera)", e.what());
 	}
 #endif
 
+	cv::Mat D;
+	if(rgbSource_ == kFishEye)
+	{
+		// ftheta/equidistant model
+		D = cv::Mat::zeros(1,6,CV_64FC1);
+		D.at<double>(0,0) = color_intrin.coeffs[0];
+		D.at<double>(0,1) = color_intrin.coeffs[1];
+		D.at<double>(0,4) = color_intrin.coeffs[2];
+		D.at<double>(0,5) = color_intrin.coeffs[3];
+	}
+	else
+	{
+		// Brown-Conrady / radtan
+		D = cv::Mat::zeros(1,5,CV_64FC1);
+		D.at<double>(0,0) = color_intrin.coeffs[0];
+		D.at<double>(0,1) = color_intrin.coeffs[1];
+		D.at<double>(0,2) = color_intrin.coeffs[2];
+		D.at<double>(0,3) = color_intrin.coeffs[3];
+		D.at<double>(0,4) = color_intrin.coeffs[4];
+	}
+	cv::Mat K = cv::Mat::eye(3, 3, CV_64FC1);
+	K.at<double>(0,0) = color_intrin.fx;
+	K.at<double>(1,1) = color_intrin.fy;
+	K.at<double>(0,2) = color_intrin.ppx;
+	K.at<double>(1,2) = color_intrin.ppy;
+	cv::Mat R = cv::Mat::eye(3, 3, CV_64FC1);
+	cv::Mat P = cv::Mat::eye(3, 4, CV_64FC1);
+	K(cv::Range(0,2), cv::Range(0,3)).copyTo(P(cv::Range(0,2), cv::Range(0,3)));
+	cameraModel_ = CameraModel(
+			dev_->get_name(),
+			cv::Size(color_intrin.width, color_intrin.height),
+			K,
+			D,
+			R,
+			P,
+			this->getLocalTransform()*imu2Camera);
+
+	UDEBUG("");
+
+	if(rgbSource_ == kColor)
+	{
+		rs::extrinsics rect_to_unrect = dev_->get_extrinsics(rs::stream::rectified_color, rs::stream::color);
+		rs::intrinsics unrect_intrin = dev_->get_stream_intrinsics(rs::stream::color);
+		rsRectificationTable_ = compute_rectification_table(color_intrin, rect_to_unrect, unrect_intrin);
+	}
+	else if(rgbSource_ == kFishEye)
+	{
+		UINFO("calibration folder=%s name=%s", calibrationFolder.c_str(), cameraName.c_str());
+		if(!calibrationFolder.empty() && !cameraName.empty())
+		{
+			CameraModel model;
+			if(!model.load(calibrationFolder, cameraName))
+			{
+				UWARN("Failed to load calibration \"%s\" in \"%s\" folder, you should calibrate the camera!",
+						cameraName.c_str(), calibrationFolder.c_str());
+			}
+			else
+			{
+				UINFO("Camera parameters: fx=%f fy=%f cx=%f cy=%f",
+						model.fx(),
+						model.fy(),
+						model.cx(),
+						model.cy());
+				cameraModel_ = model;
+				cameraModel_.setName(cameraName);
+				cameraModel_.initRectificationMap();
+				cameraModel_.setLocalTransform(this->getLocalTransform()*imu2Camera);
+			}
+		}
+	}
+
 	uSleep(1000); // ignore the first frames
 	UINFO("Enabling streams...done!");
 
@@ -3080,8 +3415,23 @@ SensorData CameraRealSense::captureImage(CameraInfo * info)
 
 		// Retrieve camera parameters for mapping between depth and color
 		rs::intrinsics depth_intrin = dev_->get_stream_intrinsics(rs::stream::depth);
-		rs::extrinsics depth_to_color = dev_->get_extrinsics(rs::stream::depth, rs::stream::color);
-		rs::intrinsics color_intrin = dev_->get_stream_intrinsics(rs::stream::color);
+		rs::extrinsics depth_to_color;
+		rs::intrinsics color_intrin;
+		if(rgbSource_ == kFishEye)
+		{
+			depth_to_color = dev_->get_extrinsics(rs::stream::depth, rs::stream::fisheye);
+			color_intrin = dev_->get_stream_intrinsics(rs::stream::fisheye);
+		}
+		else if(rgbSource_ == kInfrared)
+		{
+			depth_to_color = dev_->get_extrinsics(rs::stream::depth, rs::stream::infrared);
+			color_intrin = dev_->get_stream_intrinsics(rs::stream::infrared);
+		}
+		else // color
+		{
+			depth_to_color = dev_->get_extrinsics(rs::stream::depth, rs::stream::rectified_color);
+			color_intrin = dev_->get_stream_intrinsics(rs::stream::rectified_color);
+		}
 
 #ifdef RTABMAP_REALSENSE_SLAM
 		if(!dataReady_.acquire(1, 5000))
@@ -3089,6 +3439,7 @@ SensorData CameraRealSense::captureImage(CameraInfo * info)
 			UWARN("Not received new frames since 5 seconds, end of stream reached!");
 			return data;
 		}
+
 		{
 			UScopeMutex lock(dataMutex_);
 			rgb = lastSyncFrames_.first;
@@ -3113,84 +3464,124 @@ SensorData CameraRealSense::captureImage(CameraInfo * info)
 
 		// Retrieve our images
 		depthIn = cv::Mat(depth_intrin.height, depth_intrin.width, CV_16UC1, (unsigned char*)dev_->get_frame_data(rs::stream::depth));
-		rgb = cv::Mat(color_intrin.height, color_intrin.width, CV_8UC3, (unsigned char*)dev_->get_frame_data(rs::stream::color));
-#endif
-
-		float scale = dev_->get_depth_scale();
-
-		// factory registration...
-		cv::Mat bgr;
-		cv::cvtColor(rgb, bgr, CV_RGB2BGR);
-
-		CameraModel model(
-			color_intrin.fx, //fx
-			color_intrin.fy, //fy
-			color_intrin.ppx,  //cx
-			color_intrin.ppy,  //cy
-			this->getLocalTransform(),
-			0,
-			bgr.size());
-
-		cv::Mat depth;
-		if (color_intrin.width % depth_intrin.width == 0 && color_intrin.height % depth_intrin.height == 0 &&
-			depth_intrin.width < color_intrin.width &&
-			depth_intrin.height < color_intrin.height &&
-			!depthScaledToRGBSize_)
+		if(rgbSource_ == kFishEye)
 		{
-			//we can keep the depth image size as is
-			depth = cv::Mat::zeros(cv::Size(depth_intrin.width, depth_intrin.height), CV_16UC1);
-			float scaleX = float(depth_intrin.width) / float(color_intrin.width);
-			float scaleY = float(depth_intrin.height) / float(color_intrin.height);
-			color_intrin.fx *= scaleX;
-			color_intrin.fy *= scaleY;
-			color_intrin.ppx *= scaleX;
-			color_intrin.ppy *= scaleY;
-			color_intrin.height = depth_intrin.height;
-			color_intrin.width = depth_intrin.width;
+			rgb = cv::Mat(color_intrin.height, color_intrin.width, CV_8UC1, (unsigned char*)dev_->get_frame_data(rs::stream::fisheye));
+		}
+		else if(rgbSource_ == kInfrared)
+		{
+			rgb = cv::Mat(color_intrin.height, color_intrin.width, CV_8UC1, (unsigned char*)dev_->get_frame_data(rs::stream::infrared));
 		}
 		else
 		{
-			//depth to color
-			depth = cv::Mat::zeros(bgr.size(), CV_16UC1);
+			rgb = cv::Mat(color_intrin.height, color_intrin.width, CV_8UC3, (unsigned char*)dev_->get_frame_data(rs::stream::color));
 		}
-		for (int dy = 0; dy < depth_intrin.height; ++dy)
+#endif
+
+		// factory registration...
+		cv::Mat bgr;
+		if(rgbSource_ != kColor)
 		{
-			for (int dx = 0; dx < depth_intrin.width; ++dx)
+			bgr = rgb;
+		}
+		else
+		{
+			cv::cvtColor(rgb, bgr, CV_RGB2BGR);
+		}
+
+		bool rectified = false;
+		if(rgbSource_ == kFishEye && cameraModel_.isRectificationMapInitialized())
+		{
+			bgr = cameraModel_.rectifyImage(bgr);
+			rectified = true;
+			color_intrin.fx = cameraModel_.fx();
+			color_intrin.fy = cameraModel_.fy();
+			color_intrin.ppx = cameraModel_.cx();
+			color_intrin.ppy = cameraModel_.cy();
+			UASSERT_MSG(color_intrin.width == cameraModel_.imageWidth() && color_intrin.height == cameraModel_.imageHeight(),
+					uFormat("color_intrin=%dx%d cameraModel_=%dx%d",
+							color_intrin.width, color_intrin.height, cameraModel_.imageWidth(), cameraModel_.imageHeight()).c_str());
+			((rs_intrinsics*)&color_intrin)->model = RS_DISTORTION_NONE;
+		}
+#ifndef RTABMAP_REALSENSE_SLAM
+		else if(rgbSource_ != kColor)
+		{
+			bgr = bgr.clone();
+		}
+#endif
+
+		cv::Mat depth;
+		if(rgbSource_ != kFishEye || rectified)
+		{
+			if (color_intrin.width % depth_intrin.width == 0 && color_intrin.height % depth_intrin.height == 0 &&
+				depth_intrin.width < color_intrin.width &&
+				depth_intrin.height < color_intrin.height &&
+				!depthScaledToRGBSize_)
 			{
-				// Retrieve the 16-bit depth value and map it into a depth in meters
-				uint16_t depth_value = depthIn.at<unsigned short>(dy,dx);
-				float depth_in_meters = depth_value * scale;
+				//we can keep the depth image size as is
+				depth = cv::Mat::zeros(cv::Size(depth_intrin.width, depth_intrin.height), CV_16UC1);
+				float scaleX = float(depth_intrin.width) / float(color_intrin.width);
+				float scaleY = float(depth_intrin.height) / float(color_intrin.height);
+				color_intrin.fx *= scaleX;
+				color_intrin.fy *= scaleY;
+				color_intrin.ppx *= scaleX;
+				color_intrin.ppy *= scaleY;
+				color_intrin.height = depth_intrin.height;
+				color_intrin.width = depth_intrin.width;
+			}
+			else
+			{
+				//depth to color
+				depth = cv::Mat::zeros(bgr.size(), CV_16UC1);
+			}
 
-				// Skip over pixels with a depth value of zero, which is used to indicate no data
-				if (depth_value == 0 || depth_in_meters>10.0f) continue;
-
-				// Map from pixel coordinates in the depth image to pixel coordinates in the color image
-				rs::float2 depth_pixel = { (float)dx, (float)dy };
-				rs::float3 depth_point = depth_intrin.deproject(depth_pixel, depth_in_meters);
-				rs::float3 color_point = depth_to_color.transform(depth_point);
-				rs::float2 color_pixel = color_intrin.project(color_point);
-
-				int pdx = color_pixel.x;
-				int pdy = color_pixel.y;
-				if (uIsInBounds(pdx, 0, depth.cols) && uIsInBounds(pdy, 0, depth.rows))
+			float scale = dev_->get_depth_scale();
+			for (int dy = 0; dy < depth_intrin.height; ++dy)
+			{
+				for (int dx = 0; dx < depth_intrin.width; ++dx)
 				{
-					depth.at<unsigned short>(pdy, pdx) = (unsigned short)(depth_in_meters*1000.0f); // convert to mm
+					// Retrieve the 16-bit depth value and map it into a depth in meters
+					uint16_t depth_value = depthIn.at<unsigned short>(dy,dx);
+					float depth_in_meters = depth_value * scale;
+
+					// Skip over pixels with a depth value of zero, which is used to indicate no data
+					if (depth_value == 0 || depth_in_meters>10.0f) continue;
+
+					// Map from pixel coordinates in the depth image to pixel coordinates in the color image
+					int pdx = dx;
+					int pdy = dy;
+					if(rgbSource_ == kColor || rgbSource_ == kFishEye)
+					{
+						rs::float2 depth_pixel = { (float)dx, (float)dy };
+						rs::float3 depth_point = depth_intrin.deproject(depth_pixel, depth_in_meters);
+						rs::float3 color_point = depth_to_color.transform(depth_point);
+						rs::float2 color_pixel = color_intrin.project(color_point);
+
+						pdx = color_pixel.x;
+						pdy = color_pixel.y;
+					}
+					//else infrared is already registered
+
+					if (uIsInBounds(pdx, 0, depth.cols) && uIsInBounds(pdy, 0, depth.rows))
+					{
+						depth.at<unsigned short>(pdy, pdx) = (unsigned short)(depth_in_meters*1000.0f); // convert to mm
+					}
 				}
 			}
+
+			if (color_intrin.width > depth_intrin.width)
+			{
+				// Fill holes
+				UTimer time;
+				util2d::fillRegisteredDepthHoles(depth, true, true, color_intrin.width > depth_intrin.width * 2);
+				util2d::fillRegisteredDepthHoles(depth, true, true, color_intrin.width > depth_intrin.width * 2);//second pass
+				UDEBUG("Filling depth holes: %fs", time.ticks());
+			}
 		}
 
-		if (color_intrin.width > depth_intrin.width)
+		if (!bgr.empty() && ((rgbSource_==kFishEye && !rectified) || !depth.empty()))
 		{
-			// Fill holes
-			UTimer time;
-			util2d::fillRegisteredDepthHoles(depth, true, true, color_intrin.width > depth_intrin.width * 2);
-			util2d::fillRegisteredDepthHoles(depth, true, true, color_intrin.width > depth_intrin.width * 2);//second pass
-			UDEBUG("Filling depth holes: %fs", time.ticks());
-		}
-		
-		if (!bgr.empty() && !depth.empty())
-		{
-			data = SensorData(bgr, depth, model, this->getNextSeqID(), UTimer::now());
+			data = SensorData(bgr, depth, cameraModel_, this->getNextSeqID(), UTimer::now());
 #ifdef RTABMAP_REALSENSE_SLAM
 			if(info && slam_)
 			{
@@ -3198,8 +3589,20 @@ SensorData CameraRealSense::captureImage(CameraInfo * info)
 				rs::slam::PoseMatrix4f pose;
 				if(slam_->get_camera_pose(pose) == rs::core::status_no_error)
 				{
-					Transform opticalRotation(0,0,1,0, -1,0,0,0, 0,-1,0,0);
-					info->odomPose = opticalRotation * rsPoseToTransform(pose) * opticalRotation.inverse();
+					/*rs::slam::tracking_accuracy accuracy = slam_->get_tracking_accuracy();
+					if( accuracy == rs::slam::tracking_accuracy::low ||
+						accuracy == rs::slam::tracking_accuracy::medium ||
+						accuracy == rs::slam::tracking_accuracy::high)*/
+					{
+						// the pose is in camera link or IMU frame, get pose of the color camera
+						Transform opticalRotation(0,0,1,0, -1,0,0,0, 0,-1,0,0);
+						info->odomPose = opticalRotation * rsPoseToTransform(pose) * opticalRotation.inverse();
+						info->odomCovariance = cv::Mat::eye(6, 6, CV_64FC1) * 0.0005;
+					}
+					/*else
+					{
+						UERROR("Odometry failed: accuracy=%d", accuracy);
+					}*/
 				}
 				else
 				{

corelib/src/CameraStereo.cpp

@@ -39,6 +39,11 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #include <rtabmap/utilite/UTimer.h>
 #include <rtabmap/utilite/UMath.h>
 
+#include <opencv2/imgproc/types_c.h>
+#if CV_MAJOR_VERSION >= 3
+#include <opencv2/videoio/videoio_c.h>
+#endif
+
 #ifdef RTABMAP_DC1394
 #include <dc1394/dc1394.h>
 #endif
@@ -361,10 +366,7 @@ CameraStereoDC1394::CameraStereoDC1394(float imageRate, const Transform & localT
 CameraStereoDC1394::~CameraStereoDC1394()
 {
 #ifdef RTABMAP_DC1394
-	if(device_)
-	{
-		delete device_;
-	}
+	delete device_;
 #endif
 }
 
@@ -829,10 +831,7 @@ CameraStereoZed::CameraStereoZed(
 CameraStereoZed::~CameraStereoZed()
 {
 #ifdef RTABMAP_ZED
-	if(zed_)
-	{
-		delete zed_;
-	}
+	delete zed_;
 #endif
 }
 
@@ -1150,10 +1149,7 @@ CameraStereoImages::CameraStereoImages(
 CameraStereoImages::~CameraStereoImages()
 {
 	UDEBUG("");
-	if(camera2_)
-	{
-		delete camera2_;
-	}
+	delete camera2_;
 	UDEBUG("");
 }
 

corelib/src/CameraThread.cpp

@@ -73,14 +73,8 @@ CameraThread::~CameraThread()
 {
 	UDEBUG("");
 	join(true);
-	if(_camera)
-	{
-		delete _camera;
-	}
-	if(_distortionModel)
-	{
-		delete _distortionModel;
-	}
+	delete _camera;
+	delete _distortionModel;
 	delete _stereoDense;
 }
 

corelib/src/DBDriver.cpp

@@ -29,12 +29,12 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 
 #include "rtabmap/core/Signature.h"
 #include "rtabmap/core/VisualWord.h"
+#include "rtabmap/core/DBDriverSqlite3.h"
 #include "rtabmap/utilite/UConversion.h"
 #include "rtabmap/utilite/UMath.h"
 #include "rtabmap/utilite/ULogger.h"
 #include "rtabmap/utilite/UTimer.h"
 #include "rtabmap/utilite/UStl.h"
-#include "DBDriverSqlite3.h"
 
 namespace rtabmap {
 

corelib/src/DBDriverSqlite3.cpp

@@ -25,7 +25,8 @@ ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
 SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 */
 
-#include "DBDriverSqlite3.h"
+#include "rtabmap/core/DBDriverSqlite3.h"
+#include <sqlite3.h>
 
 #include "rtabmap/core/Signature.h"
 #include "rtabmap/core/VisualWord.h"
@@ -42,8 +43,8 @@ namespace rtabmap {
 DBDriverSqlite3::DBDriverSqlite3(const ParametersMap & parameters) :
 	DBDriver(parameters),
 	_ppDb(0),
-	_memoryUsedEstimate(0),
 	_version("0.0.0"),
+	_memoryUsedEstimate(0),
 	_dbInMemory(Parameters::defaultDbSqlite3InMemory()),
 	_cacheSize(Parameters::defaultDbSqlite3CacheSize()),
 	_journalMode(Parameters::defaultDbSqlite3JournalMode()),
@@ -3957,7 +3958,7 @@ void DBDriverSqlite3::saveQuery(const std::list<VisualWord *> & words) const
 
 					//execute query
 					rc=sqlite3_step(ppStmt);
-					UASSERT_MSG(rc == SQLITE_DONE, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
+					UASSERT_MSG(rc == SQLITE_DONE, uFormat("DB error (%s): %s (word=%d)", _version.c_str(), sqlite3_errmsg(_ppDb), w->id()).c_str());
 
 					rc = sqlite3_reset(ppStmt);
 					UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());

corelib/src/DBReader.cpp

@@ -27,7 +27,6 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 
 #include "rtabmap/core/DBReader.h"
 #include "rtabmap/core/DBDriver.h"
-#include "DBDriverSqlite3.h"
 
 #include <rtabmap/utilite/ULogger.h>
 #include <rtabmap/utilite/UFile.h>
@@ -131,7 +130,7 @@ bool DBReader::init(
 
 	rtabmap::ParametersMap parameters;
 	parameters.insert(rtabmap::ParametersPair(rtabmap::Parameters::kDbSqlite3InMemory(), "false"));
-	_dbDriver = new DBDriverSqlite3(parameters);
+	_dbDriver = DBDriver::create(parameters);
 	if(!_dbDriver)
 	{
 		UERROR("Driver doesn't exist.");

corelib/src/Memory.cpp

@@ -61,6 +61,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #include <pcl/io/pcd_io.h>
 #include <pcl/common/common.h>
 #include <rtabmap/core/OccupancyGrid.h>
+#include <opencv2/imgproc/types_c.h>
 
 namespace rtabmap {
 
@@ -103,6 +104,8 @@ Memory::Memory(const ParametersMap & parameters) :
 	_visMaxFeatures(Parameters::defaultVisMaxFeatures()),
 	_visCorType(Parameters::defaultVisCorType()),
 	_imagesAlreadyRectified(Parameters::defaultRtabmapImagesAlreadyRectified()),
+	_rectifyOnlyFeatures(Parameters::defaultRtabmapRectifyOnlyFeatures()),
+	_covOffDiagonalIgnored(Parameters::defaultMemCovOffDiagIgnored()),
 	_idCount(kIdStart),
 	_idMapCount(kIdStart),
 	_lastSignature(0),
@@ -429,26 +432,11 @@ Memory::~Memory()
 	{
 		UWARN("Please call Memory::close() before");
 	}
-	if(_feature2D)
-	{
-		delete _feature2D;
-	}
-	if(_vwd)
-	{
-		delete _vwd;
-	}
-	if(_registrationPipeline)
-	{
-		delete _registrationPipeline;
-	}
-	if(_registrationIcpMulti)
-	{
-		delete _registrationIcpMulti;
-	}
-	if(_occupancy)
-	{
-		delete _occupancy;
-	}
+	delete _feature2D;
+	delete _vwd;
+	delete _registrationPipeline;
+	delete _registrationIcpMulti;
+	delete _occupancy;
 }
 
 void Memory::parseParameters(const ParametersMap & parameters)
@@ -499,6 +487,8 @@ void Memory::parseParameters(const ParametersMap & parameters)
 		uInsert(params, ParametersPair(Parameters::kVisCorType(), "0"));
 	}
 	Parameters::parse(params, Parameters::kRtabmapImagesAlreadyRectified(), _imagesAlreadyRectified);
+	Parameters::parse(params, Parameters::kRtabmapRectifyOnlyFeatures(), _rectifyOnlyFeatures);
+	Parameters::parse(params, Parameters::kMemCovOffDiagIgnored(), _covOffDiagonalIgnored);
 
 
 	UASSERT_MSG(_maxStMemSize >= 0, uFormat("value=%d", _maxStMemSize).c_str());
@@ -811,7 +801,30 @@ void Memory::addSignatureToStm(Signature * signature, const cv::Mat & covariance
 				   !_signatures.at(*_stMem.rbegin())->getPose().isNull())
 				{
 					UASSERT(covariance.cols == 6 && covariance.rows == 6 && covariance.type() == CV_64FC1);
-					cv::Mat infMatrix = covariance.inv();
+					double maxAngVar = uMax3(covariance.at<double>(3,3), covariance.at<double>(4,4), covariance.at<double>(5,5));
+					if(maxAngVar != 1.0 && maxAngVar > 0.1)
+					{
+						UWARN("Very large angular variance (%f) detected! Please fix odometry "
+								"twist covariance, otherwise poor graph optimizations are "
+								"expected and wrong loop closure detections creating a lot "
+								"of errors in the map could be accepted.", maxAngVar);
+					}
+
+					cv::Mat infMatrix;
+					if(_covOffDiagonalIgnored)
+					{
+						infMatrix = cv::Mat::zeros(6,6,CV_64FC1);
+						infMatrix.at<double>(0,0) = 1.0 / covariance.at<double>(0,0);
+						infMatrix.at<double>(1,1) = 1.0 / covariance.at<double>(1,1);
+						infMatrix.at<double>(2,2) = 1.0 / covariance.at<double>(2,2);
+						infMatrix.at<double>(3,3) = 1.0 / covariance.at<double>(3,3);
+						infMatrix.at<double>(4,4) = 1.0 / covariance.at<double>(4,4);
+						infMatrix.at<double>(5,5) = 1.0 / covariance.at<double>(5,5);
+					}
+					else
+					{
+						infMatrix = covariance.inv();
+					}
 					if((uIsFinite(covariance.at<double>(0,0)) && covariance.at<double>(0,0)>0.0) &&
 						!(uIsFinite(infMatrix.at<double>(0,0)) && infMatrix.at<double>(0,0)>0.0))
 					{
@@ -836,16 +849,19 @@ void Memory::addSignatureToStm(Signature * signature, const cv::Mat & covariance
 						*_stMem.rbegin(), signature->id(),
 						_signatures.at(*_stMem.rbegin())->mapId(), signature->mapId());
 
-				//Tag the first node of the map
-				std::string tag = uFormat("map%d", signature->mapId());
-				if(getSignatureIdByLabel(tag, false) == 0)
+				if(_mapLabelsAdded && isIncremental())
 				{
-					UINFO("Tagging node %d with label \"%s\"", signature->id(), tag.c_str());
-					signature->setLabel(tag);
+					//Tag the first node of the map
+					std::string tag = uFormat("map%d", signature->mapId());
+					if(getSignatureIdByLabel(tag, false) == 0)
+					{
+						UINFO("Tagging node %d with label \"%s\"", signature->id(), tag.c_str());
+						signature->setLabel(tag);
+					}
 				}
 			}
 		}
-		else if(_mapLabelsAdded)
+		else if(_mapLabelsAdded && isIncremental())
 		{
 			//Tag the first node of the map
 			std::string tag = uFormat("map%d", signature->mapId());
@@ -1501,6 +1517,8 @@ void Memory::clear()
 	_memoryChanged = false;
 	_linksChanged = false;
 	_gpsOrigin = GPS();
+	_rectCameraModels.clear();
+	_rectStereoCameraModel = StereoCameraModel();
 
 	if(_dbDriver)
 	{
@@ -2125,7 +2143,7 @@ void Memory::moveToTrash(Signature * s, bool keepLinkedToGraph, std::list<int> *
 			_lastGlobalLoopClosureId = 0;
 		}
 
-		if(	(_notLinkedNodesKeptInDb || keepLinkedToGraph) &&
+		if(	(_notLinkedNodesKeptInDb || keepLinkedToGraph || s->isSaved()) &&
 			_dbDriver &&
 			s->id()>0 &&
 			(_incrementalMemory || s->isSaved()))
@@ -2398,6 +2416,7 @@ Transform Memory::computeTransform(
 		RegistrationInfo * info,
 		bool useKnownCorrespondencesIfPossible) const
 {
+	UDEBUG("");
 	Transform transform;
 
 	// make sure we have all data needed
@@ -2489,6 +2508,7 @@ Transform Memory::computeTransform(
 			const std::map<int, Link> & links = fromS.getLinks();
 			{
 				const std::map<int, cv::Point3f> & words3 = uMultimapToMapUnique(fromS.getWords3());
+				UDEBUG("fromS.getWords3()=%d  uniques=%d", (int)fromS.getWords3().size(), (int)words3.size());
 				for(std::map<int, cv::Point3f>::const_iterator jter=words3.begin(); jter!=words3.end(); ++jter)
 				{
 					if(util3d::isFinite(jter->second))
@@ -2499,6 +2519,7 @@ Transform Memory::computeTransform(
 					}
 				}
 			}
+			UDEBUG("words3DMap=%d", (int)words3DMap.size());
 
 			for(std::map<int, Link>::const_iterator iter=links.begin(); iter!=links.end(); ++iter)
 			{
@@ -2507,7 +2528,9 @@ Transform Memory::computeTransform(
 				const std::map<int, cv::Point3f> & words3 = uMultimapToMapUnique(s->getWords3());
 				for(std::map<int, cv::Point3f>::const_iterator jter=words3.begin(); jter!=words3.end(); ++jter)
 				{
-					if(util3d::isFinite(jter->second) && words3DMap.find(jter->first) == words3DMap.end())
+					if( jter->first > 0 &&
+						util3d::isFinite(jter->second) &&
+					    words3DMap.find(jter->first) == words3DMap.end())
 					{
 						words3DMap.insert(std::make_pair(jter->first, util3d::transformPoint(jter->second, iter->second.transform())));
 						wordsMap.insert(*s->getWords().find(jter->first));
@@ -2515,6 +2538,7 @@ Transform Memory::computeTransform(
 					}
 				}
 			}
+			UDEBUG("words3DMap=%d", (int)words3DMap.size());
 			Signature tmpFrom2(fromS.id());
 			tmpFrom2.setWords3(words3DMap);
 			tmpFrom2.setWords(wordsMap);
@@ -2537,7 +2561,15 @@ Transform Memory::computeTransform(
 				for(std::map<int, Link>::iterator iter=links.begin(); iter!=links.end(); ++iter)
 				{
 					int id = iter->first;
-					const Signature * s = this->getSignature(id);
+					const Signature * s;
+					if(id == tmpTo.id())
+					{
+						s = &tmpTo; // reuse matched words
+					}
+					else
+					{
+						s = this->getSignature(id);
+					}
 					CameraModel model;
 					if(s->sensorData().cameraModels().size() == 1 && s->sensorData().cameraModels().at(0).isValidForProjection())
 					{
@@ -2572,7 +2604,8 @@ Transform Memory::computeTransform(
 					const std::map<int,cv::KeyPoint> & words = uMultimapToMapUnique(s->getWords());
 					for(std::map<int, cv::KeyPoint>::const_iterator jter=words.begin(); jter!=words.end(); ++jter)
 					{
-						if(points3DMap.find(jter->first)!=points3DMap.end())
+						if(points3DMap.find(jter->first)!=points3DMap.end() &&
+							(id == tmpTo.id() || jter->first > 0))
 						{
 							std::multimap<int, cv::Point3f>::const_iterator kter = s->getWords3().find(jter->first);
 							cv::Point3f pt3d = util3d::transformPoint(kter->second, invLocalTransform);
@@ -3665,25 +3698,41 @@ Signature * Memory::createSignature(const SensorData & inputData, const Transfor
 		}
 	}
 
-	if(!_imagesAlreadyRectified && !data.imageRaw().empty())
+	bool imagesRectified = _imagesAlreadyRectified;
+	// Stereo must be always rectified because of the stereo correspondence approach
+	if(!imagesRectified && !data.imageRaw().empty() && !(_rectifyOnlyFeatures && data.rightRaw().empty()))
 	{
-		if(!data.depthRaw().empty())
-		{
-			UERROR("RGB-D images should be already rectified! Make sure they are and set %s parameter back to true.",
-					Parameters::kRtabmapImagesAlreadyRectified().c_str());
-			return 0;
-		}
+		// we assume that once rtabmap is receiving data, the calibration won't change over time
 		if(data.cameraModels().size())
 		{
+			// Note that only RGB image is rectified, the depth image is assumed to be already registered to rectified RGB camera.
 			UASSERT(int((data.imageRaw().cols/data.cameraModels().size())*data.cameraModels().size()) == data.imageRaw().cols);
 			int subImageWidth = data.imageRaw().cols/data.cameraModels().size();
 			cv::Mat rectifiedImages(data.imageRaw().size(), data.imageRaw().type());
+			bool initRectMaps = _rectCameraModels.empty();
+			if(initRectMaps)
+			{
+				_rectCameraModels.resize(data.cameraModels().size());
+			}
 			for(unsigned int i=0; i<data.cameraModels().size(); ++i)
 			{
 				if(data.cameraModels()[i].isValidForRectification())
 				{
-					cv::Mat rectifiedImage = data.cameraModels()[i].rectifyImage(cv::Mat(data.imageRaw(), cv::Rect(subImageWidth*i, 0, subImageWidth, data.imageRaw().rows)));
+					if(initRectMaps)
+					{
+						_rectCameraModels[i] = data.cameraModels()[i];
+						if(!_rectCameraModels[i].isRectificationMapInitialized())
+						{
+							UWARN("Initializing rectification maps for camera %d (only done for the first image received)...", i);
+							_rectCameraModels[i].initRectificationMap();
+							UWARN("Initializing rectification maps for camera %d (only done for the first image received)... done!", i);
+						}
+					}
+					UASSERT(_rectCameraModels[i].imageWidth() == data.cameraModels()[i].imageWidth() &&
+							_rectCameraModels[i].imageHeight() == data.cameraModels()[i].imageHeight());
+					cv::Mat rectifiedImage = _rectCameraModels[i].rectifyImage(cv::Mat(data.imageRaw(), cv::Rect(subImageWidth*i, 0, subImageWidth, data.imageRaw().rows)));
 					rectifiedImage.copyTo(cv::Mat(rectifiedImages, cv::Rect(subImageWidth*i, 0, subImageWidth, data.imageRaw().rows)));
+					imagesRectified = true;
 				}
 				else
 				{
@@ -3698,8 +3747,21 @@ Signature * Memory::createSignature(const SensorData & inputData, const Transfor
 		}
 		else if(data.stereoCameraModel().isValidForRectification())
 		{
-			data.setImageRaw(data.stereoCameraModel().left().rectifyImage(data.imageRaw()));
-			data.setDepthOrRightRaw(data.stereoCameraModel().right().rectifyImage(data.rightRaw()));
+			if(!_rectStereoCameraModel.isValidForRectification())
+			{
+				_rectStereoCameraModel = data.stereoCameraModel();
+				if(!_rectStereoCameraModel.isRectificationMapInitialized())
+				{
+					UWARN("Initializing rectification maps (only done for the first image received)...");
+					_rectStereoCameraModel.initRectificationMap();
+					UWARN("Initializing rectification maps (only done for the first image received)...done!");
+				}
+			}
+			UASSERT(_rectStereoCameraModel.left().imageWidth() == data.stereoCameraModel().left().imageWidth());
+			UASSERT(_rectStereoCameraModel.left().imageHeight() == data.stereoCameraModel().left().imageHeight());
+			data.setImageRaw(_rectStereoCameraModel.left().rectifyImage(data.imageRaw()));
+			data.setDepthOrRightRaw(_rectStereoCameraModel.right().rectifyImage(data.rightRaw()));
+			imagesRectified = true;
 		}
 		else
 		{
@@ -3728,11 +3790,12 @@ Signature * Memory::createSignature(const SensorData & inputData, const Transfor
 
 	int preDecimation = 1;
 	std::vector<cv::Point3f> keypoints3D;
+	SensorData decimatedData;
 	if(!_useOdometryFeatures || data.keypoints().empty() || (int)data.keypoints().size() != data.descriptors().rows)
 	{
 		if(_feature2D->getMaxFeatures() >= 0 && !data.imageRaw().empty() && !isIntermediateNode)
 		{
-			SensorData decimatedData = data;
+			decimatedData = data;
 			if(_imagePreDecimation > 1)
 			{
 				preDecimation = _imagePreDecimation;
@@ -3768,7 +3831,7 @@ Signature * Memory::createSignature(const SensorData & inputData, const Transfor
 			}
 
 			cv::Mat depthMask;
-			if(!decimatedData.depthRaw().empty() && _depthAsMask)
+			if(imagesRectified && !decimatedData.depthRaw().empty() && _depthAsMask)
 			{
 				if(imageMono.rows % decimatedData.depthRaw().rows == 0 &&
 					imageMono.cols % decimatedData.depthRaw().cols == 0 &&
@@ -3812,13 +3875,134 @@ Signature * Memory::createSignature(const SensorData & inputData, const Transfor
 			{
 				descriptors = cv::Mat();
 			}
-			else if((!decimatedData.depthRaw().empty() && decimatedData.cameraModels().size() && decimatedData.cameraModels()[0].isValidForProjection()) ||
-					(!decimatedData.rightRaw().empty() && decimatedData.stereoCameraModel().isValidForProjection()))
+			else
 			{
-				keypoints3D = _feature2D->generateKeypoints3D(decimatedData, keypoints);
-				t = timer.ticks();
-				if(stats) stats->addStatistic(Statistics::kTimingMemKeypoints_3D(), t*1000.0f);
-				UDEBUG("time keypoints 3D (%d) = %fs", (int)keypoints3D.size(), t);
+				if(!imagesRectified && decimatedData.cameraModels().size())
+				{
+					std::vector<cv::KeyPoint> keypointsValid;
+					keypointsValid.reserve(keypoints.size());
+					cv::Mat descriptorsValid;
+					descriptorsValid.reserve(descriptors.rows);
+
+					//undistort keypoints before projection (RGB-D)
+					if(decimatedData.cameraModels().size() == 1)
+					{
+						std::vector<cv::Point2f> pointsIn, pointsOut;
+						cv::KeyPoint::convert(keypoints,pointsIn);
+						if(decimatedData.cameraModels()[0].D_raw().cols == 6)
+						{
+#if CV_MAJOR_VERSION > 2 or (CV_MAJOR_VERSION == 2 and (CV_MINOR_VERSION >4 or (CV_MINOR_VERSION == 4 and CV_SUBMINOR_VERSION >=10)))
+							// Equidistant / FishEye
+							// get only k parameters (k1,k2,p1,p2,k3,k4)
+							cv::Mat D(1, 4, CV_64FC1);
+							D.at<double>(0,0) = decimatedData.cameraModels()[0].D_raw().at<double>(0,1);
+							D.at<double>(0,1) = decimatedData.cameraModels()[0].D_raw().at<double>(0,1);
+							D.at<double>(0,2) = decimatedData.cameraModels()[0].D_raw().at<double>(0,4);
+							D.at<double>(0,3) = decimatedData.cameraModels()[0].D_raw().at<double>(0,5);
+							cv::fisheye::undistortPoints(pointsIn, pointsOut,
+									decimatedData.cameraModels()[0].K_raw(),
+									D,
+									decimatedData.cameraModels()[0].R(),
+									decimatedData.cameraModels()[0].P());
+						}
+						else
+#else
+							UWARN("Too old opencv version (%d,%d,%d) to support fisheye model (min 2.4.10 required)!",
+									CV_MAJOR_VERSION, CV_MINOR_VERSION, CV_SUBMINOR_VERSION);
+						}
+#endif
+						{
+							//RadialTangential
+							cv::undistortPoints(pointsIn, pointsOut,
+									decimatedData.cameraModels()[0].K_raw(),
+									decimatedData.cameraModels()[0].D_raw(),
+									decimatedData.cameraModels()[0].R(),
+									decimatedData.cameraModels()[0].P());
+						}
+						UASSERT(pointsOut.size() == keypoints.size());
+						for(unsigned int i=0; i<pointsOut.size(); ++i)
+						{
+							if(pointsOut.at(i).x>=0 && pointsOut.at(i).x<decimatedData.cameraModels()[0].imageWidth() &&
+							   pointsOut.at(i).y>=0 && pointsOut.at(i).y<decimatedData.cameraModels()[0].imageHeight())
+							{
+								keypointsValid.push_back(keypoints.at(i));
+								keypointsValid.back().pt.x = pointsOut.at(i).x;
+								keypointsValid.back().pt.y = pointsOut.at(i).y;
+								descriptorsValid.push_back(descriptors.row(i));
+							}
+						}
+					}
+					else
+					{
+						UASSERT(int((decimatedData.imageRaw().cols/decimatedData.cameraModels().size())*decimatedData.cameraModels().size()) == decimatedData.imageRaw().cols);
+						float subImageWidth = decimatedData.imageRaw().cols/decimatedData.cameraModels().size();
+						for(unsigned int i=0; i<keypoints.size(); ++i)
+						{
+							int cameraIndex = int(keypoints.at(i).pt.x / subImageWidth);
+							UASSERT_MSG(cameraIndex >= 0 && cameraIndex < (int)decimatedData.cameraModels().size(),
+									uFormat("cameraIndex=%d, models=%d, kpt.x=%f, subImageWidth=%f (Camera model image width=%d)",
+											cameraIndex, (int)decimatedData.cameraModels().size(), keypoints[i].pt.x, subImageWidth, decimatedData.cameraModels()[0].imageWidth()).c_str());
+
+							std::vector<cv::Point2f> pointsIn, pointsOut;
+							pointsIn.push_back(cv::Point2f(keypoints.at(i).pt.x-subImageWidth*cameraIndex, keypoints.at(i).pt.y));
+							if(decimatedData.cameraModels()[cameraIndex].D_raw().cols == 6)
+							{
+#if CV_MAJOR_VERSION > 2 or (CV_MAJOR_VERSION == 2 and (CV_MINOR_VERSION >4 or (CV_MINOR_VERSION == 4 and CV_SUBMINOR_VERSION >=10)))
+								// Equidistant / FishEye
+								// get only k parameters (k1,k2,p1,p2,k3,k4)
+								cv::Mat D(1, 4, CV_64FC1);
+								D.at<double>(0,0) = decimatedData.cameraModels()[cameraIndex].D_raw().at<double>(0,1);
+								D.at<double>(0,1) = decimatedData.cameraModels()[cameraIndex].D_raw().at<double>(0,1);
+								D.at<double>(0,2) = decimatedData.cameraModels()[cameraIndex].D_raw().at<double>(0,4);
+								D.at<double>(0,3) = decimatedData.cameraModels()[cameraIndex].D_raw().at<double>(0,5);
+								cv::fisheye::undistortPoints(pointsIn, pointsOut,
+										decimatedData.cameraModels()[cameraIndex].K_raw(),
+										D,
+										decimatedData.cameraModels()[cameraIndex].R(),
+										decimatedData.cameraModels()[cameraIndex].P());
+							}
+							else
+#else
+								UWARN("Too old opencv version (%d,%d,%d) to support fisheye model (min 2.4.10 required)!",
+										CV_MAJOR_VERSION, CV_MINOR_VERSION, CV_SUBMINOR_VERSION);
+							}
+#endif
+							{
+								//RadialTangential
+								cv::undistortPoints(pointsIn, pointsOut,
+										decimatedData.cameraModels()[cameraIndex].K_raw(),
+										decimatedData.cameraModels()[cameraIndex].D_raw(),
+										decimatedData.cameraModels()[cameraIndex].R(),
+										decimatedData.cameraModels()[cameraIndex].P());
+							}
+
+							if(pointsOut[0].x>=0 && pointsOut[0].x<decimatedData.cameraModels()[cameraIndex].imageWidth() &&
+							   pointsOut[0].y>=0 && pointsOut[0].y<decimatedData.cameraModels()[cameraIndex].imageHeight())
+							{
+								keypointsValid.push_back(keypoints.at(i));
+								keypointsValid.back().pt.x = pointsOut[0].x + subImageWidth*cameraIndex;
+								keypointsValid.back().pt.y = pointsOut[0].y;
+								descriptorsValid.push_back(descriptors.row(i));
+							}
+						}
+					}
+
+					keypoints = keypointsValid;
+					descriptors = descriptorsValid;
+
+					t = timer.ticks();
+					if(stats) stats->addStatistic(Statistics::kTimingMemRectification(), t*1000.0f);
+					UDEBUG("time rectification = %fs", t);
+				}
+
+				if((!decimatedData.depthRaw().empty() && decimatedData.cameraModels().size() && decimatedData.cameraModels()[0].isValidForProjection()) ||
+				   (!decimatedData.rightRaw().empty() && decimatedData.stereoCameraModel().isValidForProjection()))
+				{
+					keypoints3D = _feature2D->generateKeypoints3D(decimatedData, keypoints);
+					t = timer.ticks();
+					if(stats) stats->addStatistic(Statistics::kTimingMemKeypoints_3D(), t*1000.0f);
+					UDEBUG("time keypoints 3D (%d) = %fs", (int)keypoints3D.size(), t);
+				}
 			}
 		}
 		else if(data.imageRaw().empty())
@@ -3870,6 +4054,8 @@ Signature * Memory::createSignature(const SensorData & inputData, const Transfor
 				imageMono = data.imageRaw();
 			}
 
+			UASSERT_MSG(imagesRectified, "Cannot extract descriptors on not rectified image from keypoints which assumed to be undistorted");
+
 			descriptors = _feature2D->generateDescriptors(imageMono, keypoints);
 		}
 		t = timer.ticks();
@@ -3990,6 +4176,14 @@ Signature * Memory::createSignature(const SensorData & inputData, const Transfor
 				allWordIds[quantizedToRawIndices[i]] = *iter;
 				++i;
 			}
+			int negIndex = -1;
+			for(i=0; i<(int)allWordIds.size(); ++i)
+			{
+				if(allWordIds[i] < 0)
+				{
+					allWordIds[i] = negIndex--;
+				}
+			}
 			wordIds = uVectorToList(allWordIds);
 		}
 
@@ -4010,7 +4204,7 @@ Signature * Memory::createSignature(const SensorData & inputData, const Transfor
 		UASSERT(wordIds.size() == keypoints.size());
 		UASSERT(keypoints3D.size() == 0 || keypoints3D.size() == wordIds.size());
 		unsigned int i=0;
-		float decimationRatio = preDecimation / _imagePostDecimation;
+		float decimationRatio = float(preDecimation) / float(_imagePostDecimation);
 		double log2value = log(double(preDecimation))/log(2.0);
 		for(std::list<int>::iterator iter=wordIds.begin(); iter!=wordIds.end() && i < keypoints.size(); ++iter, ++i)
 		{
@@ -4036,9 +4230,47 @@ Signature * Memory::createSignature(const SensorData & inputData, const Transfor
 		}
 	}
 
+	cv::Mat image = data.imageRaw();
+	cv::Mat depthOrRightImage = data.depthOrRightRaw();
+	std::vector<CameraModel> cameraModels = data.cameraModels();
+	StereoCameraModel stereoCameraModel = data.stereoCameraModel();
+
+	// apply decimation?
+	if(_imagePostDecimation > 1 && !isIntermediateNode)
+	{
+		if(_imagePostDecimation == preDecimation && decimatedData.isValid())
+		{
+			image = decimatedData.imageRaw();
+			depthOrRightImage = decimatedData.depthOrRightRaw();
+			cameraModels = decimatedData.cameraModels();
+			stereoCameraModel = decimatedData.stereoCameraModel();
+		}
+		else
+		{
+			if(!data.rightRaw().empty() ||
+				(data.depthRaw().rows == image.rows && data.depthRaw().cols == image.cols))
+			{
+				depthOrRightImage = util2d::decimate(depthOrRightImage, _imagePostDecimation);
+			}
+			image = util2d::decimate(image, _imagePostDecimation);
+			for(unsigned int i=0; i<cameraModels.size(); ++i)
+			{
+				cameraModels[i] = cameraModels[i].scaled(1.0/double(_imagePostDecimation));
+			}
+			if(stereoCameraModel.isValidForProjection())
+			{
+				stereoCameraModel.scale(1.0/double(_imagePostDecimation));
+			}
+		}
+
+		t = timer.ticks();
+		if(stats) stats->addStatistic(Statistics::kTimingMemPost_decimation(), t*1000.0f);
+		UDEBUG("time post-decimation = %fs", t);
+	}
+
 	bool triangulateWordsWithoutDepth = !_depthAsMask;
 	if(!pose.isNull() &&
-		data.cameraModels().size() == 1 &&
+		cameraModels.size() == 1 &&
 		words.size() &&
 		(words3D.size() == 0 || (triangulateWordsWithoutDepth && words.size() == words3D.size())) &&
 		_registrationPipeline->isImageRequired() &&
@@ -4049,36 +4281,40 @@ Signature * Memory::createSignature(const SensorData & inputData, const Transfor
 		Signature * previousS = _signatures.rbegin()->second;
 		if(previousS->getWords().size() > 8 && words.size() > 8 && !previousS->getPose().isNull())
 		{
+			UDEBUG("Previous pose(%d) = %s", previousS->id(), previousS->getPose().prettyPrint().c_str());
+			UDEBUG("Current pose(%d) = %s", id, pose.prettyPrint().c_str());
 			Transform cameraTransform = pose.inverse() * previousS->getPose();
 
-			Signature cpPrevious(-2);
+			Signature cpPrevious(2);
+			// IDs should be unique so that registration doesn't override them
 			std::map<int, cv::KeyPoint> uniqueWords = uMultimapToMapUnique(previousS->getWords());
 			std::map<int, cv::Mat> uniqueWordsDescriptors = uMultimapToMapUnique(previousS->getWordsDescriptors());
 			cpPrevious.sensorData().setCameraModels(previousS->sensorData().cameraModels());
 			cpPrevious.setWords(std::multimap<int, cv::KeyPoint>(uniqueWords.begin(), uniqueWords.end()));
 			cpPrevious.setWordsDescriptors(std::multimap<int, cv::Mat>(uniqueWordsDescriptors.begin(), uniqueWordsDescriptors.end()));
-			Signature cpCurrent(-1);
+			Signature cpCurrent(1);
 			uniqueWords = uMultimapToMapUnique(words);
 			uniqueWordsDescriptors = uMultimapToMapUnique(wordsDescriptors);
-			cpCurrent.sensorData().setCameraModels(data.cameraModels());
+			cpCurrent.sensorData().setCameraModels(cameraModels);
 			cpCurrent.setWords(std::multimap<int, cv::KeyPoint>(uniqueWords.begin(), uniqueWords.end()));
 			cpCurrent.setWordsDescriptors(std::multimap<int, cv::Mat>(uniqueWordsDescriptors.begin(), uniqueWordsDescriptors.end()));
 			// This will force comparing descriptors between both images directly
-			Transform tmpt = _registrationPipeline->computeTransformation(cpCurrent, cpPrevious, cameraTransform);
+			Transform tmpt = _registrationPipeline->computeTransformationMod(cpCurrent, cpPrevious, cameraTransform);
 			UDEBUG("t=%s", tmpt.prettyPrint().c_str());
 
 			// compute 3D words by epipolar geometry with the previous signature
 			std::map<int, cv::Point3f> inliers = util3d::generateWords3DMono(
 					uMultimapToMapUnique(cpCurrent.getWords()),
 					uMultimapToMapUnique(cpPrevious.getWords()),
-					data.cameraModels()[0],
+					cameraModels[0],
 					cameraTransform);
 
 			UDEBUG("inliers=%d", (int)inliers.size());
 
-			// words3D should have the same size than words
+			// words3D should have the same size than words if not empty
 			float bad_point = std::numeric_limits<float>::quiet_NaN ();
-			UASSERT(words.size() == words3D.size());
+			UASSERT(words3D.size() == 0 || words.size() == words3D.size());
+			bool words3DWasEmpty = words3D.empty();
 			int added3DPointsWithoutDepth = 0;
 			for(std::multimap<int, cv::KeyPoint>::const_iterator iter=words.begin(); iter!=words.end(); ++iter)
 			{
@@ -4101,44 +4337,22 @@ Signature * Memory::createSignature(const SensorData & inputData, const Transfor
 					iter3D->second = jter->second;
 					++added3DPointsWithoutDepth;
 				}
+				else if(words3DWasEmpty && jter == inliers.end())
+				{
+					// duplicate
+					words3D.insert(std::make_pair(iter->first, cv::Point3f(bad_point,bad_point,bad_point)));
+				}
 			}
 			UDEBUG("added3DPointsWithoutDepth=%d", added3DPointsWithoutDepth);
+			if(stats) stats->addStatistic(Statistics::kMemoryTriangulated_points(), (float)added3DPointsWithoutDepth);
 
 			t = timer.ticks();
 			UASSERT(words3D.size() == words.size());
-			if(stats) stats->addStatistic(Statistics::kTimingMemKeypoints_3D(), t*1000.0f);
-			UDEBUG("time keypoints 3D (%d) = %fs", (int)words3D.size(), t);
+			if(stats) stats->addStatistic(Statistics::kTimingMemKeypoints_3D_motion(), t*1000.0f);
+			UDEBUG("time keypoints 3D by motion (%d) = %fs", (int)words3D.size(), t);
 		}
 	}
 
-	cv::Mat image = data.imageRaw();
-	cv::Mat depthOrRightImage = data.depthOrRightRaw();
-	std::vector<CameraModel> cameraModels = data.cameraModels();
-	StereoCameraModel stereoCameraModel = data.stereoCameraModel();
-
-	// apply decimation?
-	if(_imagePostDecimation > 1 && !isIntermediateNode)
-	{
-		if(!data.rightRaw().empty() ||
-			(data.depthRaw().rows == image.rows && data.depthRaw().cols == image.cols))
-		{
-			depthOrRightImage = util2d::decimate(depthOrRightImage, _imagePostDecimation);
-		}
-		image = util2d::decimate(image, _imagePostDecimation);
-		for(unsigned int i=0; i<cameraModels.size(); ++i)
-		{
-			cameraModels[i] = cameraModels[i].scaled(1.0/double(_imagePostDecimation));
-		}
-		if(stereoCameraModel.isValidForProjection())
-		{
-			stereoCameraModel.scale(1.0/double(_imagePostDecimation));
-		}
-
-		t = timer.ticks();
-		if(stats) stats->addStatistic(Statistics::kTimingMemPost_decimation(), t*1000.0f);
-		UDEBUG("time post-decimation = %fs", t);
-	}
-
 	// Filter the laser scan?
 	LaserScan laserScan = data.laserScanRaw();
 	if(!isIntermediateNode && laserScan.size())

corelib/src/OccupancyGrid.cpp

@@ -74,6 +74,11 @@ OccupancyGrid::OccupancyGrid(const ParametersMap & parameters) :
 	erode_(Parameters::defaultGridGlobalEroded()),
 	footprintRadius_(Parameters::defaultGridGlobalFootprintRadius()),
 	updateError_(Parameters::defaultGridGlobalUpdateError()),
+	occupancyThr_(Parameters::defaultGridGlobalOccupancyThr()),
+	probHit_(logodds(Parameters::defaultGridGlobalProbHit())),
+	probMiss_(logodds(Parameters::defaultGridGlobalProbMiss())),
+	probClampingMin_(logodds(Parameters::defaultGridGlobalProbClampingMin())),
+	probClampingMax_(logodds(Parameters::defaultGridGlobalProbClampingMax())),
 	xMin_(0.0f),
 	yMin_(0.0f),
 	cloudAssembling_(false),
@@ -131,6 +136,27 @@ void OccupancyGrid::parseParameters(const ParametersMap & parameters)
 	Parameters::parse(parameters, Parameters::kGridGlobalFootprintRadius(), footprintRadius_);
 	Parameters::parse(parameters, Parameters::kGridGlobalUpdateError(), updateError_);
 
+	Parameters::parse(parameters, Parameters::kGridGlobalOccupancyThr(), occupancyThr_);
+	if(Parameters::parse(parameters, Parameters::kGridGlobalProbHit(), probHit_))
+	{
+		probHit_ = logodds(probHit_);
+		UASSERT_MSG(probHit_ >= 0.0f, uFormat("probHit_=%f",probHit_).c_str());
+	}
+	if(Parameters::parse(parameters, Parameters::kGridGlobalProbMiss(), probMiss_))
+	{
+		probMiss_ = logodds(probMiss_);
+		UASSERT_MSG(probMiss_ <= 0.0f, uFormat("probMiss_=%f",probMiss_).c_str());
+	}
+	if(Parameters::parse(parameters, Parameters::kGridGlobalProbClampingMin(), probClampingMin_))
+	{
+		probClampingMin_ = logodds(probClampingMin_);
+	}
+	if(Parameters::parse(parameters, Parameters::kGridGlobalProbClampingMax(), probClampingMax_))
+	{
+		probClampingMax_ = logodds(probClampingMax_);
+	}
+	UASSERT(probClampingMax_ > probClampingMin_);
+
 	UASSERT(minMapSize_ >= 0.0f);
 
 	// convert ROI from string to vector
@@ -205,7 +231,23 @@ void OccupancyGrid::setMap(const cv::Mat & map, float xMin, float yMin, float ce
 		UASSERT(cellSize > 0.0f);
 		UASSERT(map.type() == CV_8SC1);
 		map_ = map.clone();
-		mapInfo_ = cv::Mat::zeros(map.size(), CV_32FC3);
+		mapInfo_ = cv::Mat::zeros(map.size(), CV_32FC4);
+		for(int i=0; i<map_.rows; ++i)
+		{
+			for(int j=0; j<map_.cols; ++j)
+			{
+				const char value = map_.at<char>(i,j);
+				float * info = mapInfo_.ptr<float>(i,j);
+				if(value == 0)
+				{
+					info[3] = probClampingMin_;
+				}
+				else if(value == 100)
+				{
+					info[3] = probClampingMax_;
+				}
+			}
+		}
 		xMin_ = xMin;
 		yMin_ = yMin;
 		cellSize_ = cellSize;
@@ -563,11 +605,71 @@ cv::Mat OccupancyGrid::getMap(float & xMin, float & yMin) const
 {
 	xMin = xMin_;
 	yMin = yMin_;
-	if(erode_ && !map_.empty())
+
+	cv::Mat map = map_;
+
+	UTimer t;
+	if(occupancyThr_ != 0.0f && !map.empty())
 	{
-		return util3d::erodeMap(map_);
+		float occThr = logodds(occupancyThr_);
+		map = cv::Mat(map.size(), map.type());
+		UASSERT(mapInfo_.cols == map.cols && mapInfo_.rows == map.rows);
+		for(int i=0; i<map.rows; ++i)
+		{
+			for(int j=0; j<map.cols; ++j)
+			{
+				const float * info = mapInfo_.ptr<float>(i, j);
+				if(info[3] == 0.0f)
+				{
+					map.at<char>(i, j) = -1; // unknown
+				}
+				else if(info[3] >= occThr)
+				{
+					map.at<char>(i, j) = 100; // unknown
+				}
+				else
+				{
+					map.at<char>(i, j) = 0; // empty
+				}
+			}
+		}
+		UDEBUG("Converting map from probabilities (thr=%f) = %fs", occupancyThr_, t.ticks());
 	}
-	return map_;
+
+	if(erode_ && !map.empty())
+	{
+		map = util3d::erodeMap(map);
+		UDEBUG("Eroding map = %fs", t.ticks());
+	}
+	return map;
+}
+
+cv::Mat OccupancyGrid::getProbMap(float & xMin, float & yMin) const
+{
+	xMin = xMin_;
+	yMin = yMin_;
+
+	cv::Mat map;
+	if(!mapInfo_.empty())
+	{
+		map = cv::Mat(mapInfo_.size(), map_.type());
+		for(int i=0; i<map.rows; ++i)
+		{
+			for(int j=0; j<map.cols; ++j)
+			{
+				const float * info = mapInfo_.ptr<float>(i, j);
+				if(info[3] == 0.0f)
+				{
+					map.at<char>(i, j) = -1; // unknown
+				}
+				else
+				{
+					map.at<char>(i, j) = char(probability(info[3])*100.0f); // empty
+				}
+			}
+		}
+	}
+	return map;
 }
 
 void OccupancyGrid::addToCache(
@@ -786,7 +888,7 @@ void OccupancyGrid::update(const std::map<int, Transform> & posesIn)
 	{
 		if(addedNodes_.find(iter->first) == addedNodes_.end())
 		{
-			UDEBUG("Pose %d not found in current added poses, it be added to map", iter->first);
+			UDEBUG("Pose %d not found in current added poses, it will be added to map", iter->first);
 			poses.push_back(*iter);
 		}
 	}
@@ -996,7 +1098,7 @@ void OccupancyGrid::update(const std::map<int, Transform> & posesIn)
 			{
 				UDEBUG("Map empty!");
 				map = cv::Mat::ones(newMapSize, CV_8S)*-1;
-				mapInfo = cv::Mat::zeros(newMapSize, CV_32FC3);
+				mapInfo = cv::Mat::zeros(newMapSize, CV_32FC4);
 			}
 			else
 			{
@@ -1110,6 +1212,20 @@ void OccupancyGrid::update(const std::map<int, Transform> & posesIn)
 								cter->second.first+=1;
 							}
 							value = 0; // free space
+
+							// update odds
+							if(nodeId != kter->first)
+							{
+								info[3] += probMiss_;
+								if (info[3] < probClampingMin_)
+								{
+									info[3] = probClampingMin_;
+								}
+								if (info[3] > probClampingMax_)
+								{
+									info[3] = probClampingMax_;
+								}
+							}
 						}
 					}
 				}
@@ -1220,6 +1336,20 @@ void OccupancyGrid::update(const std::map<int, Transform> & posesIn)
 								cter->second.second+=1;
 							}
 							value = 100; // obstacles
+
+							// update odds
+							if(nodeId != kter->first)
+							{
+								info[3] += probHit_;
+								if (info[3] < probClampingMin_)
+								{
+									info[3] = probClampingMin_;
+								}
+								if (info[3] > probClampingMax_)
+								{
+									info[3] = probClampingMax_;
+								}
+							}
 						}
 					}
 				}

corelib/src/OctoMap.cpp

@@ -276,11 +276,30 @@ OctoMap::OctoMap(const ParametersMap & parameters) :
 	minValues_[0] = minValues_[1] = minValues_[2] = 0.0;
 	maxValues_[0] = maxValues_[1] = maxValues_[2] = 0.0;
 
-	float occupancyThr = Parameters::defaultGridGlobalOctoMapOccupancyThr();
-	Parameters::parse(parameters, Parameters::kGridGlobalOctoMapOccupancyThr(), occupancyThr);
+	float occupancyThr = Parameters::defaultGridGlobalOccupancyThr();
+	float probHit = Parameters::defaultGridGlobalProbHit();
+	float probMiss = Parameters::defaultGridGlobalProbMiss();
+	float clampingMin = Parameters::defaultGridGlobalProbClampingMin();
+	float clampingMax = Parameters::defaultGridGlobalProbClampingMax();
+	Parameters::parse(parameters, Parameters::kGridGlobalOccupancyThr(), occupancyThr);
+	Parameters::parse(parameters, Parameters::kGridGlobalProbHit(), probHit);
+	Parameters::parse(parameters, Parameters::kGridGlobalProbMiss(), probMiss);
+	Parameters::parse(parameters, Parameters::kGridGlobalProbClampingMin(), clampingMin);
+	Parameters::parse(parameters, Parameters::kGridGlobalProbClampingMax(), clampingMax);
 
 	octree_ = new RtabmapColorOcTree(cellSize);
+	if(occupancyThr <= 0.0f)
+	{
+		UWARN("Cannot set %s to null for OctoMap, using default value %f instead.",
+				Parameters::kGridGlobalOccupancyThr().c_str(),
+				Parameters::defaultGridGlobalOccupancyThr());
+		occupancyThr = Parameters::defaultGridGlobalOccupancyThr();
+	}
 	octree_->setOccupancyThres(occupancyThr);
+	octree_->setProbHit(probHit);
+	octree_->setProbMiss(probMiss);
+	octree_->setClampingThresMin(clampingMin);
+	octree_->setClampingThresMax(clampingMax);
 	Parameters::parse(parameters, Parameters::kGridGlobalFullUpdate(), fullUpdate_);
 	Parameters::parse(parameters, Parameters::kGridGlobalUpdateError(), updateError_);
 	Parameters::parse(parameters, Parameters::kGridRangeMax(), rangeMax_);

corelib/src/OdometryDVO.cpp

@@ -55,18 +55,9 @@ OdometryDVO::OdometryDVO(const ParametersMap & parameters) :
 OdometryDVO::~OdometryDVO()
 {
 #ifdef RTABMAP_DVO
-	if(dvo_)
-	{
-		delete dvo_;
-	}
-	if(reference_)
-	{
-		delete reference_;
-	}
-	if(camera_)
-	{
-		delete camera_;
-	}
+	delete dvo_;
+	delete reference_;
+	delete camera_;
 #endif
 }
 

corelib/src/OdometryF2M.cpp

@@ -147,10 +147,7 @@ OdometryF2M::~OdometryF2M()
 	bundleLinks_.clear();
 	bundleModels_.clear();
 	bundlePoseReferences_.clear();
-	if(sba_)
-	{
-		delete sba_;
-	}
+	delete sba_;
 	delete regPipeline_;
 	UDEBUG("");
 }

corelib/src/OdometryFovis.cpp

@@ -33,7 +33,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #include "rtabmap/utilite/UStl.h"
 
 #ifdef RTABMAP_FOVIS
-#include <fovis.hpp>
+#include <libfovis/fovis.hpp>
 #endif
 
 namespace rtabmap {
@@ -60,26 +60,11 @@ OdometryFovis::OdometryFovis(const ParametersMap & parameters) :
 OdometryFovis::~OdometryFovis()
 {
 #ifdef RTABMAP_FOVIS
-	if(fovis_)
-	{
-		delete fovis_;
-	}
-	if(rect_)
-	{
-		delete rect_;
-	}
-	if(stereoCalib_)
-	{
-		delete stereoCalib_;
-	}
-	if(depthImage_)
-	{
-		delete depthImage_;
-	}
-	if(stereoDepth_)
-	{
-		delete stereoDepth_;
-	}
+	delete fovis_;
+	delete rect_;
+	delete stereoCalib_;
+	delete depthImage_;
+	delete stereoDepth_;
 #endif
 }
 

corelib/src/OdometryMSCKF.cpp

@@ -758,14 +758,8 @@ OdometryMSCKF::~OdometryMSCKF()
 {
 	UDEBUG("");
 #ifdef RTABMAP_MSCKF_VIO
-	if(imageProcessor_)
-	{
-		delete imageProcessor_;
-	}
-	if(msckf_)
-	{
-		delete msckf_;
-	}
+	delete imageProcessor_;
+	delete msckf_;
 #endif
 }
 

corelib/src/OdometryORBSLAM2.cpp

@@ -737,10 +737,7 @@ public:
 	virtual ~ORBSLAM2System()
 	{
 		shutdown();
-		if(mpVocabulary)
-		{
-			delete mpVocabulary;
-		}
+		delete mpVocabulary;
 	}
 
 	void shutdown()

corelib/src/OdometryOkvis.cpp

@@ -131,11 +131,11 @@ OdometryOkvis::OdometryOkvis(const ParametersMap & parameters) :
 #ifdef RTABMAP_OKVIS
 	okvisCallbackHandler_(new OkvisCallbackHandler),
 	okvisEstimator_(0),
+	imagesProcessed_(0),
+	initGravity_(false),
 #endif
 	okvisParameters_(parameters),
-	imagesProcessed_(0),
-	previousPose_(Transform::getIdentity()),
-	initGravity_(false)
+	previousPose_(Transform::getIdentity())
 {
 #ifdef RTABMAP_OKVIS
 	Parameters::parse(parameters, Parameters::kOdomOKVISConfigPath(), configFilename_);
@@ -150,10 +150,7 @@ OdometryOkvis::~OdometryOkvis()
 {
 	UDEBUG("");
 #ifdef RTABMAP_OKVIS
-	if(okvisEstimator_)
-	{
-		delete okvisEstimator_;
-	}
+	delete okvisEstimator_;
 	delete okvisCallbackHandler_;
 #endif
 }

corelib/src/OdometryThread.cpp

@@ -51,10 +51,7 @@ OdometryThread::~OdometryThread()
 {
 	this->unregisterFromEventsManager();
 	this->join(true);
-	if(_odometry)
-	{
-		delete _odometry;
-	}
+	delete _odometry;
 	UDEBUG("");
 }
 

corelib/src/OdometryViso2.cpp

@@ -71,10 +71,7 @@ OdometryViso2::OdometryViso2(const ParametersMap & parameters) :
 OdometryViso2::~OdometryViso2()
 {
 #ifdef RTABMAP_VISO2
-	if(viso2_)
-	{
-		delete viso2_;
-	}
+	delete viso2_;
 #endif
 }
 

corelib/src/OptimizerG2O.cpp

@@ -535,7 +535,7 @@ std::map<int, Transform> OptimizerG2O::optimize(
 		UDEBUG("Initial optimization...");
 		optimizer.initializeOptimization();
 
-		UASSERT_MSG(optimizer.verifyInformationMatrices(),
+		UASSERT_MSG(optimizer.verifyInformationMatrices(true),
 				"This error can be caused by (1) bad covariance matrix "
 				"set in odometry messages "
 				"(see requirements in g2o::OptimizableGraph::verifyInformationMatrices() function) "
@@ -1051,16 +1051,31 @@ std::map<int, Transform> OptimizerG2O::optimizeBA(
 
 		UDEBUG("fill 3D points to g2o...");
 		const int stepVertexId = poses.rbegin()->first+1;
+		int negVertexOffset = stepVertexId;
+		if(wordReferences.size() && wordReferences.rbegin()->first>0)
+		{
+			negVertexOffset += wordReferences.rbegin()->first;
+		}
+		UDEBUG("stepVertexId=%d, negVertexOffset=%d", stepVertexId, negVertexOffset);
 		std::list<g2o::OptimizableGraph::Edge*> edges;
 		for(std::map<int, std::map<int, cv::Point3f> >::const_iterator iter = wordReferences.begin(); iter!=wordReferences.end(); ++iter)
 		{
-			if(points3DMap.find(iter->first) != points3DMap.end())
+			int id = iter->first;
+			if(points3DMap.find(id) != points3DMap.end())
 			{
-				cv::Point3f pt3d = points3DMap.at(iter->first);
+				cv::Point3f pt3d = points3DMap.at(id);
 				g2o::VertexSBAPointXYZ* vpt3d = new g2o::VertexSBAPointXYZ();
 
 				vpt3d->setEstimate(Eigen::Vector3d(pt3d.x, pt3d.y, pt3d.z));
-				vpt3d->setId(stepVertexId + iter->first);
+				if(id<0)
+				{
+					vpt3d->setId(negVertexOffset + id*-1);
+				}
+				else
+				{
+					vpt3d->setId(stepVertexId + id);
+				}
+				UASSERT(vpt3d->id() > 0);
 				vpt3d->setMarginalized(true);
 				optimizer.addVertex(vpt3d);
 
@@ -1218,7 +1233,15 @@ std::map<int, Transform> OptimizerG2O::optimizeBA(
 						UDEBUG("Ignoring edge (%d<->%d) d=%f var=%f kernel=%f chi2=%f", (*iter)->vertex(0)->id()-stepVertexId, (*iter)->vertex(1)->id(), d, 1.0/((g2o::EdgeProjectP2SC*)(*iter))->information()(0,0), (*iter)->robustKernel()->delta(), (*iter)->chi2());
 #endif
 
-						cv::Point3f pt3d = points3DMap.at((*iter)->vertex(0)->id()-stepVertexId);
+						cv::Point3f pt3d;
+						if((*iter)->vertex(0)->id() > negVertexOffset)
+						{
+							pt3d = points3DMap.at(negVertexOffset - (*iter)->vertex(0)->id());
+						}
+						else
+						{
+							pt3d = points3DMap.at((*iter)->vertex(0)->id()-stepVertexId);
+						}
 						((g2o::VertexSBAPointXYZ*)(*iter)->vertex(0))->setEstimate(Eigen::Vector3d(pt3d.x, pt3d.y, pt3d.z));
 
 						if(outliers)
@@ -1294,7 +1317,17 @@ std::map<int, Transform> OptimizerG2O::optimizeBA(
 
 		for(std::map<int, cv::Point3f>::iterator iter = points3DMap.begin(); iter!=points3DMap.end(); ++iter)
 		{
-			const g2o::VertexSBAPointXYZ* v = (const g2o::VertexSBAPointXYZ*)optimizer.vertex(stepVertexId + iter->first);
+			const g2o::VertexSBAPointXYZ* v;
+			int id = iter->first;
+			if(id<0)
+			{
+				v = (const g2o::VertexSBAPointXYZ*)optimizer.vertex(negVertexOffset + id*-1);
+			}
+			else
+			{
+				v = (const g2o::VertexSBAPointXYZ*)optimizer.vertex(stepVertexId + id);
+			}
+
 			if(v)
 			{
 				cv::Point3f p(v->estimate()[0], v->estimate()[1], v->estimate()[2]);

corelib/src/Parameters.cpp

@@ -227,6 +227,9 @@ const std::map<std::string, std::pair<bool, std::string> > & Parameters::getRemo
 	{
 		// removed parameters
 
+		// 0.17.5
+		removedParameters_.insert(std::make_pair("Grid/OctoMapOccupancyThr",     std::make_pair(true,  Parameters::kGridGlobalOccupancyThr())));
+
 		// 0.17.0
 		removedParameters_.insert(std::make_pair("Grid/Scan2dMaxFilledRange",     std::make_pair(false,  Parameters::kGridRangeMax())));
 
@@ -514,7 +517,15 @@ void Parameters::parse(const ParametersMap & parameters, ParametersMap & paramet
 
 const char * Parameters::showUsage()
 {
-	return  "Logger options:\n"
+	return  "RTAB-Map options:\n"
+			"   --help                         Show usage.\n"
+			"   --version                      Show version of rtabmap and its dependencies.\n"
+			"   --params                       Show all parameters with their default value and description\n"
+			"   --\"parameter name\" \"value\"     Overwrite a specific RTAB-Map's parameter :\n"
+			"                                    --SURF/HessianThreshold 150\n"
+			"                                   For parameters in table format, add ',' between values :\n"
+			"                                    --Kp/RoiRatios 0,0,0.1,0\n"
+			"Logger options:\n"
 			"   --nolog              Disable logger\n"
 			"   --logconsole         Set logger console type\n"
 			"   --logfile \"path\"     Set logger file type\n"
@@ -526,12 +537,6 @@ const char * Parameters::showUsage()
 			"   --logtime \"bool\"     Print time when logging\n"
 			"   --logwhere \"bool\"    Print where when logging\n"
 			"   --logthread \"bool\"   Print thread id when logging\n"
-			"RTAB-Map options:\n"
-			"   --params                       Show all parameters with their default value and description\n"
-			"   --\"parameter name\" \"value\"     Overwrite a specific RTAB-Map's parameter :\n"
-			"                                    --SURF/HessianThreshold 150\n"
-			"                                   For parameters in table format, add ',' between values :\n"
-			"                                    --Kp/RoiRatios 0,0,0.1,0\n"
 			;
 }
 
@@ -545,7 +550,188 @@ ParametersMap Parameters::parseArguments(int argc, char * argv[], bool onlyParam
 		bool checkParameters = onlyParameters;
 		if(!checkParameters)
 		{
-			if(strcmp(argv[i], "--nolog") == 0)
+			if(strcmp(argv[i], "--help") == 0)
+			{
+				std::cout << showUsage() << std::endl;
+				exit(0);
+			}
+			else if(strcmp(argv[i], "--version") == 0)
+			{
+				std::string str = "RTAB-Map:";
+
+				int spacing = 30;
+				std::cout << str << std::setw(spacing - str.size()) << RTABMAP_VERSION << std::endl;
+				str = "OpenCV:";
+#ifdef RTABMAP_OPENCV3
+				std::cout << str << std::setw(spacing - str.size()) << "3" << std::endl;
+#else
+				std::cout << str << std::setw(spacing - str.size()) << "2" << std::endl;
+#endif
+				str = "With OpenCV nonfree:";
+#ifdef RTABMAP_NONFREE
+				std::cout << str << std::setw(spacing - str.size()) << "true" << std::endl;
+#else
+				std::cout << str << std::setw(spacing - str.size()) << "false" << std::endl;
+#endif
+				str = "With TORO:";
+#ifdef RTABMAP_TORO
+				std::cout << str << std::setw(spacing - str.size()) << "true" << std::endl;
+#else
+				std::cout << str << std::setw(spacing - str.size()) << "false" << std::endl;
+#endif
+				str = "With g2o:";
+#ifdef RTABMAP_G2O
+				std::cout << str << std::setw(spacing - str.size()) << "true" << std::endl;
+#else
+				std::cout << str << std::setw(spacing - str.size()) << "false" << std::endl;
+#endif
+				str = "With GTSAM:";
+#ifdef RTABMAP_GTSAM
+				std::cout << str << std::setw(spacing - str.size()) << "true" << std::endl;
+#else
+				std::cout << str << std::setw(spacing - str.size()) << "false" << std::endl;
+#endif
+				str = "With Vertigo:";
+#ifdef RTABMAP_VERTIGO
+				std::cout << str << std::setw(spacing - str.size()) << "true" << std::endl;
+#else
+				std::cout << str << std::setw(spacing - str.size()) << "false" << std::endl;
+#endif
+				str = "With CVSBA:";
+#ifdef RTABMAP_CVSBA
+				std::cout << str << std::setw(spacing - str.size()) << "true" << std::endl;
+#else
+				std::cout << str << std::setw(spacing - str.size()) << "false" << std::endl;
+#endif
+				str = "With OpenNI2:";
+#ifdef RTABMAP_OPENNI2
+				std::cout << str << std::setw(spacing - str.size()) << "true" << std::endl;
+#else
+				std::cout << str << std::setw(spacing - str.size()) << "false" << std::endl;
+#endif
+				str = "With Freenect:";
+#ifdef RTABMAP_FREENECT
+				std::cout << str << std::setw(spacing - str.size()) << "true" << std::endl;
+#else
+				std::cout << str << std::setw(spacing - str.size()) << "false" << std::endl;
+#endif
+				str = "With Freenect2:";
+#ifdef RTABMAP_FREENECT2
+				std::cout << str << std::setw(spacing - str.size()) << "true" << std::endl;
+#else
+				std::cout << str << std::setw(spacing - str.size()) << "false" << std::endl;
+#endif
+				str = "With K4W2:";
+#ifdef RTABMAP_K4W2
+				std::cout << str << std::setw(spacing - str.size()) << "true" << std::endl;
+#else
+				std::cout << str << std::setw(spacing - str.size()) << "false" << std::endl;
+#endif
+				str = "With DC1394:";
+#ifdef RTABMAP_DC1394
+				std::cout << str << std::setw(spacing - str.size()) << "true" << std::endl;
+#else
+				std::cout << str << std::setw(spacing - str.size()) << "false" << std::endl;
+#endif
+				str = "With FlyCapture2:";
+#ifdef RTABMAP_FLYCAPTURE2
+				std::cout << str << std::setw(spacing - str.size()) << "true" << std::endl;
+#else
+				std::cout << str << std::setw(spacing - str.size()) << "false" << std::endl;
+#endif
+				str = "With ZED:";
+#ifdef RTABMAP_ZED
+				std::cout << str << std::setw(spacing - str.size()) << "true" << std::endl;
+#else
+				std::cout << str << std::setw(spacing - str.size()) << "false" << std::endl;
+#endif
+				str = "With RealSense:";
+#ifdef RTABMAP_REALSENSE
+				std::cout << str << std::setw(spacing - str.size()) << "true" << std::endl;
+#else
+				std::cout << str << std::setw(spacing - str.size()) << "false" << std::endl;
+#endif
+				str = "With RealSense SLAM:";
+#ifdef RTABMAP_REALSENSE_SLAM
+				std::cout << str << std::setw(spacing - str.size()) << "true" << std::endl;
+#else
+				std::cout << str << std::setw(spacing - str.size()) << "false" << std::endl;
+#endif
+				str = "With RealSense2:";
+#ifdef RTABMAP_REALSENSE2
+				std::cout << str << std::setw(spacing - str.size()) << "true" << std::endl;
+#else
+				std::cout << str << std::setw(spacing - str.size()) << "false" << std::endl;
+#endif
+				str = "With libpointmatcher:";
+#ifdef RTABMAP_POINTMATCHER
+				std::cout << str << std::setw(spacing - str.size()) << "true" << std::endl;
+#else
+				std::cout << str << std::setw(spacing - str.size()) << "false" << std::endl;
+#endif
+				str = "With octomap:";
+#ifdef RTABMAP_OCTOMAP
+				std::cout << str << std::setw(spacing - str.size()) << "true" << std::endl;
+#else
+				std::cout << str << std::setw(spacing - str.size()) << "false" << std::endl;
+#endif
+				str = "With cpu-tsdf:";
+#ifdef RTABMAP_CPUTSDF
+				std::cout << str << std::setw(spacing - str.size()) << "true" << std::endl;
+#else
+				std::cout << str << std::setw(spacing - str.size()) << "false" << std::endl;
+#endif
+				str = "With open chisel:";
+#ifdef RTABMAP_OPENCHISEL
+				std::cout << str << std::setw(spacing - str.size()) << "true" << std::endl;
+#else
+				std::cout << str << std::setw(spacing - str.size()) << "false" << std::endl;
+#endif
+				str = "With LOAM:";
+#ifdef RTABMAP_LOAM
+				std::cout << str << std::setw(spacing - str.size()) << "true" << std::endl;
+#else
+				std::cout << str << std::setw(spacing - str.size()) << "false" << std::endl;
+#endif
+				str = "With FOVIS:";
+#ifdef RTABMAP_FOVIS
+				std::cout << str << std::setw(spacing - str.size()) << "true" << std::endl;
+#else
+				std::cout << str << std::setw(spacing - str.size()) << "false" << std::endl;
+#endif
+				str = "With Viso2:";
+#ifdef RTABMAP_VISO2
+				std::cout << str << std::setw(spacing - str.size()) << "true" << std::endl;
+#else
+				std::cout << str << std::setw(spacing - str.size()) << "false" << std::endl;
+#endif
+				str = "With DVO:";
+#ifdef RTABMAP_DVO
+				std::cout << str << std::setw(spacing - str.size()) << "true" << std::endl;
+#else
+				std::cout << str << std::setw(spacing - str.size()) << "false" << std::endl;
+#endif
+				str = "With ORB_SLAM2:";
+#ifdef RTABMAP_ORB_SLAM2
+				std::cout << str << std::setw(spacing - str.size()) << "true" << std::endl;
+#else
+				std::cout << str << std::setw(spacing - str.size()) << "false" << std::endl;
+#endif
+				str = "With OKVIS:";
+#ifdef RTABMAP_OKVIS
+				std::cout << str << std::setw(spacing - str.size()) << "true" << std::endl;
+#else
+				std::cout << str << std::setw(spacing - str.size()) << "false" << std::endl;
+#endif
+				str = "With MSCKF_VIO:";
+#ifdef RTABMAP_MSCKF_VIO
+				std::cout << str << std::setw(spacing - str.size()) << "true" << std::endl;
+#else
+				std::cout << str << std::setw(spacing - str.size()) << "false" << std::endl;
+#endif
+				exit(0);
+			}
+			else if(strcmp(argv[i], "--nolog") == 0)
 			{
 				ULogger::setType(ULogger::kTypeNoLog);
 			}

corelib/src/Registration.cpp

@@ -73,10 +73,7 @@ Registration::Registration(const ParametersMap & parameters, Registration * chil
 
 Registration::~Registration()
 {
-	if(child_)
-	{
-		delete child_;
-	}
+	delete child_;
 }
 void Registration::parseParameters(const ParametersMap & parameters)
 {

corelib/src/RegistrationIcp.cpp

@@ -389,10 +389,7 @@ RegistrationIcp::RegistrationIcp(const ParametersMap & parameters, Registration
 RegistrationIcp::~RegistrationIcp()
 {
 #ifdef RTABMAP_POINTMATCHER
-	if(_libpointmatcherICP)
-	{
-		delete (PM::ICP*)_libpointmatcherICP;
-	}
+	delete (PM::ICP*)_libpointmatcherICP;
 #endif
 }
 
@@ -1049,17 +1046,29 @@ Transform RegistrationIcp::computeTransformationImpl(
 							t = Transform(v[0], v[1], v[2], roll, pitch, yaw);
 							icpT = guess * t.inverse() * guessInv;
 
-							// we were using normals, so compute correspondences using normals
-							pcl::PointCloud<pcl::PointNormal>::Ptr fromCloudNormalsRegistered = util3d::laserScanToPointCloudNormal(fromScan, icpT * fromScan.localTransform());
-							pcl::PointCloud<pcl::PointNormal>::Ptr toCloudNormals = util3d::laserScanToPointCloudNormal(toScan, guess * toScan.localTransform());
+							if(fromScan.hasNormals() && toScan.hasNormals())
+							{
+								// we were using normals, so compute correspondences using normals
+								pcl::PointCloud<pcl::PointNormal>::Ptr fromCloudNormalsRegistered = util3d::laserScanToPointCloudNormal(fromScan, icpT * fromScan.localTransform());
+								pcl::PointCloud<pcl::PointNormal>::Ptr toCloudNormals = util3d::laserScanToPointCloudNormal(toScan, guess * toScan.localTransform());
 
-							util3d::computeVarianceAndCorrespondences(
-									fromCloudNormalsRegistered,
-									toCloudNormals,
-									_maxCorrespondenceDistance,
-									_maxRotation,
-									variance,
-									correspondences);
+								util3d::computeVarianceAndCorrespondences(
+										fromCloudNormalsRegistered,
+										toCloudNormals,
+										_maxCorrespondenceDistance,
+										_maxRotation,
+										variance,
+										correspondences);
+							}
+							else
+							{
+								util3d::computeVarianceAndCorrespondences(
+										fromCloudRegistered,
+										toCloudFiltered,
+										_maxCorrespondenceDistance,
+										variance,
+										correspondences);
+							}
 						}
 						else
 						{
@@ -1100,6 +1109,7 @@ Transform RegistrationIcp::computeTransformationImpl(
 					// verify if there are enough correspondences (using "To" by default if set, in case if "From" is merged from multiple scans)
 					int maxLaserScans = maxLaserScansTo?maxLaserScansTo:maxLaserScansFrom;
 					UDEBUG("Max scans=%d (from=%d, to=%d)", maxLaserScans, maxLaserScansFrom, maxLaserScansTo);
+
 					if(maxLaserScans)
 					{
 						correspondencesRatio = float(correspondences)/float(maxLaserScans);
@@ -1128,10 +1138,17 @@ Transform RegistrationIcp::computeTransformationImpl(
 							info.icpTranslation,
 							info.icpRotation);
 
-					info.covariance = cv::Mat::eye(6,6,CV_64FC1)*variance;
+					if(correspondences == 0)
+					{
+						UERROR("Transform is found but no correspondences has been found!? Variance is unknown!");
+					}
+					else
+					{
+						info.covariance = cv::Mat::eye(6,6,CV_64FC1)*variance;
+					}
 					info.icpInliersRatio = correspondencesRatio;
 
-					if(correspondencesRatio < _correspondenceRatio)
+					if(correspondencesRatio <= _correspondenceRatio)
 					{
 						msg = uFormat("Cannot compute transform (cor=%d corrRatio=%f/%f maxLaserScans=%d)",
 								correspondences, correspondencesRatio, _correspondenceRatio, maxLaserScans);

corelib/src/Rtabmap.cpp

@@ -114,7 +114,7 @@ Rtabmap::Rtabmap() :
 	_proximityAngle(Parameters::defaultRGBDProximityAngle()*M_PI/180.0f),
 	_databasePath(""),
 	_optimizeFromGraphEnd(Parameters::defaultRGBDOptimizeFromGraphEnd()),
-	_optimizationMaxLinearError(Parameters::defaultRGBDOptimizeMaxError()),
+	_optimizationMaxError(Parameters::defaultRGBDOptimizeMaxError()),
 	_startNewMapOnLoopClosure(Parameters::defaultRtabmapStartNewMapOnLoopClosure()),
 	_startNewMapOnGoodSignature(Parameters::defaultRtabmapStartNewMapOnGoodSignature()),
 	_goalReachedRadius(Parameters::defaultRGBDGoalReachedRadius()),
@@ -455,7 +455,7 @@ void Rtabmap::parseParameters(const ParametersMap & parameters)
 		_proximityAngle *= M_PI/180.0f;
 	}
 	Parameters::parse(parameters, Parameters::kRGBDOptimizeFromGraphEnd(), _optimizeFromGraphEnd);
-	Parameters::parse(parameters, Parameters::kRGBDOptimizeMaxError(), _optimizationMaxLinearError);
+	Parameters::parse(parameters, Parameters::kRGBDOptimizeMaxError(), _optimizationMaxError);
 	Parameters::parse(parameters, Parameters::kRtabmapStartNewMapOnLoopClosure(), _startNewMapOnLoopClosure);
 	Parameters::parse(parameters, Parameters::kRtabmapStartNewMapOnGoodSignature(), _startNewMapOnGoodSignature);
 	Parameters::parse(parameters, Parameters::kRGBDGoalReachedRadius(), _goalReachedRadius);
@@ -1203,6 +1203,7 @@ bool Rtabmap::process(
 						UINFO("Odometry refining rejected: %s", info.rejectedMsg.c_str());
 						if(!info.covariance.empty() && info.covariance.at<double>(0,0) > 0.0 && info.covariance.at<double>(0,0) != 1.0 && info.covariance.at<double>(5,5) > 0.0 && info.covariance.at<double>(5,5) != 1.0)
 						{
+							std::cout << info.covariance << std::endl;
 							_memory->updateLink(Link(oldId, signature->id(), signature->getLinks().begin()->second.type(), guess, (info.covariance*100.0).inv()));
 						}
 					}
@@ -1902,6 +1903,8 @@ bool Rtabmap::process(
 	std::list<std::pair<int, int> > loopClosureLinksAdded;
 	int loopClosureVisualInliers = 0; // for statistics
 	int loopClosureVisualMatches = 0;
+	float loopClosureLinearVariance = 0.0f;
+	float loopClosureAngularVariance = 0.0f;
 	if(_loopClosureHypothesis.first>0)
 	{
 		//Compute transform if metric data are present
@@ -1913,6 +1916,11 @@ bool Rtabmap::process(
 			transform = _memory->computeTransform(_loopClosureHypothesis.first, signature->id(), Transform(), &info);
 			loopClosureVisualInliers = info.inliers;
 			loopClosureVisualMatches = info.matches;
+			if(info.covariance.cols == 6 && info.covariance.rows == 6 && info.covariance.type() == CV_64FC1)
+			{
+				loopClosureLinearVariance = info.covariance.at<double>(0,0);
+				loopClosureAngularVariance = info.covariance.at<double>(3,3);
+			}
 			rejectedHypothesis = transform.isNull();
 			if(rejectedHypothesis)
 			{
@@ -2038,19 +2046,18 @@ bool Rtabmap::process(
 									_memory->addLink(Link(signature->id(), nearestId, Link::kGlobalClosure, transform, info.covariance.inv()));
 									loopClosureLinksAdded.push_back(std::make_pair(signature->id(), nearestId));
 
-									if(loopClosureVisualInliers == 0)
-									{
-										loopClosureVisualInliers = info.inliers;
-									}
-									if(loopClosureVisualMatches == 0)
-									{
-										loopClosureVisualMatches = info.matches;
-									}
-
 									if(_loopClosureHypothesis.first == 0)
 									{
 										++proximityDetectionsAddedVisually;
 										lastProximitySpaceClosureId = nearestId;
+
+										loopClosureVisualInliers = info.inliers;
+										loopClosureVisualMatches = info.matches;
+										if(info.covariance.cols == 6 && info.covariance.rows == 6 && info.covariance.type() == CV_64FC1)
+										{
+											loopClosureLinearVariance = info.covariance.at<double>(0,0);
+											loopClosureAngularVariance = info.covariance.at<double>(3,3);
+										}
 									}
 								}
 								else
@@ -2238,6 +2245,8 @@ bool Rtabmap::process(
 	//============================================================
 	float maxLinearError = 0.0f;
 	float maxLinearErrorRatio = 0.0f;
+	float maxAngularError = 0.0f;
+	float maxAngularErrorRatio = 0.0f;
 	double optimizationError = 0.0;
 	int optimizationIterations = 0;
 	cv::Mat localizationCovariance;
@@ -2334,12 +2343,13 @@ bool Rtabmap::process(
 				rejectedHypothesis = true;
 			}
 			else if(_memory->isIncremental() && // FIXME: not tested in localization mode, so do it only in mapping mode
-			  _optimizationMaxLinearError > 0.0f &&
+			  _optimizationMaxError > 0.0f &&
 			  loopClosureLinksAdded.size() &&
 			  optimizationIterations > 0 &&
 			  constraints.size())
 			{
 				const Link * maxLinearLink = 0;
+				const Link * maxAngularLink = 0;
 				for(std::multimap<int, Link>::iterator iter=constraints.begin(); iter!=constraints.end(); ++iter)
 				{
 					// ignore links with high variance
@@ -2352,27 +2362,43 @@ bool Rtabmap::process(
 								fabs(iter->second.transform().x() - t.x()),
 								fabs(iter->second.transform().y() - t.y()),
 								fabs(iter->second.transform().z() - t.z()));
-						float stddev = sqrt(iter->second.transVariance());
-						float linearErrorRatio = linearError/stddev;
+						float opt_roll,opt__pitch,opt__yaw;
+						float link_roll,link_pitch,link_yaw;
+						t.getEulerAngles(opt_roll, opt__pitch, opt__yaw);
+						iter->second.transform().getEulerAngles(link_roll, link_pitch, link_yaw);
+						float angularError = uMax3(
+								fabs(opt_roll - link_roll),
+								fabs(opt__pitch - link_pitch),
+								fabs(opt__yaw - link_yaw));
+						float stddevLinear = sqrt(iter->second.transVariance());
+						float linearErrorRatio = linearError/stddevLinear;
 						if(linearErrorRatio > maxLinearErrorRatio)
 						{
 							maxLinearError = linearError;
 							maxLinearErrorRatio = linearErrorRatio;
 							maxLinearLink = &iter->second;
 						}
+						float stddevAngular = sqrt(iter->second.rotVariance());
+						float angularErrorRatio = angularError/stddevAngular;
+						if(angularErrorRatio > maxAngularErrorRatio)
+						{
+							maxAngularError = angularError;
+							maxAngularErrorRatio = angularErrorRatio;
+							maxAngularLink = &iter->second;
+						}
 					}
 				}
+				bool reject = false;
 				if(maxLinearLink)
 				{
-					UINFO("Max optimization error = %f m (link %d->%d, var=%f, ratio error/std=%f)", maxLinearError, maxLinearLink->from(), maxLinearLink->to(), maxLinearLink->transVariance(), maxLinearError/sqrt(maxLinearLink->transVariance()));
-
-					if(maxLinearErrorRatio > _optimizationMaxLinearError)
+					UINFO("Max optimization linear error = %f m (link %d->%d, var=%f, ratio error/std=%f)", maxLinearError, maxLinearLink->from(), maxLinearLink->to(), maxLinearLink->transVariance(), maxLinearError/sqrt(maxLinearLink->transVariance()));
+					if(maxLinearErrorRatio > _optimizationMaxError)
 					{
 						UWARN("Rejecting all added loop closures (%d) in this "
 							  "iteration because a wrong loop closure has been "
 							  "detected after graph optimization, resulting in "
-							  "a maximum graph error ratio of %f (edge %d->%d, type=%d, abs error=%f, stddev=%f). The "
-							  "maximum error ratio parameter is %f of std deviation.",
+							  "a maximum graph error ratio of %f (edge %d->%d, type=%d, abs error=%f m, stddev=%f). The "
+							  "maximum error ratio parameter \"%s\" is %f of std deviation.",
 							  (int)loopClosureLinksAdded.size(),
 							  maxLinearErrorRatio,
 							  maxLinearLink->from(),
@@ -2380,18 +2406,46 @@ bool Rtabmap::process(
 							  maxLinearLink->type(),
 							  maxLinearError,
 							  sqrt(maxLinearLink->transVariance()),
-							  _optimizationMaxLinearError);
-						for(std::list<std::pair<int, int> >::iterator iter=loopClosureLinksAdded.begin(); iter!=loopClosureLinksAdded.end(); ++iter)
-						{
-							_memory->removeLink(iter->first, iter->second);
-							UWARN("Loop closure %d->%d rejected!", iter->first, iter->second);
-						}
-						updateConstraints = false;
-						_loopClosureHypothesis.first = 0;
-						lastProximitySpaceClosureId = 0;
-						rejectedHypothesis = true;
+							  Parameters::kRGBDOptimizeMaxError().c_str(),
+							  _optimizationMaxError);
+						reject = true;
 					}
 				}
+				if(maxAngularLink)
+				{
+					UINFO("Max optimization angular error = %f deg (link %d->%d, var=%f, ratio error/std=%f)", maxAngularError*180.0f/CV_PI, maxAngularLink->from(), maxAngularLink->to(), maxAngularLink->rotVariance(), maxAngularError/sqrt(maxAngularLink->rotVariance()));
+					if(maxAngularErrorRatio > _optimizationMaxError)
+					{
+						UWARN("Rejecting all added loop closures (%d) in this "
+							  "iteration because a wrong loop closure has been "
+							  "detected after graph optimization, resulting in "
+							  "a maximum graph error ratio of %f (edge %d->%d, type=%d, abs error=%f deg, stddev=%f). The "
+							  "maximum error ratio parameter \"%s\" is %f of std deviation.",
+							  (int)loopClosureLinksAdded.size(),
+							  maxAngularErrorRatio,
+							  maxAngularLink->from(),
+							  maxAngularLink->to(),
+							  maxAngularLink->type(),
+							  maxAngularError*180.0f/CV_PI,
+							  sqrt(maxAngularLink->rotVariance()),
+							  Parameters::kRGBDOptimizeMaxError().c_str(),
+							  _optimizationMaxError);
+						reject = true;
+					}
+				}
+
+				if(reject)
+				{
+					for(std::list<std::pair<int, int> >::iterator iter=loopClosureLinksAdded.begin(); iter!=loopClosureLinksAdded.end(); ++iter)
+					{
+						_memory->removeLink(iter->first, iter->second);
+						UWARN("Loop closure %d->%d rejected!", iter->first, iter->second);
+					}
+					updateConstraints = false;
+					_loopClosureHypothesis.first = 0;
+					lastProximitySpaceClosureId = 0;
+					rejectedHypothesis = true;
+				}
 			}
 
 			if(updateConstraints)
@@ -2468,6 +2522,8 @@ bool Rtabmap::process(
 			statistics_.addStatistic(Statistics::kLoopHypothesis_ratio(), hypothesisRatio);
 			statistics_.addStatistic(Statistics::kLoopVisual_inliers(), loopClosureVisualInliers);
 			statistics_.addStatistic(Statistics::kLoopVisual_matches(), loopClosureVisualMatches);
+			statistics_.addStatistic(Statistics::kLoopLinear_variance(), loopClosureLinearVariance);
+			statistics_.addStatistic(Statistics::kLoopAngular_variance(), loopClosureAngularVariance);
 			statistics_.addStatistic(Statistics::kLoopLast_id(), _memory->getLastGlobalLoopClosureId());
 			statistics_.addStatistic(Statistics::kLoopOptimization_max_error(), maxLinearError);
 			statistics_.addStatistic(Statistics::kLoopOptimization_max_error_ratio(), maxLinearErrorRatio);
@@ -3009,21 +3065,111 @@ void Rtabmap::setWorkingDirectory(std::string path)
 	}
 }
 
-void Rtabmap::rejectLoopClosure(int oldId, int newId)
+void Rtabmap::rejectLastLoopClosure()
 {
-	UDEBUG("_loopClosureHypothesis.first=%d", _loopClosureHypothesis.first);
-	if(_loopClosureHypothesis.first)
+	if(_memory && _memory->getStMem().find(getLastLocationId())!=_memory->getStMem().end())
 	{
-		_loopClosureHypothesis.first = 0;
-		if(_memory)
+		std::map<int, Link> links = _memory->getLinks(getLastLocationId(), false);
+		bool linksRemoved = false;
+		for(std::map<int, Link>::iterator iter = links.begin(); iter!=links.end(); ++iter)
 		{
-			_memory->removeLink(oldId, newId);
+			if(iter->second.type() == Link::kGlobalClosure ||
+				iter->second.type() == Link::kLocalSpaceClosure ||
+				iter->second.type() == Link::kLocalTimeClosure ||
+				iter->second.type() == Link::kUserClosure)
+			{
+				_memory->removeLink(iter->second.from(), iter->second.to());
+				std::multimap<int, Link>::iterator jter = graph::findLink(_constraints, iter->second.from(), iter->second.to(), true);
+				if(jter!=_constraints.end())
+				{
+					_constraints.erase(jter);
+					// second time if link is also inverted
+					jter = graph::findLink(_constraints, iter->second.from(), iter->second.to(), true);
+					if(jter!=_constraints.end())
+					{
+						_constraints.erase(jter);
+					}
+				}
+				linksRemoved = true;
+			}
 		}
-		if(uContains(statistics_.data(), rtabmap::Statistics::kLoopRejectedHypothesis()))
+
+		if(linksRemoved)
 		{
-			statistics_.addStatistic(rtabmap::Statistics::kLoopRejectedHypothesis(), 1.0f);
+			_loopClosureHypothesis.first = 0;
+
+			// we have to re-optimize the graph without the rejected links
+			if(_memory->isIncremental() && _optimizedPoses.size())
+			{
+				UINFO("Update graph");
+				std::map<int, Transform> poses = _optimizedPoses;
+				std::multimap<int, Link> constraints;
+				cv::Mat covariance;
+				optimizeCurrentMap(getLastLocationId(), false, poses, covariance, &constraints);
+
+				if(poses.empty())
+				{
+					UWARN("Graph optimization failed after removing loop closure links from last location!");
+				}
+				else
+				{
+					UINFO("Updated local map (old size=%d, new size=%d)", (int)_optimizedPoses.size(), (int)poses.size());
+					_optimizedPoses = poses;
+					_constraints = constraints;
+					_mapCorrection = _optimizedPoses.at(_memory->getLastWorkingSignature()->id()) * _memory->getLastWorkingSignature()->getPose().inverse();
+				}
+			}
+		}
+	}
+}
+
+void Rtabmap::deleteLastLocation()
+{
+	if(_memory && _memory->getStMem().size())
+	{
+		int lastId = *_memory->getStMem().rbegin();
+		_memory->deleteLocation(lastId);
+		// we have to re-optimize the graph without the deleted location
+		if(_memory->isIncremental() && _optimizedPoses.size())
+		{
+			UINFO("Update graph");
+			_optimizedPoses.erase(lastId);
+			std::map<int, Transform> poses = _optimizedPoses;
+			//remove all constraints with last localization id
+			for(std::multimap<int, Link>::iterator iter=_constraints.begin(); iter!=_constraints.end();)
+			{
+				if(iter->second.from() == lastId || iter->second.to() == lastId)
+				{
+					_constraints.erase(iter++);
+				}
+				else
+				{
+					++iter;
+				}
+			}
+
+			if(poses.empty())
+			{
+				_mapCorrection.setIdentity();
+			}
+			else
+			{
+				std::multimap<int, Link> constraints;
+				cv::Mat covariance;
+				optimizeCurrentMap(_memory->getLastWorkingSignature()->id(), false, poses, covariance, &constraints);
+
+				if(poses.empty())
+				{
+					UWARN("Graph optimization failed after deleting the last location!");
+				}
+				else
+				{
+					_optimizedPoses = poses;
+					_constraints = constraints;
+					_mapCorrection = _optimizedPoses.at(_memory->getLastWorkingSignature()->id()) * _memory->getLastWorkingSignature()->getPose().inverse();
+				}
+			}
 		}
-		statistics_.setLoopClosureId(0);
 	}
 }
 
@@ -3779,7 +3925,7 @@ int Rtabmap::detectMoreLoopClosures(float clusterRadius, float clusterAngle, int
 					if(!t.isNull())
 					{
 						bool updateConstraints = true;
-						if(_optimizationMaxLinearError > 0.0f)
+						if(_optimizationMaxError > 0.0f)
 						{
 							//optimize the graph to see if the new constraint is globally valid
 
@@ -3853,7 +3999,7 @@ int Rtabmap::detectMoreLoopClosures(float clusterRadius, float clusterAngle, int
 									UINFO("Max optimization angular error = %f deg (link %d->%d)", maxAngularError*180.0f/M_PI, maxAngularLink->from(), maxAngularLink->to());
 								}
 
-								if(maxLinearError > _optimizationMaxLinearError)
+								if(maxLinearError > _optimizationMaxError)
 								{
 									msg = uFormat("Rejecting edge %d->%d because "
 											  "graph error is too large after optimization (%f m for edge %d->%d, %f deg for edge %d->%d). "
@@ -3867,7 +4013,7 @@ int Rtabmap::detectMoreLoopClosures(float clusterRadius, float clusterAngle, int
 											  maxAngularLink?maxAngularLink->from():0,
 											  maxAngularLink?maxAngularLink->to():0,
 											  Parameters::kRGBDOptimizeMaxError().c_str(),
-											  _optimizationMaxLinearError);
+											  _optimizationMaxError);
 								}
 							}
 							else

corelib/src/StereoDense.cpp

@@ -29,6 +29,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #include <rtabmap/utilite/ULogger.h>
 #include <opencv2/calib3d/calib3d.hpp>
 #include <opencv2/imgproc/imgproc.hpp>
+#include <opencv2/imgproc/types_c.h>
 
 namespace rtabmap {
 

corelib/src/VWDictionary.cpp

@@ -154,7 +154,7 @@ void VWDictionary::setFixedDictionary(const std::string & dictionaryPath)
 					driver->load(this, false);
 					for(std::map<int, VisualWord*>::iterator iter=_visualWords.begin(); iter!=_visualWords.end(); ++iter)
 					{
-						iter->second->setSaved(false);
+						iter->second->setSaved(true);
 					}
 					_incrementalDictionary = _visualWords.size()==0;
 					driver->closeConnection(false);
@@ -222,6 +222,7 @@ void VWDictionary::setFixedDictionary(const std::string & dictionaryPath)
 								}
 
 								VisualWord * vw = new VisualWord(id, descriptor, 0);
+								vw->setSaved(true);
 								_visualWords.insert(_visualWords.end(), std::pair<int, VisualWord*>(id, vw));
 								_notIndexedWords.insert(_notIndexedWords.end(), id);
 								_unusedWords.insert(_unusedWords.end(), std::pair<int, VisualWord*>(id, vw));
@@ -601,21 +602,18 @@ int VWDictionary::getNextId()
 
 void VWDictionary::addWordRef(int wordId, int signatureId)
 {
-	if(signatureId > 0)
+	VisualWord * vw = 0;
+	vw = uValue(_visualWords, wordId, vw);
+	if(vw)
 	{
-		VisualWord * vw = 0;
-		vw = uValue(_visualWords, wordId, vw);
-		if(vw)
-		{
-			vw->addRef(signatureId);
-			_totalActiveReferences += 1;
+		vw->addRef(signatureId);
+		_totalActiveReferences += 1;
 
-			_unusedWords.erase(vw->id());
-		}
-		else
-		{
-			UERROR("Not found word %d (dict size=%d)", wordId, (int)_visualWords.size());
-		}
+		_unusedWords.erase(vw->id());
+	}
+	else
+	{
+		UERROR("Not found word %d (dict size=%d)", wordId, (int)_visualWords.size());
 	}
 }
 
@@ -636,8 +634,6 @@ void VWDictionary::removeAllWordRef(int wordId, int signatureId)
 std::list<int> VWDictionary::addNewWords(const cv::Mat & descriptorsIn,
 							   int signatureId)
 {
-	UASSERT(signatureId > 0);
-
 	UDEBUG("id=%d descriptors=%d", signatureId, descriptorsIn.rows);
 	UTimer timer;
 	std::list<int> wordIds;
@@ -818,7 +814,7 @@ std::list<int> VWDictionary::addNewWords(const cv::Mat & descriptorsIn,
 					index = *((size_t*)&results.at<int>(i, j));
 				}
 				int id = uValue(_mapIndexId, index);
-				if(d >= 0.0f && id > 0)
+				if(d >= 0.0f && id != 0)
 				{
 					fullResults.insert(std::pair<float, int>(d, id));
 				}
@@ -834,7 +830,7 @@ std::list<int> VWDictionary::addNewWords(const cv::Mat & descriptorsIn,
 			{
 				float d = matches.at(i).at(j).distance;
 				int id = uValue(_mapIndexId, matches.at(i).at(j).trainIdx);
-				if(d >= 0.0f && id > 0)
+				if(d >= 0.0f && id != 0)
 				{
 					fullResults.insert(std::pair<float, int>(d, id));
 				}
@@ -857,7 +853,7 @@ std::list<int> VWDictionary::addNewWords(const cv::Mat & descriptorsIn,
 			{
 				float d = matchesNewWords.at(0).at(j).distance;
 				int id = newWordsId[matchesNewWords.at(0).at(j).trainIdx];
-				if(d >= 0.0f && id > 0)
+				if(d >= 0.0f && id != 0)
 				{
 					fullResults.insert(std::pair<float, int>(d, id));
 				}
@@ -1166,7 +1162,7 @@ std::vector<int> VWDictionary::findNN(const cv::Mat & queryIn) const
 				{
 					float d = dists.at<float>(i,j);
 					int index;
-					UWARN("results elemSize1=%d", results.elemSize1());
+
 					if (sizeof(size_t) == 8)
 					{
 						index = *((size_t*)&results.at<double>(i, j));
@@ -1176,7 +1172,7 @@ std::vector<int> VWDictionary::findNN(const cv::Mat & queryIn) const
 						index = *((size_t*)&results.at<int>(i, j));
 					}
 					int id = uValue(_mapIndexId, index);
-					if(d >= 0.0f && id > 0)
+					if(d >= 0.0f && id != 0)
 					{
 						fullResults.insert(std::pair<float, int>(d, id));
 					}
@@ -1188,7 +1184,7 @@ std::vector<int> VWDictionary::findNN(const cv::Mat & queryIn) const
 				{
 					float d = matches.at(i).at(j).distance;
 					int id = uValue(_mapIndexId, matches.at(i).at(j).trainIdx);
-					if(d >= 0.0f && id > 0)
+					if(d >= 0.0f && id != 0)
 					{
 						fullResults.insert(std::pair<float, int>(d, id));
 					}
@@ -1202,7 +1198,7 @@ std::vector<int> VWDictionary::findNN(const cv::Mat & queryIn) const
 				{
 					float d = matchesNotIndexed.at(i).at(j).distance;
 					int id = uValue(mapIndexIdNotIndexed, matchesNotIndexed.at(i).at(j).trainIdx);
-					if(d >= 0.0f && id > 0)
+					if(d >= 0.0f && id != 0)
 					{
 						fullResults.insert(std::pair<float, int>(d, id));
 					}

corelib/src/clams/discrete_depth_distortion_model_helpers.cpp

@@ -198,7 +198,7 @@ namespace clams
 
 	cv::Mat roiA(targetImage, cv::Rect( 0, 0, overview.cols/2, overview.rows ));
 	cv::Mat(overview, cv::Rect( 0, 0, overview.cols/2, overview.rows )).copyTo(roiA);
-	cv::Mat roiB( targetImage, cvRect( overview.cols/2, 0, pub.cols, pub.rows ) );
+	cv::Mat roiB( targetImage, cv::Rect( overview.cols/2, 0, pub.cols, pub.rows ) );
 	pub.copyTo(roiB);
 
     return targetImage;

corelib/src/opencv/solvepnp.h

@@ -46,6 +46,9 @@
 
 #include <opencv2/core/core.hpp>
 #include <opencv2/calib3d/calib3d.hpp>
+#if CV_MAJOR_VERSION >= 3
+#include <opencv2/calib3d/calib3d_c.h>
+#endif
 
 namespace cv3 {
 

corelib/src/util2d.cpp

@@ -38,6 +38,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #include <opencv2/imgproc/imgproc.hpp>
 #include <opencv2/video/tracking.hpp>
 #include <opencv2/highgui/highgui.hpp>
+#include <opencv2/imgproc/types_c.h>
 #include <map>
 #include <Eigen/Core>
 

corelib/src/util3d.cpp

@@ -39,6 +39,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #include <pcl/io/ply_io.h>
 #include <pcl/common/transforms.h>
 #include <opencv2/imgproc/imgproc.hpp>
+#include <opencv2/imgproc/types_c.h>
 
 namespace rtabmap
 {

corelib/src/util3d_mapping.cpp

@@ -574,6 +574,8 @@ cv::Mat create2DMap(const std::map<int, Transform> & poses,
 		float scanMaxRange)
 {
 	UDEBUG("poses=%d, scans = %d scanMaxRange=%f", poses.size(), scans.size(), scanMaxRange);
+
+	// local scans contain end points of each ray in map frame (pose+localTransform)
 	std::map<int, std::pair<cv::Mat, cv::Mat> > localScans;
 
 	pcl::PointCloud<pcl::PointXYZ> minMax;
@@ -629,12 +631,14 @@ cv::Mat create2DMap(const std::map<int, Transform> & poses,
 		float xMax = (unknownSpaceFilled && scanMaxRange > 0 && scanMaxRange > max.x?scanMaxRange:max.x) + margin;
 		float yMax = (unknownSpaceFilled && scanMaxRange > 0 && scanMaxRange > max.y?scanMaxRange:max.y) + margin;
 
-		//UWARN("map min=(%fm, %fm) max=(%fm,%fm) (margin=%fm, cellSize=%fm, scan range=%f, min=[%fm,%fm] max=[%fm,%fm])",
-		//		xMin, yMin, xMax, yMax, margin, cellSize, scanMaxRange, min.x, min.y, max.x, max.y);
+		UDEBUG("map min=(%fm, %fm) max=(%fm,%fm) (margin=%fm, cellSize=%fm, scan range=%f, min=[%fm,%fm] max=[%fm,%fm])",
+				xMin, yMin, xMax, yMax, margin, cellSize, scanMaxRange, min.x, min.y, max.x, max.y);
 
 		UTimer timer;
 
 		map = cv::Mat::ones((yMax - yMin) / cellSize, (xMax - xMin) / cellSize, CV_8S)*-1;
+		UDEBUG("map size = %dx%d", map.cols, map.rows);
+
 		int j=0;
 		float scanMaxRangeSqr = scanMaxRange * scanMaxRange;
 		for(std::map<int, std::pair<cv::Mat, cv::Mat> >::iterator iter = localScans.begin(); iter!=localScans.end(); ++iter)
@@ -647,16 +651,15 @@ cv::Mat create2DMap(const std::map<int, Transform> & poses,
 				viewpoint = kter->second;
 			}
 			cv::Point2i start(((pose.x()+viewpoint.x)-xMin)/cellSize, ((pose.y()+viewpoint.y)-yMin)/cellSize);
-			cv::Point2f startf(pose.x()+viewpoint.x, pose.y()+viewpoint.y);
 
 			// Set obstacles first
 			for(int i=0; i<iter->second.first.cols; ++i)
 			{
 				const float * ptr = iter->second.first.ptr<float>(0, i);
-				bool ignore = scanMaxRange>cellSize && uNormSquared(ptr[0]+cellSize, ptr[1]+cellSize) > scanMaxRangeSqr;
+				bool ignore = scanMaxRange>cellSize && uNormSquared(ptr[0]-(pose.x()+viewpoint.x)+cellSize, ptr[1]-(pose.y()+viewpoint.y)+cellSize) > scanMaxRangeSqr;
 				if(!ignore)
 				{
-					cv::Point2i end((ptr[0]+startf.x-xMin)/cellSize, (ptr[1]+startf.y-yMin)/cellSize);
+					cv::Point2i end((ptr[0]-xMin)/cellSize, (ptr[1]-yMin)/cellSize);
 					if(end!=start)
 					{
 						map.at<char>(end.y, end.x) = 100; // obstacle
@@ -669,17 +672,20 @@ cv::Mat create2DMap(const std::map<int, Transform> & poses,
 			{
 				const float * ptr = iter->second.first.ptr<float>(0, i);
 
-				cv::Vec2f v(ptr[0], ptr[1]);
+				cv::Vec2f pt(ptr[0], ptr[1]);
 				if(scanMaxRange>cellSize)
 				{
+					cv::Vec2f v(pt[0]-(pose.x()+viewpoint.x), pt[1]-(pose.y()+viewpoint.y));
 					float n = cv::norm(v);
 					if(n > scanMaxRange+cellSize)
 					{
 						v = (v/n) * scanMaxRange;
+						pt[0] = pose.x()+viewpoint.x + v[0];
+						pt[1] = pose.y()+viewpoint.y + v[1];
 					}
 				}
 
-				cv::Point2i end((v[0]+startf.x-xMin)/cellSize, (v[1]+startf.y-yMin)/cellSize);
+				cv::Point2i end((pt[0]-xMin)/cellSize, (pt[1]-yMin)/cellSize);
 				if(end!=start)
 				{
 					if(localScans.size() > 1 || map.at<char>(end.y, end.x) != 0)
@@ -693,17 +699,20 @@ cv::Mat create2DMap(const std::map<int, Transform> & poses,
 			{
 				const float * ptr = iter->second.second.ptr<float>(0, i);
 
-				cv::Vec2f v(ptr[0], ptr[1]);
+				cv::Vec2f pt(ptr[0], ptr[1]);
 				if(scanMaxRange>cellSize)
 				{
+					cv::Vec2f v(pt[0]-(pose.x()+viewpoint.x), pt[1]-(pose.y()+viewpoint.y));
 					float n = cv::norm(v);
 					if(n > scanMaxRange+cellSize)
 					{
 						v = (v/n) * scanMaxRange;
+						pt[0] = pose.x()+viewpoint.x + v[0];
+						pt[1] = pose.y()+viewpoint.y + v[1];
 					}
 				}
 
-				cv::Point2i end((v[0]+startf.x-xMin)/cellSize, (v[1]+startf.y-yMin)/cellSize);
+				cv::Point2i end((pt[0]-xMin)/cellSize, (pt[1]-yMin)/cellSize);
 				if(end!=start)
 				{
 					if(localScans.size() > 1 || map.at<char>(end.y, end.x) != 0)
@@ -748,10 +757,10 @@ cv::Mat create2DMap(const std::map<int, Transform> & poses,
 						cv::Mat origin(2,1,CV_32F), endFirst(2,1,CV_32F), endLast(2,1,CV_32F);
 						origin.at<float>(0) = pose.x()+viewpoint.x;
 						origin.at<float>(1) = pose.y()+viewpoint.y;
-						endFirst.at<float>(0) = iter->second.first.ptr<float>(0,0)[0]+origin.at<float>(0);
-						endFirst.at<float>(1) = iter->second.first.ptr<float>(0,0)[1]+origin.at<float>(1);
-						endLast.at<float>(0) = iter->second.first.ptr<float>(0,iter->second.first.cols-1)[0]+origin.at<float>(0);
-						endLast.at<float>(1) = iter->second.first.ptr<float>(0,iter->second.first.cols-1)[1]+origin.at<float>(1);
+						endFirst.at<float>(0) = iter->second.first.ptr<float>(0,0)[0];
+						endFirst.at<float>(1) = iter->second.first.ptr<float>(0,0)[1];
+						endLast.at<float>(0) = iter->second.first.ptr<float>(0,iter->second.first.cols-1)[0];
+						endLast.at<float>(1) = iter->second.first.ptr<float>(0,iter->second.first.cols-1)[1];
 						//UWARN("origin = %f %f", origin.at<float>(0), origin.at<float>(1));
 						//UWARN("endFirst = %f %f", endFirst.at<float>(0), endFirst.at<float>(1));
 						//UWARN("endLast = %f %f", endLast.at<float>(0), endLast.at<float>(1));

corelib/src/util3d_surface.cpp

@@ -37,6 +37,8 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #include "rtabmap/utilite/UConversion.h"
 #include "rtabmap/utilite/UMath.h"
 #include "rtabmap/utilite/UTimer.h"
+#include <opencv2/core/core_c.h>
+#include <opencv2/imgproc/types_c.h>
 #include <pcl/search/kdtree.h>
 #include <pcl/surface/gp3.h>
 #include <pcl/features/normal_3d_omp.h>

examples/CMakeLists.txt

@@ -5,6 +5,7 @@ IF(TARGET rtabmap_gui)
    ADD_SUBDIRECTORY( RGBDMapping )
    ADD_SUBDIRECTORY( WifiMapping )
    ADD_SUBDIRECTORY( NoEventsExample )
+   ADD_SUBDIRECTORY( Export )
 ELSE()
    MESSAGE(STATUS "RTAB-Map GUI lib is not built, the RGBDMapping and WifiMapping examples will not be built...")
 ENDIF()

examples/Export/CMakeLists.txt

@@ -0,0 +1,56 @@
+cmake_minimum_required(VERSION 2.8)
+
+IF(DEFINED PROJECT_NAME)
+   set(internal TRUE)
+ENDIF(DEFINED PROJECT_NAME)
+
+if(internal)
+	# inside rtabmap project (see below for external build)
+	SET(RTABMap_INCLUDE_DIRS 
+	    ${PROJECT_SOURCE_DIR}/utilite/include
+		${PROJECT_SOURCE_DIR}/corelib/include
+	)
+	SET(RTABMap_LIBRARIES 
+	    rtabmap_core
+		rtabmap_utilite
+	)  
+else()
+	# external build
+	PROJECT( MyProject )
+	
+	FIND_PACKAGE(RTABMap REQUIRED)
+	FIND_PACKAGE(OpenCV REQUIRED)
+	FIND_PACKAGE(PCL 1.7 REQUIRED)
+	
+endif()
+
+if(POLICY CMP0020)
+	cmake_policy(SET CMP0020 OLD)
+endif()
+
+SET(INCLUDE_DIRS
+	${RTABMap_INCLUDE_DIRS}
+    ${OpenCV_INCLUDE_DIRS}
+    ${PCL_INCLUDE_DIRS}
+)
+
+SET(LIBRARIES
+	${RTABMap_LIBRARIES}
+	${OpenCV_LIBRARIES}
+	${PCL_LIBRARIES}
+)
+
+INCLUDE_DIRECTORIES(${INCLUDE_DIRS})
+
+ADD_EXECUTABLE(export main.cpp)
+  
+TARGET_LINK_LIBRARIES(export ${LIBRARIES})
+
+if(internal)
+	SET_TARGET_PROPERTIES( export 
+	  PROPERTIES OUTPUT_NAME ${PROJECT_PREFIX}-export)
+endif(internal)
+
+
+
+

examples/Export/main.cpp

@@ -0,0 +1,295 @@
+/*
+Copyright (c) 2010-2016, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
+All rights reserved.
+
+Redistribution and use in source and binary forms, with or without
+modification, are permitted provided that the following conditions are met:
+    * Redistributions of source code must retain the above copyright
+      notice, this list of conditions and the following disclaimer.
+    * Redistributions in binary form must reproduce the above copyright
+      notice, this list of conditions and the following disclaimer in the
+      documentation and/or other materials provided with the distribution.
+    * Neither the name of the Universite de Sherbrooke nor the
+      names of its contributors may be used to endorse or promote products
+      derived from this software without specific prior written permission.
+
+THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
+ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY
+DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+*/
+
+#include <rtabmap/core/DBDriver.h>
+#include <rtabmap/core/Rtabmap.h>
+#include <rtabmap/core/util3d.h>
+#include <rtabmap/core/util3d_filtering.h>
+#include <rtabmap/core/util3d_transforms.h>
+#include <rtabmap/core/util3d_surface.h>
+#include <rtabmap/utilite/UMath.h>
+#include <rtabmap/utilite/UTimer.h>
+#include <rtabmap/utilite/UFile.h>
+#include <pcl/filters/filter.h>
+#include <pcl/io/ply_io.h>
+#include <pcl/io/obj_io.h>
+#include <pcl/common/common.h>
+#include <pcl/surface/poisson.h>
+#include <stdio.h>
+
+using namespace rtabmap;
+
+void showUsage()
+{
+	printf("\nUsage:\n"
+			"rtabmap-exportCloud [options] database.db\n"
+			"Options:\n"
+			"    --mesh          Create a mesh.\n"
+			"    --texture       Create a mesh with texture.\n"
+			"\n");
+	exit(1);
+}
+
+int main(int argc, char * argv[])
+{
+	ULogger::setType(ULogger::kTypeConsole);
+	ULogger::setLevel(ULogger::kError);
+
+	if(argc < 2)
+	{
+		showUsage();
+	}
+
+	bool mesh = false;
+	bool texture = false;
+	for(int i=1; i<argc-1; ++i)
+	{
+		if(std::strcmp(argv[i], "--mesh") == 0)
+		{
+			mesh = true;
+		}
+		else if(std::strcmp(argv[i], "--texture") == 0)
+		{
+			texture = true;
+		}
+	}
+
+	std::string dbPath = argv[argc-1];
+
+	// Get parameters
+	ParametersMap parameters;
+	DBDriver * driver = DBDriver::create();
+	if(driver->openConnection(dbPath))
+	{
+		parameters = driver->getLastParameters();
+		driver->closeConnection(false);
+	}
+	else
+	{
+		UERROR("Cannot open database %s!", dbPath.c_str());
+	}
+	delete driver;
+
+	// Get the global optimized map
+	Rtabmap rtabmap;
+	rtabmap.init(parameters, dbPath);
+
+	std::map<int, Signature> nodes;
+	std::map<int, Transform> optimizedPoses;
+	std::multimap<int, Link> links;
+	rtabmap.get3DMap(nodes, optimizedPoses, links, true, true);
+
+	// Construct the cloud
+	pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr mergedClouds(new pcl::PointCloud<pcl::PointXYZRGBNormal>);
+	std::map<int, rtabmap::Transform> cameraPoses;
+	std::map<int, std::vector<rtabmap::CameraModel> > cameraModels;
+	std::map<int, cv::Mat> cameraDepths;
+	for(std::map<int, Transform>::iterator iter=optimizedPoses.begin(); iter!=optimizedPoses.end(); ++iter)
+	{
+		Signature node = nodes.find(iter->first)->second;
+
+		// uncompress data
+		node.sensorData().uncompressData();
+		std::vector<CameraModel> models = node.sensorData().cameraModels();
+		cv::Mat depth = node.sensorData().depthRaw();
+
+		pcl::IndicesPtr indices(new std::vector<int>);
+		pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud = util3d::cloudRGBFromSensorData(
+				node.sensorData(),
+				4,           // image decimation before creating the clouds
+				4.0f,        // maximum depth of the cloud
+				0.0f,
+				indices.get());
+
+		pcl::PointCloud<pcl::PointXYZRGB>::Ptr transformedCloud(new pcl::PointCloud<pcl::PointXYZRGB>);
+		transformedCloud = rtabmap::util3d::voxelize(cloud, indices, 0.01);
+		transformedCloud = rtabmap::util3d::transformPointCloud(transformedCloud, iter->second);
+
+		Eigen::Vector3f viewpoint( iter->second.x(),  iter->second.y(),  iter->second.z());
+		pcl::PointCloud<pcl::Normal>::Ptr normals = rtabmap::util3d::computeNormals(transformedCloud, 10, 0.0f, viewpoint);
+
+		pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr cloudWithNormals(new pcl::PointCloud<pcl::PointXYZRGBNormal>);
+		pcl::concatenateFields(*transformedCloud, *normals, *cloudWithNormals);
+
+		if(mergedClouds->size() == 0)
+		{
+			*mergedClouds = *cloudWithNormals;
+		}
+		else
+		{
+			*mergedClouds += *cloudWithNormals;
+		}
+
+		cameraPoses.insert(std::make_pair(iter->first, iter->second));
+		if(!models.empty())
+		{
+			cameraModels.insert(std::make_pair(iter->first, models));
+		}
+		if(!depth.empty())
+		{
+			cameraDepths.insert(std::make_pair(iter->first, depth));
+		}
+	}
+	if(mergedClouds->size())
+	{
+		if(!(mesh || texture))
+		{
+			printf("Voxel grid filtering of the assembled cloud (voxel=%f, %d points)\n", 0.01f, (int)mergedClouds->size());
+			mergedClouds = util3d::voxelize(mergedClouds, 0.01f);
+
+			printf("Saving cloud.ply... (%d points)\n", (int)mergedClouds->size());
+			pcl::io::savePLYFile("cloud.ply", *mergedClouds);
+			printf("Saving cloud.ply... done!\n");
+		}
+		else
+		{
+			Eigen::Vector4f min,max;
+			pcl::getMinMax3D(*mergedClouds, min, max);
+			float mapLength = uMax3(max[0]-min[0], max[1]-min[1], max[2]-min[2]);
+			int optimizedDepth = 12;
+			for(int i=6; i<12; ++i)
+			{
+				if(mapLength/float(1<<i) < 0.03f)
+				{
+					optimizedDepth = i;
+					break;
+				}
+			}
+
+			// Mesh reconstruction
+			printf("Mesh reconstruction...\n");
+			pcl::PolygonMesh::Ptr mesh(new pcl::PolygonMesh);
+			pcl::Poisson<pcl::PointXYZRGBNormal> poisson;
+			poisson.setDepth(optimizedDepth);
+			poisson.setInputCloud(mergedClouds);
+			UTimer timer;
+			poisson.reconstruct(*mesh);
+			printf("Mesh reconstruction... done! %fs (%d polygons)\n", timer.ticks(), (int)mesh->polygons.size());
+
+			if(mesh->polygons.size())
+			{
+				rtabmap::util3d::denseMeshPostProcessing<pcl::PointXYZRGBNormal>(
+						mesh,
+						0.0f,
+						0,
+						mergedClouds,
+						0.05,
+						!texture);
+
+				if(!texture)
+				{
+					printf("Saving mesh.ply...\n");
+					pcl::io::savePLYFile("mesh.ply", *mesh);
+					printf("Saving mesh.ply... done!\n");
+				}
+				else
+				{
+					printf("Texturing... cameraPoses=%d, cameraDepths=%d\n", (int)cameraPoses.size(), (int)cameraDepths.size());
+					std::vector<std::map<int, pcl::PointXY> > vertexToPixels;
+					pcl::TextureMeshPtr textureMesh = rtabmap::util3d::createTextureMesh(
+							mesh,
+							cameraPoses,
+							cameraModels,
+							cameraDepths,
+							3,
+							0.0f,
+							0.0f,
+							50,
+							std::vector<float>(),
+							0,
+							&vertexToPixels);
+					printf("Texturing... done! %fs\n", timer.ticks());
+
+					// Remove occluded polygons (polygons with no texture)
+					if(textureMesh->tex_coordinates.size())
+					{
+						printf("Cleanup mesh...\n");
+						rtabmap::util3d::cleanTextureMesh(*textureMesh, 0);
+						printf("Cleanup mesh... done! %fs\n", timer.ticks());
+					}
+
+					if(textureMesh->tex_materials.size())
+					{
+						printf("Merging %d textures...\n", (int)textureMesh->tex_materials.size());
+						cv::Mat textures = rtabmap::util3d::mergeTextures(
+								*textureMesh,
+								std::map<int, cv::Mat>(),
+								std::map<int, std::vector<rtabmap::CameraModel> >(),
+								rtabmap.getMemory(),
+								0,
+								4096,
+								1,
+								vertexToPixels,
+								true, 10.0f, true ,true, 0, 0, 0, false);
+
+
+						// TextureMesh OBJ
+						bool success = false;
+						UASSERT(!textures.empty());
+						UASSERT(textureMesh->tex_materials.size() == 1);
+
+						std::string filePath = "mesh.jpg";
+						textureMesh->tex_materials[0].tex_file = filePath;
+						printf("Saving texture to %s.\n", filePath.c_str());
+						success = cv::imwrite(filePath, textures);
+						if(!success)
+						{
+							UERROR("Failed saving %s!", filePath.c_str());
+						}
+						else
+						{
+							printf("Saved %s.\n", filePath.c_str());
+						}
+
+						if(success)
+						{
+
+							std::string filePath = "mesh.obj";
+							printf("Saving obj (%d vertices) to %s.\n", (int)textureMesh->cloud.data.size()/textureMesh->cloud.point_step, filePath.c_str());
+							success = pcl::io::saveOBJFile(filePath, *textureMesh) == 0;
+
+							if(success)
+							{
+								printf("Saved obj to %s!\n", filePath.c_str());
+							}
+							else
+							{
+								UERROR("Failed saving obj to %s!", filePath.c_str());
+							}
+						}
+					}
+				}
+			}
+		}
+	}
+	else
+	{
+		printf("Export failed! The cloud is empty.\n");
+	}
+
+	return 0;
+}

examples/NoEventsExample/main.cpp

@@ -47,7 +47,7 @@ void showUsage()
 			"\n"
 			"Example:\n"
 			"     (with images from \"https://github.com/introlab/rtabmap/wiki/Stereo-mapping#process-a-directory-of-stereo-images\") \n"
-			"     $ rtabmap-noEventsExample 20 2 10 stereo_20hz stereo_20Hz stereo_20hz/left stereo_20hz/right\n"
+			"     $ rtabmap-noEventsExample 20 2 10 stereo_20Hz stereo_20Hz stereo_20Hz/left stereo_20Hz/right\n"
 			"       Camera rate = 20 Hz\n"
 			"       Odometry update rate = 10 Hz\n"
 			"       Map update rate = 1 Hz\n");

guilib/include/rtabmap/gui/CloudViewer.h

@@ -32,6 +32,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 
 #include "rtabmap/core/Transform.h"
 #include "rtabmap/core/StereoCameraModel.h"
+#include "rtabmap/gui/CloudViewerInteractorStyle.h"
 
 #include <QVTKWidget.h>
 #include <pcl/pcl_base.h>
@@ -48,9 +49,6 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #include <opencv2/opencv.hpp>
 #include <set>
 
-#include <pcl/visualization/mouse_event.h>
-#include <pcl/visualization/point_picking_event.h>
-#include <pcl/visualization/interactor_style.h>
 #include <pcl/PCLPointCloud2.h>
 
 namespace pcl {
@@ -67,32 +65,6 @@ class vtkOBBTree;
 namespace rtabmap {
 
 class OctoMap;
-class CloudViewer;
-
-class RTABMAPGUI_EXP CloudViewerInteractorStyle: public pcl::visualization::PCLVisualizerInteractorStyle
-{
-public:
-    static CloudViewerInteractorStyle *New ();
-
-public:
-	CloudViewerInteractorStyle();
-	virtual void Rotate();
-protected:
-	virtual void OnMouseMove();
-	virtual void OnLeftButtonDown();
-
-protected:
-	friend class CloudViewer;
-	void setCloudViewer(CloudViewer * cloudViewer) {viewer_ = cloudViewer;}
-	CloudViewer * viewer_;
-
-private:
-	unsigned int NumberOfClicks;
-	int PreviousPosition[2];
-	int ResetPixelDistance;
-	float PreviousMeasure[3];
-	pcl::PointCloud<pcl::PointXYZRGB>::Ptr pointsHolder_;
-};
 
 class RTABMAPGUI_EXP CloudViewer : public QVTKWidget
 {
@@ -262,6 +234,26 @@ public:
 	void removeAllCubes();
 	const std::set<std::string> & getAddedCubes() const {return _cubes;}
 
+	void addOrUpdateQuad(
+			const std::string & id,
+			const Transform & pose,
+			float width,
+			float height,
+			const QColor & color,
+			bool foreground = false);
+	void addOrUpdateQuad(
+			const std::string & id,
+			const Transform & pose,
+			float widthLeft,
+			float widthRight,
+			float heightBottom,
+			float heightTop,
+			const QColor & color,
+			bool foreground = false);
+	void removeQuad(const std::string & id);
+	void removeAllQuads();
+	const std::set<std::string> & getAddedQuads() const {return _quads;}
+
 	void addOrUpdateFrustum(
 			const std::string & id,
 			const Transform & transform,
@@ -316,6 +308,9 @@ public:
 	const QColor & getDefaultBackgroundColor() const;
 	const QColor & getBackgroundColor() const;
 	Transform getTargetPose() const;
+	std::string getIdByActor(vtkProp * actor) const;
+	QColor getColor(const std::string & id);
+	void setColor(const std::string & id, const QColor & color);
 
 	void setBackfaceCulling(bool enabled, bool frontfaceCulling);
 	void setPolygonPicking(bool enabled);
@@ -323,6 +318,7 @@ public:
 	void setLighting(bool on);
 	void setShading(bool on);
 	void setEdgeVisibility(bool visible);
+	void setInteractorLayer(int layer);
 	double getRenderingRate() const;
 
 	void getCameraPosition(
@@ -333,6 +329,7 @@ public:
 	bool isCameraTargetFollow() const;
 	bool isCameraFree() const;
 	bool isCameraLockZ() const;
+	bool isCameraOrtho() const;
 	bool isGridShown() const;
 	unsigned int getGridCellCount() const;
 	float getGridCellSize() const;
@@ -345,6 +342,7 @@ public:
 	void setCameraTargetFollow(bool enabled = true);
 	void setCameraFree();
 	void setCameraLockZ(bool enabled = true);
+	void setCameraOrtho(bool enabled = true);
 	void setGridShown(bool shown);
 	void setNormalsShown(bool shown);
 	void setGridCellCount(unsigned int count);
@@ -391,6 +389,7 @@ private:
     QAction * _aFollowCamera;
     QAction * _aResetCamera;
     QAction * _aLockViewZ;
+    QAction * _aCameraOrtho;
     QAction * _aShowTrajectory;
     QAction * _aSetTrajectorySize;
     QAction * _aClearTrajectory;
@@ -417,6 +416,7 @@ private:
     std::set<std::string> _lines;
     std::set<std::string> _spheres;
     std::set<std::string> _cubes;
+    std::set<std::string> _quads;
     QMap<std::string, Transform> _frustums;
     pcl::PointCloud<pcl::PointXYZ>::Ptr _trajectory;
     unsigned int _maxTrajectorySize;

guilib/include/rtabmap/gui/CloudViewerCellPicker.h

@@ -0,0 +1,41 @@
+/*
+ * CloudViewerCellPicker.h
+ *
+ *  Created on: Aug 21, 2018
+ *      Author: mathieu
+ */
+
+#ifndef GUILIB_SRC_CLOUDVIEWERCELLPICKER_H_
+#define GUILIB_SRC_CLOUDVIEWERCELLPICKER_H_
+
+#include "rtabmap/gui/RtabmapGuiExp.h" // DLL export/import defines
+
+#include <vtkCellPicker.h>
+
+namespace rtabmap {
+
+class RTABMAPGUI_EXP CloudViewerCellPicker : public vtkCellPicker {
+public:
+public:
+    static CloudViewerCellPicker *New ();
+    vtkTypeMacro(CloudViewerCellPicker, vtkCellPicker);
+	CloudViewerCellPicker();
+	virtual ~CloudViewerCellPicker();
+
+protected:
+	// overrided to ignore back faces
+    virtual double IntersectActorWithLine(const double p1[3],
+    		const double p2[3],
+			double t1, double t2,
+			double tol,
+			vtkProp3D *prop,
+			vtkMapper *mapper);
+
+private:
+	vtkGenericCell * cell_; //used to accelerate picking
+	vtkIdList * pointIds_; // used to accelerate picking
+};
+
+} /* namespace rtabmap */
+
+#endif /* GUILIB_SRC_CLOUDVIEWERCELLPICKER_H_ */

guilib/include/rtabmap/gui/CloudViewerInteractorStyle.h

@@ -0,0 +1,52 @@
+/*
+ * CloudViewerInteractorStyl.h
+ *
+ *  Created on: Aug 21, 2018
+ *      Author: mathieu
+ */
+
+#ifndef GUILIB_SRC_CLOUDVIEWERINTERACTORSTYLE_H_
+#define GUILIB_SRC_CLOUDVIEWERINTERACTORSTYLE_H_
+
+#include "rtabmap/gui/RtabmapGuiExp.h" // DLL export/import defines
+
+#include <pcl/visualization/mouse_event.h>
+#include <pcl/visualization/point_picking_event.h>
+#include <pcl/visualization/interactor_style.h>
+#include <pcl/point_types.h>
+
+namespace rtabmap {
+
+class CloudViewer;
+
+class RTABMAPGUI_EXP CloudViewerInteractorStyle: public pcl::visualization::PCLVisualizerInteractorStyle
+{
+public:
+    static CloudViewerInteractorStyle *New ();
+    vtkTypeMacro(CloudViewerInteractorStyle, pcl::visualization::PCLVisualizerInteractorStyle);
+
+public:
+	CloudViewerInteractorStyle();
+	virtual void Rotate();
+	void setOrthoMode(bool enabled);
+protected:
+	virtual void OnMouseMove();
+	virtual void OnLeftButtonDown();
+
+protected:
+	friend class CloudViewer;
+	void setCloudViewer(CloudViewer * cloudViewer) {viewer_ = cloudViewer;}
+	CloudViewer * viewer_;
+
+private:
+	unsigned int NumberOfClicks;
+	int PreviousPosition[2];
+	int ResetPixelDistance;
+	float PreviousMeasure[3];
+	pcl::PointCloud<pcl::PointXYZRGB>::Ptr pointsHolder_;
+	bool orthoMode_;
+};
+
+} /* namespace rtabmap */
+
+#endif /* GUILIB_SRC_CLOUDVIEWERINTERACTORSTYLE_H_ */

guilib/include/rtabmap/gui/ImageView.h

@@ -64,6 +64,7 @@ public:
 	int getFeaturesSize() const {return _featuresSize;}
 	bool isGraphicsViewMode() const;
 	bool isGraphicsViewScaled() const;
+	bool isGraphicsViewScaledToHeight() const;
 	const QColor & getDefaultBackgroundColor() const;
 	const QColor & getBackgroundColor() const;
 
@@ -75,6 +76,7 @@ public:
 	void setLinesShown(bool shown);
 	void setGraphicsViewMode(bool on);
 	void setGraphicsViewScaled(bool scaled);
+	void setGraphicsViewScaledToHeight(bool scaled);
 	void setDefaultBackgroundColor(const QColor & color);
 	void setBackgroundColor(const QColor & color);
 
@@ -83,6 +85,7 @@ public:
 	void addFeature(int id, const cv::KeyPoint & kpt, float depth, QColor color);
 	void addLine(float x1, float y1, float x2, float y2, QColor color, const QString & text = QString());
 	void setImage(const QImage & image);
+	void setImageDepth(const cv::Mat & imageDepth);
 	void setImageDepth(const QImage & image);
 	void setFeatureColor(int id, QColor color);
 	void setFeaturesColor(QColor color);
@@ -129,6 +132,13 @@ private:
 	QAction * _setFeaturesSize;
 	QAction * _graphicsViewMode;
 	QAction * _graphicsViewScaled;
+	QAction * _graphicsViewScaledToHeight;
+	QAction * _graphicsViewNoScaling;
+	QAction * _colorMapWhiteToBlack;
+	QAction * _colorMapBlackToWhite;
+	QAction * _colorMapRedToBlue;
+	QAction * _colorMapBlueToRed;
+	QMenu * _scaleMenu;
 
 	QGraphicsView * _graphicsView;
 	QMultiMap<int, rtabmap::KeypointItem *> _features;
@@ -137,6 +147,7 @@ private:
 	QGraphicsPixmapItem * _imageDepthItem;
 	QPixmap _image;
 	QPixmap _imageDepth;
+	cv::Mat _imageDepthCv;
 };
 
 }

guilib/include/rtabmap/gui/MainWindow.h

@@ -112,7 +112,7 @@ public:
 public Q_SLOTS:
 	virtual void processStats(const rtabmap::Statistics & stat);
 	void updateCacheFromDatabase(const QString & path);
-	void openDatabase(const QString & path);
+	void openDatabase(const QString & path, const rtabmap::ParametersMap & overridedParameters = rtabmap::ParametersMap());
 	void updateParameters(const rtabmap::ParametersMap & parameters);
 
 protected:
@@ -327,6 +327,7 @@ private:
 	bool _savedMaximized;
 	QStringList _waypoints;
 	int _waypointsIndex;
+	std::vector<CameraModel> _rectCameraModels;
 
 	QMap<int, Signature> _cachedSignatures;
 	long _cachedMemoryUsage;
@@ -340,6 +341,7 @@ private:
 	std::set<int> _cachedEmptyClouds;
 	std::pair<int, std::pair<std::pair<pcl::PointCloud<pcl::PointXYZRGB>::Ptr, pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr>, pcl::IndicesPtr> > _previousCloud; // used for subtraction
 	std::map<int, float> _cachedWordsCount;
+	std::map<int, float> _cachedLocalizationsCount;
 
 	std::map<int, LaserScan> _createdScans;
 

guilib/include/rtabmap/gui/PreferencesDialog.h

@@ -155,6 +155,7 @@ public:
 	bool isOdomOnlyInliersShown() const;
 	bool isPosteriorGraphView() const;
 	bool isWordsCountGraphView() const;
+	bool isLocalizationsCountGraphView() const;
 	int getOdomRegistrationApproach() const;
 	bool isOdomDisabled() const;
 	bool isGroundTruthAligned() const;

guilib/include/rtabmap/utilite/UCv2Qt.h

@@ -21,11 +21,19 @@
 #define UCV2QT_H_
 
 #include <QtGui/QImage>
+#include <QtGui/QColor>
 #include <opencv2/core/core.hpp>
 #include <rtabmap/utilite/UMath.h>
 #include <rtabmap/utilite/UThread.h>
 #include <stdio.h>
 
+enum uCvQtDepthColorMap{
+	uCvQtDepthWhiteToBlack,
+	uCvQtDepthBlackToWhite,
+	uCvQtDepthRedToBlue,
+	uCvQtDepthBlueToRed
+};
+
 /**
  * Convert a cv::Mat image to a QImage. Support 
  * depth (float32, uint16) image and RGB/BGR 8bits images.
@@ -33,7 +41,7 @@
  * @param isBgr if 3 channels, it is BGR or RGB order.
  * @return the QImage
  */
-inline QImage uCvMat2QImage(const cv::Mat & image, bool isBgr = true)
+inline QImage uCvMat2QImage(const cv::Mat & image, bool isBgr = true, uCvQtDepthColorMap colorMap = uCvQtDepthWhiteToBlack)
 {
 	QImage qtemp;
 	if(!image.empty() && image.depth() == CV_8U)
@@ -66,7 +74,8 @@ inline QImage uCvMat2QImage(const cv::Mat & image, bool isBgr = true)
 			// mono grayscale
 			qtemp = QImage(image.data, image.cols, image.rows, image.cols, QImage::Format_Indexed8).copy();
 			QVector<QRgb> my_table;
-			for(int i = 0; i < 256; i++) my_table.push_back(qRgb(i,i,i));
+			for(int i = 0; i < 256; i++)
+				my_table.push_back(qRgb(i,i,i));
 			qtemp.setColorTable(my_table);
 		}
 		else
@@ -113,11 +122,26 @@ inline QImage uCvMat2QImage(const cv::Mat & image, bool isBgr = true)
 						*p = 0;
 					}
 				}
+				if(*p!=0 && (colorMap == uCvQtDepthBlackToWhite || colorMap == uCvQtDepthRedToBlue))
+				{
+					*p = 255-*p;
+				}
 			}
 		}
 
 		QVector<QRgb> my_table;
-		for(int i = 0; i < 256; i++) my_table.push_back(qRgb(i,i,i));
+		my_table.reserve(256);
+		if(colorMap == uCvQtDepthRedToBlue || colorMap == uCvQtDepthBlueToRed)
+		{
+			my_table.push_back(qRgb(0,0,0));
+			for(int i = 1; i < 256; i++)
+				my_table.push_back(QColor::fromHsv(i, 255, 255, 255).rgb());
+		}
+		else
+		{
+			for(int i = 0; i < 256; i++)
+				my_table.push_back(qRgb(i,i,i));
+		}
 		qtemp.setColorTable(my_table);
     }
 	else if(image.depth() == CV_16U && image.channels()==1)
@@ -160,11 +184,26 @@ inline QImage uCvMat2QImage(const cv::Mat & image, bool isBgr = true)
 						*p = 0;
 					}
 				}
+				if(*p!=0 && (colorMap == uCvQtDepthBlackToWhite || colorMap == uCvQtDepthRedToBlue))
+				{
+					*p = 255-*p;
+				}
 			}
 		}
 
 		QVector<QRgb> my_table;
-		for(int i = 0; i < 256; i++) my_table.push_back(qRgb(i,i,i));
+		my_table.reserve(256);
+		if(colorMap == uCvQtDepthRedToBlue || colorMap == uCvQtDepthBlueToRed)
+		{
+			my_table.push_back(qRgb(0,0,0));
+			for(int i = 1; i < 256; i++)
+				my_table.push_back(QColor::fromHsv(i, 255, 255, 255).rgb());
+		}
+		else
+		{
+			for(int i = 0; i < 256; i++)
+				my_table.push_back(qRgb(i,i,i));
+		}
 		qtemp.setColorTable(my_table);
 	}
 	else if(!image.empty() && image.depth() != CV_8U)

guilib/src/CMakeLists.txt

@@ -84,6 +84,8 @@ SET(SRC_FILES
 	./DatabaseViewer.cpp
 	./utilite/UPlot.cpp
 	./CloudViewer.cpp
+	./CloudViewerCellPicker.cpp
+	./CloudViewerInteractorStyle.cpp
 	./OdometryViewer.cpp
 	./LoopClosureViewer.cpp
 	./DataRecorder.cpp

guilib/src/CalibrationDialog.cpp

@@ -30,7 +30,11 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 
 #include <opencv2/core/core.hpp>
 #include <opencv2/imgproc/imgproc.hpp>
+#include <opencv2/imgproc/imgproc_c.h>
 #include <opencv2/calib3d/calib3d.hpp>
+#if CV_MAJOR_VERSION >= 3
+#include <opencv2/calib3d/calib3d_c.h>
+#endif
 #include <opencv2/highgui/highgui.hpp>
 
 #include <QFileDialog>
@@ -97,6 +101,9 @@ CalibrationDialog::CalibrationDialog(bool stereo, const QString & savingDirector
 
 	ui_->checkBox_switchImages->setChecked(switchImages);
 
+	ui_->checkBox_fisheye->setChecked(false);
+	ui_->checkBox_fisheye->setEnabled(false);
+
 	this->setStereoMode(stereo_);
 }
 
@@ -623,6 +630,7 @@ void CalibrationDialog::restart()
 	maxIrs_[1] = 0x7fff;
 
 	ui_->pushButton_calibrate->setEnabled(ui_->checkBox_unlock->isChecked());
+	ui_->checkBox_fisheye->setEnabled(ui_->checkBox_unlock->isChecked());
 	ui_->pushButton_save->setEnabled(false);
 	ui_->radioButton_raw->setChecked(true);
 	ui_->radioButton_rectified->setEnabled(false);
@@ -666,6 +674,7 @@ void CalibrationDialog::restart()
 void CalibrationDialog::unlock()
 {
 	ui_->pushButton_calibrate->setEnabled(true);
+	ui_->checkBox_fisheye->setEnabled(true);
 }
 
 void CalibrationDialog::calibrate()
@@ -702,13 +711,44 @@ void CalibrationDialog::calibrate()
 		K = cv::Mat::eye(3,3,CV_64FC1);
 		UINFO("calibrate!");
 		//Find intrinsic and extrinsic camera parameters
-		double rms = cv::calibrateCamera(objectPoints,
-				imagePoints_[id],
-				imageSize_[id],
-				K,
-				D,
-				rvecs,
-				tvecs);
+		double rms = 0.0;
+#if CV_MAJOR_VERSION > 2 or (CV_MAJOR_VERSION == 2 and (CV_MINOR_VERSION >4 or (CV_MINOR_VERSION == 4 and CV_SUBMINOR_VERSION >=10)))
+		bool fishEye = ui_->checkBox_fisheye->isChecked();
+
+		if(fishEye)
+		{
+			try
+			{
+				rms = cv::fisheye::calibrate(objectPoints,
+					imagePoints_[id],
+					imageSize_[id],
+					K,
+					D,
+					rvecs,
+					tvecs,
+					cv::fisheye::CALIB_RECOMPUTE_EXTRINSIC |
+					cv::fisheye::CALIB_CHECK_COND |
+					cv::fisheye::CALIB_FIX_SKEW);
+			}
+			catch(const cv::Exception & e)
+			{
+				UERROR("Error: %s (try restarting the calibration)", e.what());
+				QMessageBox::warning(this, tr("Calibration failed!"), tr("Error: %1 (try restarting the calibration)").arg(e.what()));
+				processingData_ = false;
+				return;
+			}
+		}
+		else
+#endif
+		{
+			rms = cv::calibrateCamera(objectPoints,
+					imagePoints_[id],
+					imageSize_[id],
+					K,
+					D,
+					rvecs,
+					tvecs);
+		}
 
 		UINFO("Re-projection error reported by calibrateCamera: %f", rms);
 
@@ -720,7 +760,16 @@ void CalibrationDialog::calibrate()
 
 		for( i = 0; i < (int)objectPoints.size(); ++i )
 		{
-			cv::projectPoints( cv::Mat(objectPoints[i]), rvecs[i], tvecs[i], K, D, imagePoints2);
+#if CV_MAJOR_VERSION > 2 or (CV_MAJOR_VERSION == 2 and (CV_MINOR_VERSION >4 or (CV_MINOR_VERSION == 4 and CV_SUBMINOR_VERSION >=10)))
+			if(fishEye)
+			{
+				cv::fisheye::projectPoints( cv::Mat(objectPoints[i]), imagePoints2, rvecs[i], tvecs[i], K, D);
+			}
+			else
+#endif
+			{
+				cv::projectPoints( cv::Mat(objectPoints[i]), rvecs[i], tvecs[i], K, D, imagePoints2);
+			}
 			err = cv::norm(cv::Mat(imagePoints_[id][i]), cv::Mat(imagePoints2), CV_L2);
 
 			int n = (int)objectPoints[i].size();
@@ -737,6 +786,19 @@ void CalibrationDialog::calibrate()
 		P.at<double>(2,3) = 1;
 		K.copyTo(P.colRange(0,3).rowRange(0,3));
 
+#if CV_MAJOR_VERSION > 2 or (CV_MAJOR_VERSION == 2 and (CV_MINOR_VERSION >4 or (CV_MINOR_VERSION == 4 and CV_SUBMINOR_VERSION >=10)))
+		if(fishEye)
+		{
+			// Convert to unified distortion model (k1,k2,p1,p2,k3,k4)
+			cv::Mat newD = cv::Mat::zeros(1,6,CV_64FC1);
+			newD.at<double>(0,0) = D.at<double>(0,0);
+			newD.at<double>(0,1) = D.at<double>(0,1);
+			newD.at<double>(0,4) = D.at<double>(0,2);
+			newD.at<double>(0,5) = D.at<double>(0,3);
+			D = newD;
+		}
+#endif
+
 		std::cout << "K = " << K << std::endl;
 		std::cout << "D = " << D << std::endl;
 		std::cout << "width = " << imageSize_[id].width << std::endl;
@@ -871,28 +933,59 @@ StereoCameraModel CalibrationDialog::stereoCalibration(const CameraModel & left,
 			objectPoints[0].push_back(cv::Point3f(float(j*squareSize), float(i*squareSize), 0));
 	objectPoints.resize(stereoImagePoints_[0].size(), objectPoints[0]);
 
+	double rms = 0.0;
+#if CV_MAJOR_VERSION > 2 or (CV_MAJOR_VERSION == 2 and (CV_MINOR_VERSION >4 or (CV_MINOR_VERSION == 4 and CV_SUBMINOR_VERSION >=10)))
+	bool fishEye = left.D_raw().cols == 6;
 	// calibrate extrinsic
-#if CV_MAJOR_VERSION < 3
-	double rms = cv::stereoCalibrate(
-			objectPoints,
-			stereoImagePoints_[0],
-			stereoImagePoints_[1],
-			left.K_raw(), left.D_raw(),
-			right.K_raw(), right.D_raw(),
-			imageSize, R, T, E, F,
-			cv::TermCriteria(cv::TermCriteria::COUNT+cv::TermCriteria::EPS, 100, 1e-5),
-			cv::CALIB_FIX_INTRINSIC);
-#else
-	double rms = cv::stereoCalibrate(
-			objectPoints,
-			stereoImagePoints_[0],
-			stereoImagePoints_[1],
-			left.K_raw(), left.D_raw(),
-			right.K_raw(), right.D_raw(),
-			imageSize, R, T, E, F,
-			cv::CALIB_FIX_INTRINSIC,
-			cv::TermCriteria(cv::TermCriteria::COUNT+cv::TermCriteria::EPS, 100, 1e-5));
+	if(fishEye)
+	{
+		cv::Mat D_left(1,4,CV_64FC1);
+		D_left.at<double>(0,0) = left.D_raw().at<double>(0,0);
+		D_left.at<double>(0,1) = left.D_raw().at<double>(0,1);
+		D_left.at<double>(0,2) = left.D_raw().at<double>(0,4);
+		D_left.at<double>(0,3) = left.D_raw().at<double>(0,5);
+		cv::Mat D_right(1,4,CV_64FC1);
+		UASSERT(right.D_raw().cols == 6);
+		D_right.at<double>(0,0) = right.D_raw().at<double>(0,0);
+		D_right.at<double>(0,1) = right.D_raw().at<double>(0,1);
+		D_right.at<double>(0,2) = right.D_raw().at<double>(0,4);
+		D_right.at<double>(0,3) = right.D_raw().at<double>(0,5);
+
+		rms = cv::fisheye::stereoCalibrate(
+				objectPoints,
+				stereoImagePoints_[0],
+				stereoImagePoints_[1],
+				left.K_raw(), D_left,
+				right.K_raw(), D_right,
+				imageSize, R, T,
+				cv::CALIB_FIX_INTRINSIC,
+				cv::TermCriteria(cv::TermCriteria::COUNT+cv::TermCriteria::EPS, 100, 1e-5));
+	}
+	else
 #endif
+	{
+#if CV_MAJOR_VERSION < 3
+		rms = cv::stereoCalibrate(
+				objectPoints,
+				stereoImagePoints_[0],
+				stereoImagePoints_[1],
+				left.K_raw(), left.D_raw(),
+				right.K_raw(), right.D_raw(),
+				imageSize, R, T, E, F,
+				cv::TermCriteria(cv::TermCriteria::COUNT+cv::TermCriteria::EPS, 100, 1e-5),
+				cv::CALIB_FIX_INTRINSIC);
+#else
+		rms = cv::stereoCalibrate(
+				objectPoints,
+				stereoImagePoints_[0],
+				stereoImagePoints_[1],
+				left.K_raw(), left.D_raw(),
+				right.K_raw(), right.D_raw(),
+				imageSize, R, T, E, F,
+				cv::CALIB_FIX_INTRINSIC,
+				cv::TermCriteria(cv::TermCriteria::COUNT+cv::TermCriteria::EPS, 100, 1e-5));
+#endif
+	}
 	UINFO("stereo calibration... done with RMS error=%f", rms);
 
 	std::cout << "R = " << R << std::endl;
@@ -988,8 +1081,7 @@ bool CalibrationDialog::save()
 	else
 	{
 		UASSERT(stereoModel_.left().isValidForRectification() &&
-				stereoModel_.right().isValidForRectification() &&
-				(!ui_->label_baseline->isVisible() || stereoModel_.baseline() > 0.0));
+				stereoModel_.right().isValidForRectification());
 		QString cameraName = stereoModel_.name().c_str();
 		QString filePath = QFileDialog::getSaveFileName(this, tr("Export"), savingDirectory_ + "/" + cameraName, "*.yaml");
 		QString name = QFileInfo(filePath).baseName();

guilib/src/CameraViewer.cpp

@@ -136,7 +136,7 @@ void CameraViewer::showImage(const rtabmap::SensorData & data)
 	}
 	if(!data.depthOrRightRaw().empty())
 	{
-		imageView_->setImageDepth(uCvMat2QImage(util2d::decimate(data.depthOrRightRaw(), validDecimationValue_)));
+		imageView_->setImageDepth(util2d::decimate(data.depthOrRightRaw(), validDecimationValue_));
 	}
 
 	if(!data.depthOrRightRaw().empty() &&

guilib/src/CloudViewer.cpp

@@ -26,6 +26,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 */
 
 #include "rtabmap/gui/CloudViewer.h"
+#include "rtabmap/gui/CloudViewerCellPicker.h"
 
 #include <rtabmap/core/Version.h>
 #include <rtabmap/core/util3d_transforms.h>
@@ -66,10 +67,10 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #include <vtkTIFFReader.h>
 #include <vtkOpenGLRenderWindow.h>
 #include <vtkPointPicker.h>
-#include <vtkCellPicker.h>
 #include <vtkTextActor.h>
 #include <vtkOBBTree.h>
 #include <vtkObjectFactory.h>
+#include <vtkQuad.h>
 #include <opencv/vtkImageMatSource.h>
 
 #ifdef RTABMAP_OCTOMAP
@@ -78,286 +79,13 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 
 namespace rtabmap {
 
-// Standard VTK macro for *New ()
-vtkStandardNewMacro (CloudViewerInteractorStyle);
-
-CloudViewerInteractorStyle::CloudViewerInteractorStyle() :
-	pcl::visualization::PCLVisualizerInteractorStyle(),
-	viewer_(0),
-	NumberOfClicks(0),
-	ResetPixelDistance(0),
-	pointsHolder_(new pcl::PointCloud<pcl::PointXYZRGB>)
-{
-	PreviousPosition[0] = PreviousPosition[1] = 0;
-	PreviousMeasure[0] = PreviousMeasure[1] =  PreviousMeasure[2] = 0.0f;
-}
-
-void CloudViewerInteractorStyle::Rotate()
-{
-	if (this->CurrentRenderer == NULL)
-	{
-		return;
-	}
-
-	vtkRenderWindowInteractor *rwi = this->Interactor;
-
-	int dx = rwi->GetEventPosition()[0] - rwi->GetLastEventPosition()[0];
-	int dy = rwi->GetEventPosition()[1] - rwi->GetLastEventPosition()[1];
-
-	int *size = this->CurrentRenderer->GetRenderWindow()->GetSize();
-
-	double delta_elevation = -20.0 / size[1];
-	double delta_azimuth = -20.0 / size[0];
-
-	double rxf = dx * delta_azimuth * this->MotionFactor;
-	double ryf = dy * delta_elevation * this->MotionFactor;
-
-	vtkCamera *camera = this->CurrentRenderer->GetActiveCamera();
-	UASSERT(camera);
-	camera->Azimuth(rxf);
-	camera->Elevation(ryf);
-	camera->OrthogonalizeViewUp();
-
-	if (this->AutoAdjustCameraClippingRange)
-	{
-		this->CurrentRenderer->ResetCameraClippingRange();
-	}
-
-	if (rwi->GetLightFollowCamera())
-	{
-		this->CurrentRenderer->UpdateLightsGeometryToFollowCamera();
-	}
-
-	//rwi->Render();
-}
-
-void CloudViewerInteractorStyle::OnMouseMove()
-{
-	if(this->CurrentRenderer &&
-		this->CurrentRenderer->GetLayer() == 1 &&
-		this->GetInteractor()->GetShiftKey() && this->GetInteractor()->GetControlKey() &&
-		viewer_ &&
-		viewer_->getLocators().size())
-	{
-		vtkCellPicker * cellPicker = dynamic_cast<vtkCellPicker*>(this->Interactor->GetPicker());
-		if(cellPicker)
-		{
-			int pickPosition[2];
-			this->GetInteractor()->GetEventPosition(pickPosition);
-			this->Interactor->GetPicker()->Pick(pickPosition[0], pickPosition[1],
-					0,  // always zero.
-					this->CurrentRenderer);
-			double picked[3];
-			this->Interactor->GetPicker()->GetPickPosition(picked);
-
-			UDEBUG("Control move! Picked value: %f %f %f", picked[0], picked[1], picked[2]);
-
-			float textSize = 0.05;
-
-			viewer_->removeCloud("interactor_points_alt");
-			pointsHolder_->resize(2);
-			pcl::PointXYZRGB pt(255,0,0);
-			pt.x = picked[0];
-			pt.y = picked[1];
-			pt.z = picked[2];
-			pointsHolder_->at(0) = pt;
-
-			viewer_->removeLine("interactor_ray_alt");
-			viewer_->removeText("interactor_ray_text_alt");
-
-			// Intersect the locator with the line
-			double length = 5.0;
-			double pickedNormal[3];
-			cellPicker->GetPickNormal(pickedNormal);
-			double lineP0[3] = {picked[0], picked[1], picked[2]};
-			double lineP1[3] = {picked[0]+pickedNormal[0]*length, picked[1]+pickedNormal[1]*length, picked[2]+pickedNormal[2]*length};
-			vtkSmartPointer<vtkPoints> intersectPoints = vtkSmartPointer<vtkPoints>::New();
-
-			viewer_->getLocators().begin()->second->IntersectWithLine(lineP0, lineP1, intersectPoints, NULL);
-
-			// Display list of intersections
-			double intersection[3];
-			double previous[3] = {picked[0], picked[1], picked[2]};
-			for(int i = 0; i < intersectPoints->GetNumberOfPoints(); i++ )
-			{
-				intersectPoints->GetPoint(i, intersection);
-
-				Eigen::Vector3f v(intersection[0]-previous[0], intersection[1]-previous[1], intersection[2]-previous[2]);
-				float n = v.norm();
-				if(n  > 0.01f)
-				{
-					v/=n;
-					v *= n/2.0f;
-					pt.r = 125;
-					pt.g = 125;
-					pt.b = 125;
-					pt.x = intersection[0];
-					pt.y = intersection[1];
-					pt.z = intersection[2];
-					pointsHolder_->at(1) = pt;
-					viewer_->addOrUpdateText("interactor_ray_text_alt", uFormat("%.2f m", n),
-							Transform(previous[0]+v[0], previous[1]+v[1],previous[2]+v[2], 0, 0, 0),
-							textSize,
-							Qt::gray);
-					viewer_->addOrUpdateLine("interactor_ray_alt",
-							Transform(previous[0], previous[1], previous[2], 0, 0, 0),
-							Transform(intersection[0], intersection[1], intersection[2], 0, 0, 0),
-							Qt::gray);
-
-					previous[0] = intersection[0];
-					previous[1] = intersection[1];
-					previous[2] = intersection[2];
-					break;
-				}
-			}
-			viewer_->addCloud("interactor_points_alt", pointsHolder_);
-			viewer_->setCloudPointSize("interactor_points_alt", 15);
-			viewer_->setCloudOpacity("interactor_points_alt", 0.5);
-		}
-	}
-	// Forward events
-	PCLVisualizerInteractorStyle::OnMouseMove();
-}
-
-void CloudViewerInteractorStyle::OnLeftButtonDown()
-{
-	// http://www.vtk.org/Wiki/VTK/Examples/Cxx/Interaction/DoubleClick
-	// http://www.vtk.org/Wiki/VTK/Examples/Cxx/Interaction/PointPicker
-	if(this->CurrentRenderer && this->CurrentRenderer->GetLayer() == 1)
-	{
-		this->NumberOfClicks++;
-		int pickPosition[2];
-		this->GetInteractor()->GetEventPosition(pickPosition);
-		int xdist = pickPosition[0] - this->PreviousPosition[0];
-		int ydist = pickPosition[1] - this->PreviousPosition[1];
-
-		this->PreviousPosition[0] = pickPosition[0];
-		this->PreviousPosition[1] = pickPosition[1];
-
-		int moveDistance = (int)sqrt((double)(xdist*xdist + ydist*ydist));
-
-		// Reset numClicks - If mouse moved further than resetPixelDistance
-		if(moveDistance > this->ResetPixelDistance)
-		{
-			this->NumberOfClicks = 1;
-		}
-
-		if(this->NumberOfClicks >= 2)
-		{
-			this->NumberOfClicks = 0;
-			this->Interactor->GetPicker()->Pick(pickPosition[0], pickPosition[1],
-					0,  // always zero.
-					this->CurrentRenderer);
-			double picked[3];
-			this->Interactor->GetPicker()->GetPickPosition(picked);
-			UDEBUG("Double clicked! Picked value: %f %f %f", picked[0], picked[1], picked[2]);
-			if(this->GetInteractor()->GetControlKey()==0)
-			{
-				vtkCamera *camera = this->CurrentRenderer->GetActiveCamera();
-				UASSERT(camera);
-				double position[3];
-				double focal[3];
-				camera->GetPosition(position[0], position[1], position[2]);
-				camera->GetFocalPoint(focal[0], focal[1], focal[2]);
-				//camera->SetPosition (position[0] + (picked[0]-focal[0]), position[1] + (picked[1]-focal[1]), position[2] + (picked[2]-focal[2]));
-				camera->SetFocalPoint (picked[0], picked[1], picked[2]);
-				camera->OrthogonalizeViewUp();
-
-				if (this->AutoAdjustCameraClippingRange)
-				{
-					this->CurrentRenderer->ResetCameraClippingRange();
-				}
-
-				if (this->Interactor->GetLightFollowCamera())
-				{
-					this->CurrentRenderer->UpdateLightsGeometryToFollowCamera();
-				}
-			}
-			else if(viewer_)
-			{
-				viewer_->removeText("interactor_pose");
-				viewer_->removeLine("interactor_line");
-				viewer_->removeCloud("interactor_points");
-				viewer_->removeLine("interactor_ray");
-				viewer_->removeText("interactor_ray_text");
-				viewer_->removeCloud("interactor_points_alt");
-				viewer_->removeLine("interactor_ray_alt");
-				viewer_->removeText("interactor_ray_text_alt");
-				PreviousMeasure[0] = 0.0f;
-				PreviousMeasure[1] = 0.0f;
-				PreviousMeasure[2] = 0.0f;
-			}
-		}
-		else if(this->GetInteractor()->GetControlKey() && viewer_)
-		{
-			this->Interactor->GetPicker()->Pick(pickPosition[0], pickPosition[1],
-					0,  // always zero.
-					this->CurrentRenderer);
-			double picked[3];
-			this->Interactor->GetPicker()->GetPickPosition(picked);
-
-			UDEBUG("Shift clicked! Picked value: %f %f %f", picked[0], picked[1], picked[2]);
-
-			float textSize = 0.05;
-
-			viewer_->removeCloud("interactor_points");
-			pointsHolder_->clear();
-			pcl::PointXYZRGB pt(255,0,0);
-			pt.x = picked[0];
-			pt.y = picked[1];
-			pt.z = picked[2];
-			pointsHolder_->push_back(pt);
-
-			viewer_->removeLine("interactor_ray");
-			viewer_->removeText("interactor_ray_text");
-
-			if(	PreviousMeasure[0] != 0.0f && PreviousMeasure[1] != 0.0f && PreviousMeasure[2] != 0.0f &&
-				viewer_->getAddedLines().find("interactor_line") == viewer_->getAddedLines().end())
-			{
-				viewer_->addOrUpdateLine("interactor_line",
-						Transform(PreviousMeasure[0], PreviousMeasure[1], PreviousMeasure[2], 0, 0, 0),
-						Transform(picked[0], picked[1], picked[2], 0, 0, 0),
-						Qt::red);
-				pt.x = PreviousMeasure[0];
-				pt.y = PreviousMeasure[1];
-				pt.z = PreviousMeasure[2];
-				pointsHolder_->push_back(pt);
-
-				Eigen::Vector3f v(picked[0]-PreviousMeasure[0], picked[1]-PreviousMeasure[1], picked[2]-PreviousMeasure[2]);
-				float n = v.norm();
-				v/=n;
-				v *= n/2.0f;
-				viewer_->addOrUpdateText("interactor_pose", uFormat("%.2f m", n),
-						Transform(PreviousMeasure[0]+v[0], PreviousMeasure[1]+v[1],PreviousMeasure[2]+v[2], 0, 0, 0),
-						textSize,
-						Qt::red);
-			}
-			else
-			{
-				viewer_->removeText("interactor_pose");
-				viewer_->removeLine("interactor_line");
-			}
-			PreviousMeasure[0] = picked[0];
-			PreviousMeasure[1] = picked[1];
-			PreviousMeasure[2] = picked[2];
-
-			viewer_->addCloud("interactor_points", pointsHolder_);
-			viewer_->setCloudPointSize("interactor_points", 15);
-			viewer_->setCloudOpacity("interactor_points", 0.5);
-		}
-	}
-
-	// Forward events
-	PCLVisualizerInteractorStyle::OnLeftButtonDown();
-}
-
-
 CloudViewer::CloudViewer(QWidget *parent, CloudViewerInteractorStyle * style) :
 		QVTKWidget(parent),
 		_aLockCamera(0),
 		_aFollowCamera(0),
 		_aResetCamera(0),
 		_aLockViewZ(0),
+		_aCameraOrtho(0),
 		_aShowTrajectory(0),
 		_aSetTrajectorySize(0),
 		_aClearTrajectory(0),
@@ -497,6 +225,9 @@ void CloudViewer::createMenu()
 	_aLockViewZ = new QAction("Lock view Z", this);
 	_aLockViewZ->setCheckable(true);
 	_aLockViewZ->setChecked(true);
+	_aCameraOrtho = new QAction("Ortho mode", this);
+	_aCameraOrtho->setCheckable(true);
+	_aCameraOrtho->setChecked(false);
 	_aResetCamera = new QAction("Reset position", this);
 	_aShowTrajectory= new QAction("Show trajectory", this);
 	_aShowTrajectory->setCheckable(true);
@@ -540,6 +271,7 @@ void CloudViewer::createMenu()
 	cameraMenu->addAction(freeCamera);
 	cameraMenu->addSeparator();
 	cameraMenu->addAction(_aLockViewZ);
+	cameraMenu->addAction(_aCameraOrtho);
 	cameraMenu->addAction(_aResetCamera);
 	QActionGroup * group = new QActionGroup(this);
 	group->addAction(_aLockCamera);
@@ -631,6 +363,7 @@ void CloudViewer::saveSettings(QSettings & settings, const QString & group) cons
 	settings.setValue("camera_target_follow", this->isCameraTargetFollow());
 	settings.setValue("camera_free", this->isCameraFree());
 	settings.setValue("camera_lockZ", this->isCameraLockZ());
+	settings.setValue("camera_ortho", this->isCameraOrtho());
 
 	settings.setValue("bg_color", this->getDefaultBackgroundColor());
 	settings.setValue("rendering_rate", this->getRenderingRate());
@@ -677,6 +410,7 @@ void CloudViewer::loadSettings(QSettings & settings, const QString & group)
 		this->setCameraFree();
 	}
 	this->setCameraLockZ(settings.value("camera_lockZ", this->isCameraLockZ()).toBool());
+	this->setCameraOrtho(settings.value("camera_ortho", this->isCameraOrtho()).toBool());
 
 	this->setDefaultBackgroundColor(settings.value("bg_color", this->getDefaultBackgroundColor()).value<QColor>());
 	
@@ -909,6 +643,7 @@ bool CloudViewer::addCloudMesh(
 	UDEBUG("Adding %s with %d points and %d polygons", id.c_str(), (int)cloud->size(), (int)polygons.size());
 	if(_visualizer->addPolygonMesh<pcl::PointXYZ>(cloud, polygons, id, 1))
 	{
+		_visualizer->getCloudActorMap()->find(id)->second.actor->GetProperty()->SetAmbient(0.5);
 		_visualizer->getCloudActorMap()->find(id)->second.actor->GetProperty()->SetLighting(_aSetLighting->isChecked());
 		_visualizer->getCloudActorMap()->find(id)->second.actor->GetProperty()->SetInterpolation(_aSetFlatShading->isChecked()?VTK_FLAT:VTK_PHONG);
 		_visualizer->getCloudActorMap()->find(id)->second.actor->GetProperty()->SetEdgeVisibility(_aSetEdgeVisibility->isChecked());
@@ -942,6 +677,7 @@ bool CloudViewer::addCloudMesh(
 	UDEBUG("Adding %s with %d points and %d polygons", id.c_str(), (int)cloud->size(), (int)polygons.size());
 	if(_visualizer->addPolygonMesh<pcl::PointXYZRGB>(cloud, polygons, id, 1))
 	{
+		_visualizer->getCloudActorMap()->find(id)->second.actor->GetProperty()->SetAmbient(0.5);
 		_visualizer->getCloudActorMap()->find(id)->second.actor->GetProperty()->SetLighting(_aSetLighting->isChecked());
 		_visualizer->getCloudActorMap()->find(id)->second.actor->GetProperty()->SetInterpolation(_aSetFlatShading->isChecked()?VTK_FLAT:VTK_PHONG);
 		_visualizer->getCloudActorMap()->find(id)->second.actor->GetProperty()->SetEdgeVisibility(_aSetEdgeVisibility->isChecked());
@@ -975,6 +711,7 @@ bool CloudViewer::addCloudMesh(
 	UDEBUG("Adding %s with %d points and %d polygons", id.c_str(), (int)cloud->size(), (int)polygons.size());
 	if(_visualizer->addPolygonMesh<pcl::PointXYZRGBNormal>(cloud, polygons, id, 1))
 	{
+		_visualizer->getCloudActorMap()->find(id)->second.actor->GetProperty()->SetAmbient(0.5);
 		_visualizer->getCloudActorMap()->find(id)->second.actor->GetProperty()->SetLighting(_aSetLighting->isChecked());
 		_visualizer->getCloudActorMap()->find(id)->second.actor->GetProperty()->SetInterpolation(_aSetFlatShading->isChecked()?VTK_FLAT:VTK_PHONG);
 		_visualizer->getCloudActorMap()->find(id)->second.actor->GetProperty()->SetEdgeVisibility(_aSetEdgeVisibility->isChecked());
@@ -1007,6 +744,7 @@ bool CloudViewer::addCloudMesh(
 	UDEBUG("Adding %s with %d polygons", id.c_str(), (int)mesh->polygons.size());
 	if(_visualizer->addPolygonMesh(*mesh, id, 1))
 	{
+		_visualizer->getCloudActorMap()->find(id)->second.actor->GetProperty()->SetAmbient(0.5);
 		_visualizer->getCloudActorMap()->find(id)->second.actor->GetProperty()->SetLighting(_aSetLighting->isChecked());
 		_visualizer->getCloudActorMap()->find(id)->second.actor->GetProperty()->SetEdgeVisibility(_aSetEdgeVisibility->isChecked());
 		_visualizer->getCloudActorMap()->find(id)->second.actor->GetProperty()->SetBackfaceCulling(_aBackfaceCulling->isChecked());
@@ -1453,6 +1191,7 @@ bool CloudViewer::addTextureMesh (
   // Save the viewpoint transformation matrix to the global actor map
   (*_visualizer->getCloudActorMap())[id].viewpoint_transformation_ = transformation;
 
+  _visualizer->getCloudActorMap()->find(id)->second.actor->GetProperty()->SetAmbient(0.5);
   _visualizer->getCloudActorMap()->find(id)->second.actor->GetProperty()->SetLighting(_aSetLighting->isChecked());
   _visualizer->getCloudActorMap()->find(id)->second.actor->GetProperty()->SetInterpolation(_aSetFlatShading->isChecked()?VTK_FLAT:VTK_PHONG);
   _visualizer->getCloudActorMap()->find(id)->second.actor->GetProperty()->SetEdgeVisibility(_aSetEdgeVisibility->isChecked());
@@ -1783,6 +1522,148 @@ void CloudViewer::removeAllCubes()
 	UASSERT(_cubes.empty());
 }
 
+void CloudViewer::addOrUpdateQuad(
+		const std::string & id,
+		const Transform & pose,
+		float width,
+		float height,
+		const QColor & color,
+		bool foreground)
+{
+	addOrUpdateQuad(id, pose, width/2.0f, width/2.0f, height/2.0f, height/2.0f, color, foreground);
+}
+
+void CloudViewer::addOrUpdateQuad(
+		const std::string & id,
+		const Transform & pose,
+		float widthLeft,
+		float widthRight,
+		float heightBottom,
+		float heightTop,
+		const QColor & color,
+		bool foreground)
+{
+	if(id.empty())
+	{
+		UERROR("id should not be empty!");
+		return;
+	}
+
+	removeQuad(id);
+
+	if(!pose.isNull())
+	{
+		_quads.insert(id);
+
+		QColor c = Qt::gray;
+		if(color.isValid())
+		{
+			c = color;
+		}
+
+		// Create four points (must be in counter clockwise order)
+		double p0[3] = {0.0, -widthLeft, heightTop};
+		double p1[3] = {0.0, -widthLeft, -heightBottom};
+		double p2[3] = {0.0, widthRight, -heightBottom};
+		double p3[3] = {0.0, widthRight, heightTop};
+
+		// Add the points to a vtkPoints object
+		vtkSmartPointer<vtkPoints> points =
+				vtkSmartPointer<vtkPoints>::New();
+		points->InsertNextPoint(p0);
+		points->InsertNextPoint(p1);
+		points->InsertNextPoint(p2);
+		points->InsertNextPoint(p3);
+
+		// Create a quad on the four points
+		vtkSmartPointer<vtkQuad> quad =
+				vtkSmartPointer<vtkQuad>::New();
+		quad->GetPointIds()->SetId(0,0);
+		quad->GetPointIds()->SetId(1,1);
+		quad->GetPointIds()->SetId(2,2);
+		quad->GetPointIds()->SetId(3,3);
+
+		// Create a cell array to store the quad in
+		vtkSmartPointer<vtkCellArray> quads =
+				vtkSmartPointer<vtkCellArray>::New();
+		quads->InsertNextCell(quad);
+
+		// Create a polydata to store everything in
+		vtkSmartPointer<vtkPolyData> polydata =
+				vtkSmartPointer<vtkPolyData>::New();
+
+		// Add the points and quads to the dataset
+		polydata->SetPoints(points);
+		polydata->SetPolys(quads);
+
+		// Setup actor and mapper
+		vtkSmartPointer<vtkPolyDataMapper> mapper =
+				vtkSmartPointer<vtkPolyDataMapper>::New();
+#if VTK_MAJOR_VERSION <= 5
+		mapper->SetInput(polydata);
+#else
+		mapper->SetInputData(polydata);
+#endif
+
+		vtkSmartPointer<vtkLODActor> actor =
+				vtkSmartPointer<vtkLODActor>::New();
+		actor->SetMapper(mapper);
+		actor->GetProperty()->SetColor(c.redF(), c.greenF(), c.blueF());
+
+		//_visualizer->addActorToRenderer (actor, viewport);
+		// Add it to all renderers
+		_visualizer->getRendererCollection()->InitTraversal ();
+		vtkRenderer* renderer = NULL;
+		int i = 0;
+		while ((renderer = _visualizer->getRendererCollection()->GetNextItem ()) != NULL)
+		{
+			if ((foreground?2:1) == i)               // add the actor only to the specified viewport
+			{
+				renderer->AddActor (actor);
+			}
+			++i;
+		}
+
+		// Save the pointer/ID pair to the global actor map
+		(*_visualizer->getCloudActorMap())[id].actor = actor;
+
+		// Save the viewpoint transformation matrix to the global actor map
+		vtkSmartPointer<vtkMatrix4x4> transformation = vtkSmartPointer<vtkMatrix4x4>::New ();
+		pcl::visualization::PCLVisualizer::convertToVtkMatrix (pose.toEigen3f().matrix (), transformation);
+		(*_visualizer->getCloudActorMap())[id].viewpoint_transformation_ = transformation;
+		(*_visualizer->getCloudActorMap())[id].actor->SetUserMatrix (transformation);
+		(*_visualizer->getCloudActorMap())[id].actor->Modified ();
+
+		(*_visualizer->getCloudActorMap())[id].actor->GetProperty()->SetLighting(false);
+		_visualizer->setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_OPACITY, c.alphaF(), id);
+	}
+}
+
+void CloudViewer::removeQuad(const std::string & id)
+{
+	if(id.empty())
+	{
+		UERROR("id should not be empty!");
+		return;
+	}
+
+	if(_quads.find(id) != _quads.end())
+	{
+		_visualizer->removeShape(id);
+		_quads.erase(id);
+	}
+}
+
+void CloudViewer::removeAllQuads()
+{
+	std::set<std::string> quads = _quads;
+	for(std::set<std::string>::iterator iter = quads.begin(); iter!=quads.end(); ++iter)
+	{
+		this->removeQuad(*iter);
+	}
+	UASSERT(_quads.empty());
+}
+
 static const float frustum_vertices[] = {
     0.0f,  0.0f, 0.0f,
 	1.0f, 1.0f, 1.0f,
@@ -2151,7 +2032,14 @@ void CloudViewer::resetCamera()
 	{
 		// reset relative to last current pose
 		cv::Point3f pt = util3d::transformPoint(cv::Point3f(_lastPose.x(), _lastPose.y(), _lastPose.z()), ( _lastPose.rotation()*Transform(-1, 0, 0)).translation());
-		if(_aLockViewZ->isChecked())
+		if(_aCameraOrtho->isChecked())
+		{
+			_visualizer->setCameraPosition(
+					_lastPose.x(), _lastPose.y(), _lastPose.z()+5,
+					_lastPose.x(), _lastPose.y(), _lastPose.z(),
+					1, 0, 0, 1);
+		}
+		else if(_aLockViewZ->isChecked())
 		{
 			_visualizer->setCameraPosition(
 					pt.x, pt.y, pt.z,
@@ -2166,6 +2054,13 @@ void CloudViewer::resetCamera()
 					_lastPose.r31(), _lastPose.r32(), _lastPose.r33(), 1);
 		}
 	}
+	else if(_aCameraOrtho->isChecked())
+	{
+		_visualizer->setCameraPosition(
+				0, 0, 5,
+				0, 0, 0,
+				1, 0, 0, 1);
+	}
 	else
 	{
 		_visualizer->setCameraPosition(
@@ -2213,6 +2108,115 @@ Transform CloudViewer::getTargetPose() const
 	return _lastPose;
 }
 
+std::string CloudViewer::getIdByActor(vtkProp * actor) const
+{
+	pcl::visualization::CloudActorMapPtr cloudActorMap = _visualizer->getCloudActorMap();
+	for(pcl::visualization::CloudActorMap::iterator iter=cloudActorMap->begin(); iter!=cloudActorMap->end(); ++iter)
+	{
+		if(iter->second.actor.GetPointer() == actor)
+		{
+			return iter->first;
+		}
+	}
+
+#if PCL_VERSION_COMPARE(>=, 1, 7, 2)
+	// getShapeActorMap() not available in version < 1.7.2
+	pcl::visualization::ShapeActorMapPtr shapeActorMap = _visualizer->getShapeActorMap();
+	for(pcl::visualization::ShapeActorMap::iterator iter=shapeActorMap->begin(); iter!=shapeActorMap->end(); ++iter)
+	{
+		if(iter->second.GetPointer() == actor)
+		{
+			std::string id = iter->first;
+			while(id.back() == '*')
+			{
+				id.erase(id.size()-1);
+			}
+
+			return id;
+		}
+	}
+#endif
+	return std::string();
+}
+
+QColor CloudViewer::getColor(const std::string & id)
+{
+	QColor color;
+	pcl::visualization::CloudActorMap::iterator iter = _visualizer->getCloudActorMap()->find(id);
+	if(iter != _visualizer->getCloudActorMap()->end())
+	{
+		double r,g,b,a;
+		iter->second.actor->GetProperty()->GetColor(r,g,b);
+		a = iter->second.actor->GetProperty()->GetOpacity();
+		color.setRgbF(r, g, b, a);
+	}
+#if PCL_VERSION_COMPARE(>=, 1, 7, 2)
+	// getShapeActorMap() not available in version < 1.7.2
+	else
+	{
+		std::string idLayer1 = id+"*";
+		std::string idLayer2 = id+"**";
+		pcl::visualization::ShapeActorMap::iterator iter = _visualizer->getShapeActorMap()->find(id);
+		if(iter == _visualizer->getShapeActorMap()->end())
+		{
+			iter = _visualizer->getShapeActorMap()->find(idLayer1);
+			if(iter == _visualizer->getShapeActorMap()->end())
+			{
+				iter = _visualizer->getShapeActorMap()->find(idLayer2);
+			}
+		}
+		if(iter != _visualizer->getShapeActorMap()->end())
+		{
+			vtkActor * actor = vtkActor::SafeDownCast(iter->second);
+			if(actor)
+			{
+				double r,g,b,a;
+				actor->GetProperty()->GetColor(r,g,b);
+				a = actor->GetProperty()->GetOpacity();
+				color.setRgbF(r, g, b, a);
+			}
+		}
+	}
+#endif
+	return color;
+}
+
+void CloudViewer::setColor(const std::string & id, const QColor & color)
+{
+	pcl::visualization::CloudActorMap::iterator iter = _visualizer->getCloudActorMap()->find(id);
+	if(iter != _visualizer->getCloudActorMap()->end())
+	{
+		iter->second.actor->GetProperty()->SetColor(color.redF(),color.greenF(),color.blueF());
+		iter->second.actor->GetProperty()->SetOpacity(color.alphaF());
+	}
+#if PCL_VERSION_COMPARE(>=, 1, 7, 2)
+	// getShapeActorMap() not available in version < 1.7.2
+	else
+	{
+		std::string idLayer1 = id+"*";
+		std::string idLayer2 = id+"**";
+		pcl::visualization::ShapeActorMap::iterator iter = _visualizer->getShapeActorMap()->find(id);
+		if(iter == _visualizer->getShapeActorMap()->end())
+		{
+			iter = _visualizer->getShapeActorMap()->find(idLayer1);
+			if(iter == _visualizer->getShapeActorMap()->end())
+			{
+				iter = _visualizer->getShapeActorMap()->find(idLayer2);
+			}
+		}
+		if(iter != _visualizer->getShapeActorMap()->end())
+		{
+			vtkActor * actor = vtkActor::SafeDownCast(iter->second);
+			if(actor)
+			{
+				actor->GetProperty()->SetColor(color.redF(),color.greenF(),color.blueF());
+				actor->GetProperty()->SetOpacity(color.alphaF());
+			}
+		}
+	}
+#endif
+}
+
 void CloudViewer::setBackfaceCulling(bool enabled, bool frontfaceCulling)
 {
 	_aBackfaceCulling->setChecked(enabled);
@@ -2240,7 +2244,7 @@ void CloudViewer::setPolygonPicking(bool enabled)
 	}
 	else
 	{
-		vtkSmartPointer<vtkCellPicker> pp = vtkSmartPointer<vtkCellPicker>::New ();
+		vtkSmartPointer<CloudViewerCellPicker> pp = vtkSmartPointer<CloudViewerCellPicker>::New ();
 		pp->SetTolerance (pp->GetTolerance());
 		this->GetInteractor()->SetPicker (pp);
 		setMouseTracking(true);
@@ -2288,6 +2292,24 @@ void CloudViewer::setEdgeVisibility(bool visible)
 	this->update();
 }
 
+void CloudViewer::setInteractorLayer(int layer)
+{
+	_visualizer->getRendererCollection()->InitTraversal ();
+	vtkRenderer* renderer = NULL;
+	int i =0;
+	while ((renderer = _visualizer->getRendererCollection()->GetNextItem ()) != NULL)
+	{
+		if(i==layer)
+		{
+			_visualizer->getInteractorStyle()->SetDefaultRenderer(renderer);
+			_visualizer->getInteractorStyle()->SetCurrentRenderer(renderer);
+			return;
+		}
+		++i;
+	}
+	UWARN("Could not set layer %d to interactor (layers=%d).", layer, _visualizer->getRendererCollection()->GetNumberOfItems());
+}
+
 void CloudViewer::getCameraPosition(
 		float & x, float & y, float & z,
 		float & focalX, float & focalY, float & focalZ,
@@ -2370,7 +2392,7 @@ void CloudViewer::updateCameraTargetPosition(const Transform & pose)
 			std::vector<pcl::visualization::Camera> cameras;
 			_visualizer->getCameras(cameras);
 
-			if(_aLockCamera->isChecked())
+			if(_aLockCamera->isChecked() || _aCameraOrtho->isChecked())
 			{
 				//update camera position
 				Eigen::Vector3f diff = pos - Eigen::Vector3f(_lastPose.x(), _lastPose.y(), _lastPose.z());
@@ -2598,6 +2620,17 @@ void CloudViewer::setCameraLockZ(bool enabled)
 	_lastCameraOrientation= _lastCameraPose = cv::Vec3f(0,0,0);
 	_aLockViewZ->setChecked(enabled);
 }
+void CloudViewer::setCameraOrtho(bool enabled)
+{
+	_lastCameraOrientation= _lastCameraPose = cv::Vec3f(0,0,0);
+	CloudViewerInteractorStyle * interactor = CloudViewerInteractorStyle::SafeDownCast(this->GetInteractor()->GetInteractorStyle());
+	if(interactor)
+	{
+		interactor->setOrthoMode(enabled);
+		this->update();
+	}
+	_aCameraOrtho->setChecked(enabled);
+}
 bool CloudViewer::isCameraTargetLocked() const
 {
 	return _aLockCamera->isChecked();
@@ -2614,6 +2647,10 @@ bool CloudViewer::isCameraLockZ() const
 {
 	return _aLockViewZ->isChecked();
 }
+bool CloudViewer::isCameraOrtho() const
+{
+	return _aCameraOrtho->isChecked();
+}
 double CloudViewer::getRenderingRate() const
 {
 	return _renderingRate;
@@ -2933,7 +2970,7 @@ void CloudViewer::mouseMoveEvent(QMouseEvent * event)
 	QVTKWidget::mouseMoveEvent(event);
 
 	// camera view up z locked?
-	if(_aLockViewZ->isChecked())
+	if(_aLockViewZ->isChecked() && !_aCameraOrtho->isChecked())
 	{
 		std::vector<pcl::visualization::Camera> cameras;
 		_visualizer->getCameras(cameras);
@@ -2954,6 +2991,18 @@ void CloudViewer::mouseMoveEvent(QMouseEvent * event)
 			_lastCameraOrientation = newCameraOrientation;
 			_lastCameraPose = cv::Vec3d(cameras.front().pos);
 		}
+		else
+		{
+			if(cameras.front().view[2] == 0)
+			{
+				cameras.front().pos[0] -= 0.00001*cameras.front().view[0];
+				cameras.front().pos[1] -= 0.00001*cameras.front().view[1];
+			}
+			else
+			{
+				cameras.front().pos[0] -= 0.00001;
+			}
+		}
 		cameras.front().view[0] = 0;
 		cameras.front().view[1] = 0;
 		cameras.front().view[2] = 1;
@@ -2972,7 +3021,7 @@ void CloudViewer::mouseMoveEvent(QMouseEvent * event)
 void CloudViewer::wheelEvent(QWheelEvent * event)
 {
 	QVTKWidget::wheelEvent(event);
-	if(_aLockViewZ->isChecked())
+	if(_aLockViewZ->isChecked() && !_aCameraOrtho->isChecked())
 	{
 		std::vector<pcl::visualization::Camera> cameras;
 		_visualizer->getCameras(cameras);
@@ -3111,6 +3160,10 @@ void CloudViewer::handleAction(QAction * a)
 			this->update();
 		}
 	}
+	else if(a == _aCameraOrtho)
+	{
+		this->setCameraOrtho(_aCameraOrtho->isChecked());
+	}
 	else if(a == _aSetLighting)
 	{
 		this->setLighting(_aSetLighting->isChecked());

guilib/src/CloudViewerCellPicker.cpp

@@ -0,0 +1,387 @@
+/*
+ * CloudViewerCellPicker.cpp
+ *
+ *  Created on: Aug 21, 2018
+ *      Author: mathieu
+ */
+
+#include "rtabmap/gui/CloudViewerCellPicker.h"
+
+#include <vtkImageData.h>
+#include <vtkRenderer.h>
+#include <vtkAbstractPicker.h>
+#include <vtkPicker.h>
+#include <vtkAbstractCellLocator.h>
+#include <vtkIdList.h>
+#include <vtkCellPicker.h>
+#include <vtkLODProp3D.h>
+#include <vtkMapper.h>
+#include <vtkGenericCell.h>
+#include <vtkMath.h>
+#include <vtkTexture.h>
+#include <vtkObjectFactory.h>
+#include <vtkSmartPointer.h>
+#include <vtkPoints.h>
+#include <vtkProperty.h>
+
+namespace rtabmap {
+
+// Standard VTK macro for *New ()
+vtkStandardNewMacro (CloudViewerCellPicker);
+
+CloudViewerCellPicker::CloudViewerCellPicker()
+{
+	cell_ = vtkGenericCell::New();
+	pointIds_ = vtkIdList::New();
+}
+
+CloudViewerCellPicker::~CloudViewerCellPicker()
+{
+	cell_->Delete();
+	pointIds_->Delete();
+}
+
+double CloudViewerCellPicker::IntersectActorWithLine(const double p1[3],
+		const double p2[3],
+		double t1, double t2,
+		double tol,
+		vtkProp3D *prop,
+		vtkMapper *mapper)
+{
+	// This code was taken from the original CellPicker with almost no
+	// modification except for the locator and texture additions.
+
+	// Intersect each cell with ray.  Keep track of one closest to
+	// the eye (within the tolerance tol) and within the clipping range).
+	// Note that we fudge the "closest to" (tMin+this->Tolerance) a little and
+	// keep track of the cell with the best pick based on parametric
+	// coordinate (pick the minimum, maximum parametric distance). This
+	// breaks ties in a reasonable way when cells are the same distance
+	// from the eye (like cells laying on a 2D plane).
+
+	vtkDataSet *data = mapper->GetInput();
+	double tMin = VTK_DOUBLE_MAX;
+	double minPCoords[3];
+	double pDistMin = VTK_DOUBLE_MAX;
+	vtkIdType minCellId = -1;
+	int minSubId = -1;
+	double minXYZ[3];
+	minXYZ[0] = minXYZ[1] = minXYZ[2] = 0.0;
+	double ray[3] = {p2[0]-p1[0], p2[1]-p1[1], p2[2]-p1[2]};
+	vtkMath::Normalize(ray);
+	vtkActor * actor = vtkActor::SafeDownCast(prop);
+
+	// Polydata has no 3D cells
+	int isPolyData = data->IsA("vtkPolyData");
+
+	vtkCollectionSimpleIterator iter;
+	vtkAbstractCellLocator *locator = 0;
+	this->Locators->InitTraversal(iter);
+	while ( (locator = static_cast<vtkAbstractCellLocator *>(
+			this->Locators->GetNextItemAsObject(iter))) )
+	{
+		if (locator->GetDataSet() == data)
+		{
+			break;
+		}
+	}
+
+	// Make a new p1 and p2 using the clipped t1 and t2
+	double q1[3], q2[3];
+	q1[0] = p1[0]; q1[1] = p1[1]; q1[2] = p1[2];
+	q2[0] = p2[0]; q2[1] = p2[1]; q2[2] = p2[2];
+	if (t1 != 0.0 || t2 != 1.0)
+	{
+		for (int j = 0; j < 3; j++)
+		{
+			q1[j] = p1[j]*(1.0 - t1) + p2[j]*t1;
+			q2[j] = p1[j]*(1.0 - t2) + p2[j]*t2;
+		}
+	}
+
+	// Use the locator if one exists for this data
+	if (locator)
+	{
+		vtkSmartPointer<vtkPoints> intersectPoints = vtkSmartPointer<vtkPoints>::New();
+		vtkSmartPointer<vtkIdList> intersectCells = vtkSmartPointer<vtkIdList>::New();
+
+		locator->IntersectWithLine(q1, q2, intersectPoints, intersectCells);
+		for(int i = 0; i < intersectPoints->GetNumberOfPoints(); i++ )
+		{
+			double intersection[3];
+			intersectPoints->GetPoint(i, intersection);
+		}
+
+		if (!locator->IntersectWithLine(q1, q2, tol, tMin, minXYZ,
+				minPCoords, minSubId, minCellId,
+				this->cell_))
+		{
+			return VTK_DOUBLE_MAX;
+		}
+
+		// Stretch tMin out to the original range
+		if (t1 != 0.0 || t2 != 1.0)
+		{
+			tMin = t1*(1.0 - tMin) + t2*tMin;
+		}
+
+		// If cell is a strip, then replace cell with a sub-cell
+		this->SubCellFromCell(this->cell_, minSubId);
+	}
+	else
+	{
+		vtkIdList *pointIds = this->pointIds_;
+		vtkIdType numCells = data->GetNumberOfCells();
+
+		for (vtkIdType cellId = 0; cellId < numCells; cellId++)
+		{
+			double t;
+			double x[3];
+			double pcoords[3];
+			pcoords[0] = pcoords[1] = pcoords[2] = 0;
+			int newSubId = -1;
+			int numSubIds = 1;
+
+			// If it is a strip, we need to iterate over the subIds
+			int cellType = data->GetCellType(cellId);
+			int useSubCells = this->HasSubCells(cellType);
+			if (useSubCells)
+			{
+				// Get the pointIds for the strip and the length of the strip
+				data->GetCellPoints(cellId, pointIds);
+				numSubIds = this->GetNumberOfSubCells(pointIds, cellType);
+			}
+
+			// This will only loop once unless we need to deal with a strip
+			for (int subId = 0; subId < numSubIds; subId++)
+			{
+				if (useSubCells)
+				{
+					// Get a sub-cell from a the strip
+					this->GetSubCell(data, pointIds, subId, cellType, this->cell_);
+				}
+				else
+				{
+					data->GetCell(cellId, this->cell_);
+				}
+
+				int cellPicked = 0;
+				if (isPolyData)
+				{
+					// Polydata can always be picked with original endpoints
+					cellPicked = this->cell_->IntersectWithLine(
+							const_cast<double *>(p1), const_cast<double *>(p2),
+							tol, t, x, pcoords, newSubId);
+				}
+				else
+				{
+					// Any 3D cells need to be intersected with a line segment that
+					// has been clipped with the clipping planes, in case one end is
+					// actually inside the cell.
+					cellPicked = this->cell_->IntersectWithLine(
+							q1, q2, tol, t, x, pcoords, newSubId);
+
+					// Stretch t out to the original range
+					if (t1 != 0.0 || t2 != 1.0)
+					{
+						t = t1*(1.0 - t) + t2*t;
+					}
+				}
+
+				if (cellPicked && t <= (tMin + this->Tolerance) && t >= t1 && t <= t2)
+				{
+					double pDist = this->cell_->GetParametricDistance(pcoords);
+					if (pDist < pDistMin || (pDist == pDistMin && t < tMin))
+					{
+////////////////////////////////////////////////////////////////////////////////////
+// BEGIN: Modifications from VTK 6.2
+////////////////////////////////////////////////////////////////////////////////////
+						bool visible = true;
+						if(actor->GetProperty()->GetBackfaceCulling() ||
+						   actor->GetProperty()->GetFrontfaceCulling())
+						{
+							// Get the cell weights
+							vtkIdType numPoints = this->cell_->GetNumberOfPoints();
+							double *weights = new double[numPoints];
+							for (vtkIdType i = 0; i < numPoints; i++)
+							{
+								weights[i] = 0;
+							}
+
+							// Get the interpolation weights (point is thrown away)
+							double point[3] = {0.0,0.0,0.0};
+							this->cell_->EvaluateLocation(minSubId, minPCoords, point, weights);
+
+							double normal[3] = {0.0,0.0,0.0};
+
+							if (this->ComputeSurfaceNormal(data, this->cell_, weights, normal))
+							{
+								if(actor->GetProperty()->GetBackfaceCulling())
+								{
+									visible = ray[0]*normal[0] + ray[1]*normal[1] + ray[2]*normal[2] <= 0;
+								}
+								else
+								{
+									visible = ray[0]*normal[0] + ray[1]*normal[1] + ray[2]*normal[2] >= 0;
+								}
+							}
+							delete [] weights;
+						}
+						if(visible)
+						{
+							tMin = t;
+							pDistMin = pDist;
+							// save all of these
+							minCellId = cellId;
+							minSubId = newSubId;
+							if (useSubCells)
+							{
+								minSubId = subId;
+							}
+							for (int k = 0; k < 3; k++)
+							{
+								minXYZ[k] = x[k];
+								minPCoords[k] = pcoords[k];
+							}
+						}
+////////////////////////////////////////////////////////////////////////////////////
+// END: Modifications from VTK 6.2
+////////////////////////////////////////////////////////////////////////////////////
+					} // for all subIds
+				} // if minimum, maximum
+			} // if a close cell
+		} // for all cells
+	}
+
+	// Do this if a cell was intersected
+	if (minCellId >= 0 && tMin < this->GlobalTMin)
+	{
+		this->ResetPickInfo();
+
+		// Get the cell, convert to triangle if it is a strip
+		vtkGenericCell *cell = this->cell_;
+
+		// If we used a locator, we already have the picked cell
+		if (!locator)
+		{
+			int cellType = data->GetCellType(minCellId);
+
+			if (this->HasSubCells(cellType))
+			{
+				data->GetCellPoints(minCellId, this->pointIds_);
+				this->GetSubCell(data, this->pointIds_, minSubId, cellType, cell);
+			}
+			else
+			{
+				data->GetCell(minCellId, cell);
+			}
+		}
+
+		// Get the cell weights
+		vtkIdType numPoints = cell->GetNumberOfPoints();
+		double *weights = new double[numPoints];
+		for (vtkIdType i = 0; i < numPoints; i++)
+		{
+			weights[i] = 0;
+		}
+
+		// Get the interpolation weights (point is thrown away)
+		double point[3];
+		cell->EvaluateLocation(minSubId, minPCoords, point, weights);
+
+		this->Mapper = mapper;
+
+		// Get the texture from the actor or the LOD
+		vtkActor *actor = 0;
+		vtkLODProp3D *lodActor = 0;
+		if ( (actor = vtkActor::SafeDownCast(prop)) )
+		{
+			this->Texture = actor->GetTexture();
+		}
+		else if ( (lodActor = vtkLODProp3D::SafeDownCast(prop)) )
+		{
+			int lodId = lodActor->GetPickLODID();
+			lodActor->GetLODTexture(lodId, &this->Texture);
+		}
+
+		if (this->PickTextureData && this->Texture)
+		{
+			// Return the texture's image data to the user
+			vtkImageData *image = this->Texture->GetInput();
+			this->DataSet = image;
+
+			// Get and check the image dimensions
+			int extent[6];
+			image->GetExtent(extent);
+			int dimensionsAreValid = 1;
+			int dimensions[3];
+			for (int i = 0; i < 3; i++)
+			{
+				dimensions[i] = extent[2*i + 1] - extent[2*i] + 1;
+				dimensionsAreValid = (dimensionsAreValid && dimensions[i] > 0);
+			}
+
+			// Use the texture coord to set the information
+			double tcoord[3];
+			if (dimensionsAreValid &&
+					this->ComputeSurfaceTCoord(data, cell, weights, tcoord))
+			{
+				// Take the border into account when computing coordinates
+				double x[3];
+				x[0] = extent[0] + tcoord[0]*dimensions[0] - 0.5;
+				x[1] = extent[2] + tcoord[1]*dimensions[1] - 0.5;
+				x[2] = extent[4] + tcoord[2]*dimensions[2] - 0.5;
+
+				this->SetImageDataPickInfo(x, extent);
+			}
+		}
+		else
+		{
+			// Return the polydata to the user
+			this->DataSet = data;
+			this->CellId = minCellId;
+			this->SubId = minSubId;
+			this->PCoords[0] = minPCoords[0];
+			this->PCoords[1] = minPCoords[1];
+			this->PCoords[2] = minPCoords[2];
+
+			// Find the point with the maximum weight
+			double maxWeight = 0;
+			vtkIdType iMaxWeight = -1;
+			for (vtkIdType i = 0; i < numPoints; i++)
+			{
+				if (weights[i] > maxWeight)
+				{
+					iMaxWeight = i;
+				}
+			}
+
+			// If maximum weight is found, use it to get the PointId
+			if (iMaxWeight != -1)
+			{
+				this->PointId = cell->PointIds->GetId(iMaxWeight);
+			}
+		}
+
+		// Set the mapper position
+		this->MapperPosition[0] = minXYZ[0];
+		this->MapperPosition[1] = minXYZ[1];
+		this->MapperPosition[2] = minXYZ[2];
+
+		// Compute the normal
+		if (!this->ComputeSurfaceNormal(data, cell, weights, this->MapperNormal))
+		{
+			// By default, the normal points back along view ray
+			this->MapperNormal[0] = p1[0] - p2[0];
+			this->MapperNormal[1] = p1[1] - p2[1];
+			this->MapperNormal[2] = p1[2] - p2[2];
+			vtkMath::Normalize(this->MapperNormal);
+		}
+
+		delete [] weights;
+	}
+
+	return tMin;
+}
+
+} /* namespace rtabmap */

guilib/src/CloudViewerInteractorStyle.cpp

@@ -0,0 +1,328 @@
+/*
+ * CloudViewerInteractorStyl.cpp
+ *
+ *  Created on: Aug 21, 2018
+ *      Author: mathieu
+ */
+
+#include "rtabmap/gui/CloudViewerInteractorStyle.h"
+#include "rtabmap/gui/CloudViewer.h"
+#include "rtabmap/gui/CloudViewerCellPicker.h"
+
+#include "rtabmap/utilite/ULogger.h"
+#include "rtabmap/utilite/UConversion.h"
+#include "rtabmap/utilite/UMath.h"
+
+#include <vtkRenderer.h>
+#include <vtkRenderWindow.h>
+#include <vtkObjectFactory.h>
+#include <vtkOBBTree.h>
+#include <vtkCamera.h>
+
+namespace rtabmap {
+
+// Standard VTK macro for *New ()
+vtkStandardNewMacro (CloudViewerInteractorStyle);
+
+CloudViewerInteractorStyle::CloudViewerInteractorStyle() :
+	pcl::visualization::PCLVisualizerInteractorStyle(),
+	viewer_(0),
+	NumberOfClicks(0),
+	ResetPixelDistance(0),
+	pointsHolder_(new pcl::PointCloud<pcl::PointXYZRGB>),
+	orthoMode_(false)
+{
+	PreviousPosition[0] = PreviousPosition[1] = 0;
+	PreviousMeasure[0] = PreviousMeasure[1] =  PreviousMeasure[2] = 0.0f;
+
+	this->MotionFactor = 5;
+}
+
+void CloudViewerInteractorStyle::Rotate()
+{
+	if (this->CurrentRenderer == NULL)
+	{
+		return;
+	}
+
+	vtkRenderWindowInteractor *rwi = this->Interactor;
+
+	int dx = rwi->GetEventPosition()[0] - rwi->GetLastEventPosition()[0];
+	int dy = orthoMode_?0:rwi->GetEventPosition()[1] - rwi->GetLastEventPosition()[1];
+
+	int *size = this->CurrentRenderer->GetRenderWindow()->GetSize();
+
+	double delta_elevation = -20.0 / size[1];
+	double delta_azimuth = -20.0 / size[0];
+
+	double rxf = dx * delta_azimuth * this->MotionFactor;
+	double ryf = dy * delta_elevation * this->MotionFactor;
+
+	vtkCamera *camera = this->CurrentRenderer->GetActiveCamera();
+	UASSERT(camera);
+	if(!orthoMode_)
+	{
+		camera->Azimuth(rxf);
+		camera->Elevation(ryf);
+		camera->OrthogonalizeViewUp();
+	}
+	else
+	{
+		camera->Roll(-rxf);
+	}
+
+	if (this->AutoAdjustCameraClippingRange)
+	{
+		this->CurrentRenderer->ResetCameraClippingRange();
+	}
+
+	if (rwi->GetLightFollowCamera())
+	{
+		this->CurrentRenderer->UpdateLightsGeometryToFollowCamera();
+	}
+
+	//rwi->Render();
+}
+
+void CloudViewerInteractorStyle::setOrthoMode(bool enabled)
+{
+	if (this->CurrentRenderer == NULL)
+	{
+		return;
+	}
+
+	vtkCamera *camera = CurrentRenderer->GetActiveCamera ();
+	UASSERT(camera);
+	camera->SetParallelProjection (enabled);
+	if(enabled)
+	{
+		double x,y,z;
+		camera->GetFocalPoint(x, y, z);
+		camera->SetPosition(x, y, z+(camera->GetDistance()<=5?5:camera->GetDistance()));
+		camera->SetViewUp(1, 0, 0);
+	}
+	CurrentRenderer->SetActiveCamera (camera);
+	orthoMode_ = enabled;
+}
+
+void CloudViewerInteractorStyle::OnMouseMove()
+{
+	if(this->CurrentRenderer &&
+		this->CurrentRenderer->GetLayer() == 1 &&
+		this->GetInteractor()->GetShiftKey() && this->GetInteractor()->GetControlKey() &&
+		viewer_ &&
+		viewer_->getLocators().size())
+	{
+		CloudViewerCellPicker * cellPicker = dynamic_cast<CloudViewerCellPicker*>(this->Interactor->GetPicker());
+		if(cellPicker)
+		{
+			int pickPosition[2];
+			this->GetInteractor()->GetEventPosition(pickPosition);
+			this->Interactor->GetPicker()->Pick(pickPosition[0], pickPosition[1],
+					0,  // always zero.
+					this->CurrentRenderer);
+			double picked[3];
+			this->Interactor->GetPicker()->GetPickPosition(picked);
+
+			UDEBUG("Control move! Picked value: %f %f %f", picked[0], picked[1], picked[2]);
+
+			float textSize = 0.05;
+
+			viewer_->removeCloud("interactor_points_alt");
+			pointsHolder_->resize(2);
+			pcl::PointXYZRGB pt(255,0,0);
+			pt.x = picked[0];
+			pt.y = picked[1];
+			pt.z = picked[2];
+			pointsHolder_->at(0) = pt;
+
+			viewer_->removeLine("interactor_ray_alt");
+			viewer_->removeText("interactor_ray_text_alt");
+
+			// Intersect the locator with the line
+			double length = 5.0;
+			double pickedNormal[3];
+			cellPicker->GetPickNormal(pickedNormal);
+			double lineP0[3] = {picked[0], picked[1], picked[2]};
+			double lineP1[3] = {picked[0]+pickedNormal[0]*length, picked[1]+pickedNormal[1]*length, picked[2]+pickedNormal[2]*length};
+			vtkSmartPointer<vtkPoints> intersectPoints = vtkSmartPointer<vtkPoints>::New();
+
+			viewer_->getLocators().begin()->second->IntersectWithLine(lineP0, lineP1, intersectPoints, NULL);
+
+			// Display list of intersections
+			double intersection[3];
+			double previous[3] = {picked[0], picked[1], picked[2]};
+			for(int i = 0; i < intersectPoints->GetNumberOfPoints(); i++ )
+			{
+				intersectPoints->GetPoint(i, intersection);
+
+				Eigen::Vector3f v(intersection[0]-previous[0], intersection[1]-previous[1], intersection[2]-previous[2]);
+				float n = v.norm();
+				if(n  > 0.01f)
+				{
+					v/=n;
+					v *= n/2.0f;
+					pt.r = 125;
+					pt.g = 125;
+					pt.b = 125;
+					pt.x = intersection[0];
+					pt.y = intersection[1];
+					pt.z = intersection[2];
+					pointsHolder_->at(1) = pt;
+					viewer_->addOrUpdateText("interactor_ray_text_alt", uFormat("%.2f m", n),
+							Transform(previous[0]+v[0], previous[1]+v[1],previous[2]+v[2], 0, 0, 0),
+							textSize,
+							Qt::gray);
+					viewer_->addOrUpdateLine("interactor_ray_alt",
+							Transform(previous[0], previous[1], previous[2], 0, 0, 0),
+							Transform(intersection[0], intersection[1], intersection[2], 0, 0, 0),
+							Qt::gray);
+
+					previous[0] = intersection[0];
+					previous[1] = intersection[1];
+					previous[2] = intersection[2];
+					break;
+				}
+			}
+			viewer_->addCloud("interactor_points_alt", pointsHolder_);
+			viewer_->setCloudPointSize("interactor_points_alt", 15);
+			viewer_->setCloudOpacity("interactor_points_alt", 0.5);
+		}
+	}
+	// Forward events
+	PCLVisualizerInteractorStyle::OnMouseMove();
+}
+
+void CloudViewerInteractorStyle::OnLeftButtonDown()
+{
+	// http://www.vtk.org/Wiki/VTK/Examples/Cxx/Interaction/DoubleClick
+	// http://www.vtk.org/Wiki/VTK/Examples/Cxx/Interaction/PointPicker
+	if(this->CurrentRenderer && this->CurrentRenderer->GetLayer() == 1)
+	{
+		this->NumberOfClicks++;
+		int pickPosition[2];
+		this->GetInteractor()->GetEventPosition(pickPosition);
+		int xdist = pickPosition[0] - this->PreviousPosition[0];
+		int ydist = pickPosition[1] - this->PreviousPosition[1];
+
+		this->PreviousPosition[0] = pickPosition[0];
+		this->PreviousPosition[1] = pickPosition[1];
+
+		int moveDistance = (int)sqrt((double)(xdist*xdist + ydist*ydist));
+
+		// Reset numClicks - If mouse moved further than resetPixelDistance
+		if(moveDistance > this->ResetPixelDistance)
+		{
+			this->NumberOfClicks = 1;
+		}
+
+		if(this->NumberOfClicks >= 2)
+		{
+			this->NumberOfClicks = 0;
+			this->Interactor->GetPicker()->Pick(pickPosition[0], pickPosition[1],
+					0,  // always zero.
+					this->CurrentRenderer);
+			double picked[3];
+			this->Interactor->GetPicker()->GetPickPosition(picked);
+			UDEBUG("Double clicked! Picked value: %f %f %f", picked[0], picked[1], picked[2]);
+			if(this->GetInteractor()->GetControlKey()==0)
+			{
+				vtkCamera *camera = this->CurrentRenderer->GetActiveCamera();
+				UASSERT(camera);
+				double position[3];
+				double focal[3];
+				camera->GetPosition(position[0], position[1], position[2]);
+				camera->GetFocalPoint(focal[0], focal[1], focal[2]);
+				//camera->SetPosition (position[0] + (picked[0]-focal[0]), position[1] + (picked[1]-focal[1]), position[2] + (picked[2]-focal[2]));
+				camera->SetFocalPoint (picked[0], picked[1], picked[2]);
+				camera->OrthogonalizeViewUp();
+
+				if (this->AutoAdjustCameraClippingRange)
+				{
+					this->CurrentRenderer->ResetCameraClippingRange();
+				}
+
+				if (this->Interactor->GetLightFollowCamera())
+				{
+					this->CurrentRenderer->UpdateLightsGeometryToFollowCamera();
+				}
+			}
+			else if(viewer_)
+			{
+				viewer_->removeText("interactor_pose");
+				viewer_->removeLine("interactor_line");
+				viewer_->removeCloud("interactor_points");
+				viewer_->removeLine("interactor_ray");
+				viewer_->removeText("interactor_ray_text");
+				viewer_->removeCloud("interactor_points_alt");
+				viewer_->removeLine("interactor_ray_alt");
+				viewer_->removeText("interactor_ray_text_alt");
+				PreviousMeasure[0] = 0.0f;
+				PreviousMeasure[1] = 0.0f;
+				PreviousMeasure[2] = 0.0f;
+			}
+		}
+		else if(this->GetInteractor()->GetControlKey() && viewer_)
+		{
+			this->Interactor->GetPicker()->Pick(pickPosition[0], pickPosition[1],
+					0,  // always zero.
+					this->CurrentRenderer);
+			double picked[3];
+			this->Interactor->GetPicker()->GetPickPosition(picked);
+
+			UDEBUG("Shift clicked! Picked value: %f %f %f", picked[0], picked[1], picked[2]);
+
+			float textSize = 0.05;
+
+			viewer_->removeCloud("interactor_points");
+			pointsHolder_->clear();
+			pcl::PointXYZRGB pt(255,0,0);
+			pt.x = picked[0];
+			pt.y = picked[1];
+			pt.z = picked[2];
+			pointsHolder_->push_back(pt);
+
+			viewer_->removeLine("interactor_ray");
+			viewer_->removeText("interactor_ray_text");
+
+			if(	PreviousMeasure[0] != 0.0f && PreviousMeasure[1] != 0.0f && PreviousMeasure[2] != 0.0f &&
+				viewer_->getAddedLines().find("interactor_line") == viewer_->getAddedLines().end())
+			{
+				viewer_->addOrUpdateLine("interactor_line",
+						Transform(PreviousMeasure[0], PreviousMeasure[1], PreviousMeasure[2], 0, 0, 0),
+						Transform(picked[0], picked[1], picked[2], 0, 0, 0),
+						Qt::red);
+				pt.x = PreviousMeasure[0];
+				pt.y = PreviousMeasure[1];
+				pt.z = PreviousMeasure[2];
+				pointsHolder_->push_back(pt);
+
+				Eigen::Vector3f v(picked[0]-PreviousMeasure[0], picked[1]-PreviousMeasure[1], picked[2]-PreviousMeasure[2]);
+				float n = v.norm();
+				v/=n;
+				v *= n/2.0f;
+				viewer_->addOrUpdateText("interactor_pose", uFormat("%.2f m", n),
+						Transform(PreviousMeasure[0]+v[0], PreviousMeasure[1]+v[1],PreviousMeasure[2]+v[2], 0, 0, 0),
+						textSize,
+						Qt::red);
+			}
+			else
+			{
+				viewer_->removeText("interactor_pose");
+				viewer_->removeLine("interactor_line");
+			}
+			PreviousMeasure[0] = picked[0];
+			PreviousMeasure[1] = picked[1];
+			PreviousMeasure[2] = picked[2];
+
+			viewer_->addCloud("interactor_points", pointsHolder_);
+			viewer_->setCloudPointSize("interactor_points", 15);
+			viewer_->setCloudOpacity("interactor_points", 0.5);
+		}
+	}
+
+	// Forward events
+	PCLVisualizerInteractorStyle::OnLeftButtonDown();
+}
+
+} /* namespace rtabmap */

guilib/src/DataRecorder.cpp

@@ -157,7 +157,7 @@ void DataRecorder::showImage(const cv::Mat & image, const cv::Mat & depth)
 {
 	processingImages_ = true;
 	imageView_->setImage(uCvMat2QImage(image));
-	imageView_->setImageDepth(uCvMat2QImage(depth));
+	imageView_->setImageDepth(depth);
 	label_->setText(tr("Images=%1 (~%2 MB)").arg(count_).arg(totalSizeKB_/1000));
 	processingImages_ = false;
 }

guilib/src/DatabaseViewer.cpp

@@ -44,6 +44,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #include <rtabmap/utilite/UDirectory.h>
 #include <rtabmap/utilite/UConversion.h>
 #include <opencv2/core/core_c.h>
+#include <opencv2/imgproc/types_c.h>
 #include <opencv2/highgui/highgui.hpp>
 #include <rtabmap/utilite/UTimer.h>
 #include <rtabmap/utilite/UFile.h>
@@ -348,6 +349,8 @@ DatabaseViewer::DatabaseViewer(const QString & ini, QWidget * parent) :
 	connect(ui_->doubleSpinBox_gainCompensationRadius, SIGNAL(valueChanged(double)), this, SLOT(updateConstraintView()));
 	connect(ui_->doubleSpinBox_voxelSize, SIGNAL(valueChanged(double)), this, SLOT(updateConstraintView()));
 	connect(ui_->doubleSpinBox_voxelSize, SIGNAL(valueChanged(double)), this, SLOT(update3dView()));
+	connect(ui_->spinBox_decimation, SIGNAL(valueChanged(int)), this, SLOT(updateConstraintView()));
+	connect(ui_->spinBox_decimation, SIGNAL(valueChanged(int)), this, SLOT(update3dView()));
 	connect(ui_->groupBox_posefiltering, SIGNAL(clicked(bool)), this, SLOT(updateGraphView()));
 	connect(ui_->doubleSpinBox_posefilteringRadius, SIGNAL(editingFinished()), this, SLOT(updateGraphView()));
 	connect(ui_->doubleSpinBox_posefilteringAngle, SIGNAL(editingFinished()), this, SLOT(updateGraphView()));
@@ -373,6 +376,7 @@ DatabaseViewer::DatabaseViewer(const QString & ini, QWidget * parent) :
 	// Graph view
 	connect(ui_->doubleSpinBox_gainCompensationRadius, SIGNAL(valueChanged(double)), this, SLOT(configModified()));
 	connect(ui_->doubleSpinBox_voxelSize, SIGNAL(valueChanged(double)), this, SLOT(configModified()));
+	connect(ui_->spinBox_decimation, SIGNAL(valueChanged(int)), this, SLOT(configModified()));
 	connect(ui_->groupBox_posefiltering, SIGNAL(clicked(bool)), this, SLOT(configModified()));
 	connect(ui_->doubleSpinBox_posefilteringRadius, SIGNAL(valueChanged(double)), this, SLOT(configModified()));
 	connect(ui_->doubleSpinBox_posefilteringAngle, SIGNAL(valueChanged(double)), this, SLOT(configModified()));
@@ -419,15 +423,9 @@ DatabaseViewer::DatabaseViewer(const QString & ini, QWidget * parent) :
 DatabaseViewer::~DatabaseViewer()
 {
 	delete ui_;
-	if(dbDriver_)
-	{
-		delete dbDriver_;
-	}
+	delete dbDriver_;
 #ifdef RTABMAP_OCTOMAP
-	if(octomap_)
-	{
-		delete octomap_;
-	}
+	delete octomap_;
 #endif
 }
 
@@ -502,7 +500,7 @@ void DatabaseViewer::readSettings()
 	settings.beginGroup("optimization");
 	ui_->doubleSpinBox_gainCompensationRadius->setValue(settings.value("gainCompensationRadius", ui_->doubleSpinBox_gainCompensationRadius->value()).toDouble());
 	ui_->doubleSpinBox_voxelSize->setValue(settings.value("voxelSize", ui_->doubleSpinBox_voxelSize->value()).toDouble());
-
+	ui_->spinBox_decimation->setValue(settings.value("decimation", ui_->spinBox_decimation->value()).toInt());
 	settings.endGroup();
 
 	settings.beginGroup("grid");
@@ -583,6 +581,7 @@ void DatabaseViewer::writeSettings()
 	settings.beginGroup("optimization");
 	settings.setValue("gainCompensationRadius", ui_->doubleSpinBox_gainCompensationRadius->value());
 	settings.setValue("voxelSize", ui_->doubleSpinBox_voxelSize->value());
+	settings.setValue("decimation", ui_->spinBox_decimation->value());
 	settings.endGroup();
 
 	// save Grid settings
@@ -666,6 +665,7 @@ void DatabaseViewer::restoreDefaultSettings()
 	ui_->doubleSpinBox_optimizationScale->setValue(1.0);
 	ui_->doubleSpinBox_gainCompensationRadius->setValue(0.0);
 	ui_->doubleSpinBox_voxelSize->setValue(0.0);
+	ui_->spinBox_decimation->setValue(1);
 
 	ui_->groupBox_posefiltering->setChecked(false);
 	ui_->doubleSpinBox_posefilteringRadius->setValue(0.1);
@@ -978,6 +978,7 @@ bool DatabaseViewer::closeDatabase()
 		ui_->label_constraint->clear();
 		ui_->label_constraint_opt->clear();
 		ui_->label_variance->clear();
+		ui_->lineEdit_covariance->clear();
 
 		ui_->horizontalSlider_A->setEnabled(false);
 		ui_->horizontalSlider_A->setMaximum(0);
@@ -3431,7 +3432,7 @@ void DatabaseViewer::update(int value,
 		{
 			//image
 			QImage img;
-			QImage imgDepth;
+			cv::Mat imgDepth;
 			if(dbDriver_)
 			{
 				SensorData data;
@@ -3451,7 +3452,7 @@ void DatabaseViewer::update(int value,
 							depth = util2d::fillDepthHoles(depth, ui_->spinBox_mesh_fillDepthHoles->value(), float(ui_->spinBox_mesh_depthError->value())/100.0f);
 						}
 					}
-					imgDepth = uCvMat2QImage(depth);
+					imgDepth = depth;
 				}
 
 				std::list<int> ids;
@@ -3593,7 +3594,7 @@ void DatabaseViewer::update(int value,
 											data.imageRaw(),
 											depth,
 											data.cameraModels()[0],
-											1,0,0,indices.get());
+											ui_->spinBox_decimation->value(),0,0,indices.get());
 									if(indices->size())
 									{
 										cloud = util3d::transformPointCloud(cloud, data.cameraModels()[0].localTransform());
@@ -3602,7 +3603,7 @@ void DatabaseViewer::update(int value,
 								}
 								else
 								{
-									cloud = util3d::cloudRGBFromSensorData(data, 1, 0, 0, indices.get(), ui_->parameters_toolbox->getParameters());
+									cloud = util3d::cloudRGBFromSensorData(data, ui_->spinBox_decimation->value(), 0, 0, indices.get(), ui_->parameters_toolbox->getParameters());
 								}
 								if(indices->size())
 								{
@@ -3670,7 +3671,7 @@ void DatabaseViewer::update(int value,
 							{
 								pcl::PointCloud<pcl::PointXYZ>::Ptr cloud;
 								pcl::IndicesPtr indices(new std::vector<int>);
-								cloud = util3d::cloudFromSensorData(data, 1, 0, 0, indices.get(), ui_->parameters_toolbox->getParameters());
+								cloud = util3d::cloudFromSensorData(data, ui_->spinBox_decimation->value(), 0, 0, indices.get(), ui_->parameters_toolbox->getParameters());
 								if(indices->size())
 								{
 									if(ui_->doubleSpinBox_voxelSize->value() > 0.0)
@@ -3931,12 +3932,13 @@ void DatabaseViewer::update(int value,
 				ULOGGER_DEBUG("Image is empty");
 			}
 
-			if(!imgDepth.isNull())
+			if(!imgDepth.empty())
 			{
 				view->setImageDepth(imgDepth);
 				if(img.isNull())
 				{
-					rect = imgDepth.rect();
+					rect.setWidth(imgDepth.cols);
+					rect.setHeight(imgDepth.rows);
 				}
 			}
 			else
@@ -4223,7 +4225,7 @@ void DatabaseViewer::updateStereo(const SensorData * data)
 		ui_->graphicsView_stereo->setImageDepthShown(true);
 
 		ui_->graphicsView_stereo->setImage(uCvMat2QImage(data->imageRaw()));
-		ui_->graphicsView_stereo->setImageDepth(uCvMat2QImage(data->depthOrRightRaw()));
+		ui_->graphicsView_stereo->setImageDepth(data->depthOrRightRaw());
 
 		// Draw lines between corresponding features...
 		for(unsigned int i=0; i<kpts.size(); ++i)
@@ -4283,7 +4285,7 @@ void DatabaseViewer::updateWordsMatching()
 			QList<int> ids =  wordsA.uniqueKeys();
 			for(int i=0; i<ids.size(); ++i)
 			{
-				if(wordsA.count(ids[i]) == 1 && wordsB.count(ids[i]) == 1)
+				if(ids[i] > 0 && wordsA.count(ids[i]) == 1 && wordsB.count(ids[i]) == 1)
 				{
 					// PINK features
 					ui_->graphicsView_A->setFeatureColor(ids[i], Qt::magenta);
@@ -4447,8 +4449,11 @@ void DatabaseViewer::updateConstraintView(
 				 link.type()==Link::kUserClosure?"User link":
 				 link.type()==Link::kVirtualClosure?"Virtual link":"Undefined"));
 	ui_->label_variance->setText(QString("%1, %2")
-			.arg(sqrt(link.rotVariance()))
-			.arg(sqrt(link.transVariance())));
+			.arg(sqrt(link.transVariance()))
+			.arg(sqrt(link.rotVariance())));
+	std::stringstream out;
+	out << link.infMatrix().inv();
+	ui_->lineEdit_covariance->setText(out.str().c_str());
 	ui_->label_constraint->setText(QString("%1").arg(t.prettyPrint().c_str()).replace(" ", "\n"));
 	if(graphes_.size() &&
 	   (int)graphes_.size()-1 == ui_->horizontalSlider_iterations->maximum())
@@ -4574,11 +4579,11 @@ void DatabaseViewer::updateConstraintView(
 			pcl::IndicesPtr indicesTo(new std::vector<int>);
 			if(!dataFrom.imageRaw().empty() && !dataFrom.depthOrRightRaw().empty())
 			{
-				cloudFrom=util3d::cloudRGBFromSensorData(dataFrom, 1, 0, 0, indicesFrom.get(), ui_->parameters_toolbox->getParameters());
+				cloudFrom=util3d::cloudRGBFromSensorData(dataFrom, ui_->spinBox_decimation->value(), 0, 0, indicesFrom.get(), ui_->parameters_toolbox->getParameters());
 			}
 			if(!dataTo.imageRaw().empty() && !dataTo.depthOrRightRaw().empty())
 			{
-				cloudTo=util3d::cloudRGBFromSensorData(dataTo, 1, 0, 0, indicesTo.get(), ui_->parameters_toolbox->getParameters());
+				cloudTo=util3d::cloudRGBFromSensorData(dataTo, ui_->spinBox_decimation->value(), 0, 0, indicesTo.get(), ui_->parameters_toolbox->getParameters());
 			}
 
 			if(cloudTo.get() && indicesTo->size())
@@ -6272,7 +6277,9 @@ bool DatabaseViewer::addConstraint(int from, int to, bool silent)
 		std::multimap<int, Link> linksIn = updateLinksWithModifications(links_);
 		linksIn.insert(std::make_pair(newLink.from(), newLink));
 		const Link * maxLinearLink = 0;
+		const Link * maxAngularLink = 0;
 		float maxLinearErrorRatio = 0.0f;
+		float maxAngularErrorRatio = 0.0f;
 		Optimizer * optimizer = Optimizer::create(ui_->parameters_toolbox->getParameters());
 		std::map<int, Transform> poses;
 		std::multimap<int, Link> links;
@@ -6288,38 +6295,52 @@ bool DatabaseViewer::addConstraint(int from, int to, bool silent)
 		std::string msg;
 		if(poses.size())
 		{
+			float maxLinearError = 0.0f;
+			float maxAngularError = 0.0f;
 			for(std::multimap<int, Link>::iterator iter=links.begin(); iter!=links.end(); ++iter)
 			{
 				// ignore links with high variance
 				if(iter->second.transVariance() <= 1.0 && iter->second.from() != iter->second.to())
 				{
-					UASSERT(poses.find(iter->second.from())!=poses.end());
-					UASSERT(poses.find(iter->second.to())!=poses.end());
-					Transform t1 = poses.at(iter->second.from());
-					Transform t2 = poses.at(iter->second.to());
-					UASSERT(!t1.isNull() && !t2.isNull());
+					Transform t1 = uValue(poses, iter->second.from(), Transform());
+					Transform t2 = uValue(poses, iter->second.to(), Transform());
 					Transform t = t1.inverse()*t2;
 					float linearError = uMax3(
 							fabs(iter->second.transform().x() - t.x()),
 							fabs(iter->second.transform().y() - t.y()),
 							fabs(iter->second.transform().z() - t.z()));
-					float stddev = sqrt(iter->second.transVariance());
-					float linearErrorRatio = linearError/stddev;
+					float opt_roll,opt__pitch,opt__yaw;
+					float link_roll,link_pitch,link_yaw;
+					t.getEulerAngles(opt_roll, opt__pitch, opt__yaw);
+					iter->second.transform().getEulerAngles(link_roll, link_pitch, link_yaw);
+					float angularError = uMax3(
+							fabs(opt_roll - link_roll),
+							fabs(opt__pitch - link_pitch),
+							fabs(opt__yaw - link_yaw));
+					float stddevLinear = sqrt(iter->second.transVariance());
+					float linearErrorRatio = linearError/stddevLinear;
 					if(linearErrorRatio > maxLinearErrorRatio)
 					{
+						maxLinearError = linearError;
 						maxLinearErrorRatio = linearErrorRatio;
 						maxLinearLink = &iter->second;
 					}
+					float stddevAngular = sqrt(iter->second.rotVariance());
+					float angularErrorRatio = angularError/stddevAngular;
+					if(angularErrorRatio > maxAngularErrorRatio)
+					{
+						maxAngularError = angularError;
+						maxAngularErrorRatio = angularErrorRatio;
+						maxAngularLink = &iter->second;
+					}
 				}
 			}
 			if(maxLinearLink)
 			{
-				UINFO("Max optimization linear error ratio = %f (link %d->%d)", maxLinearErrorRatio, maxLinearLink->from(), maxLinearLink->to());
-			}
-
-			if(maxLinearErrorRatio > maxOptimizationError)
-			{
-				msg = uFormat("Rejecting edge %d->%d because "
+				UINFO("Max optimization linear error = %f m (link %d->%d, var=%f, ratio error/std=%f)", maxLinearError, maxLinearLink->from(), maxLinearLink->to(), maxLinearLink->transVariance(), maxLinearError/sqrt(maxLinearLink->transVariance()));
+				if(maxLinearErrorRatio > maxOptimizationError)
+				{
+					msg = uFormat("Rejecting edge %d->%d because "
 						  "graph error is too large after optimization (ratio %f for edge %d->%d, stddev=%f). "
 						  "\"%s\" is %f.",
 						  newLink.from(),
@@ -6330,6 +6351,25 @@ bool DatabaseViewer::addConstraint(int from, int to, bool silent)
 						  sqrt(maxLinearLink->transVariance()),
 						  Parameters::kRGBDOptimizeMaxError().c_str(),
 						  maxOptimizationError);
+				}
+			}
+			if(maxAngularLink)
+			{
+				UINFO("Max optimization angular error = %f deg (link %d->%d, var=%f, ratio error/std=%f)", maxAngularError*180.0f/CV_PI, maxAngularLink->from(), maxAngularLink->to(), maxAngularLink->rotVariance(), maxAngularError/sqrt(maxAngularLink->rotVariance()));
+				if(maxAngularErrorRatio > maxOptimizationError)
+				{
+					msg = uFormat("Rejecting edge %d->%d because "
+						  "graph error is too large after optimization (ratio %f for edge %d->%d, stddev=%f). "
+						  "\"%s\" is %f.",
+						  newLink.from(),
+						  newLink.to(),
+						  maxAngularErrorRatio,
+						  maxAngularLink->from(),
+						  maxAngularLink->to(),
+						  sqrt(maxAngularLink->rotVariance()),
+						  Parameters::kRGBDOptimizeMaxError().c_str(),
+						  maxOptimizationError);
+				}
 			}
 		}
 		else

guilib/src/DepthCalibrationDialog.cpp

@@ -94,10 +94,7 @@ DepthCalibrationDialog::DepthCalibrationDialog(QWidget *parent) :
 DepthCalibrationDialog::~DepthCalibrationDialog()
 {
 	delete _ui;
-	if(_model)
-	{
-		delete _model;
-	}
+	delete _model;
 }
 
 void DepthCalibrationDialog::saveSettings(QSettings & settings, const QString & group) const

guilib/src/ExportCloudsDialog.cpp

@@ -148,6 +148,7 @@ ExportCloudsDialog::ExportCloudsDialog(QWidget *parent) :
 	connect(_ui->spinBox_randomPoints, SIGNAL(valueChanged(int)), this, SIGNAL(configChanged()));
 	connect(_ui->doubleSpinBox_dilationVoxelSize, SIGNAL(valueChanged(double)), this, SIGNAL(configChanged()));
 	connect(_ui->spinBox_dilationSteps, SIGNAL(valueChanged(int)), this, SIGNAL(configChanged()));
+	connect(_ui->doubleSpinBox_mls_outputVoxelSize, SIGNAL(valueChanged(double)), this, SIGNAL(configChanged()));
 	_ui->stackedWidget_upsampling->setCurrentIndex(_ui->comboBox_upsamplingMethod->currentIndex());
 	connect(_ui->comboBox_upsamplingMethod, SIGNAL(currentIndexChanged(int)), _ui->stackedWidget_upsampling, SLOT(setCurrentIndex(int)));
 	connect(_ui->comboBox_upsamplingMethod, SIGNAL(currentIndexChanged(int)), this, SLOT(updateMLSGrpVisibility()));
@@ -252,10 +253,7 @@ ExportCloudsDialog::ExportCloudsDialog(QWidget *parent) :
 ExportCloudsDialog::~ExportCloudsDialog()
 {
 	delete _ui;
-	if(_compensator)
-	{
-		delete _compensator;
-	}
+	delete _compensator;
 }
 
 void ExportCloudsDialog::updateMLSGrpVisibility()
@@ -338,6 +336,7 @@ void ExportCloudsDialog::saveSettings(QSettings & settings, const QString & grou
 	settings.setValue("mls_point_density", _ui->spinBox_randomPoints->value());
 	settings.setValue("mls_dilation_voxel_size", _ui->doubleSpinBox_dilationVoxelSize->value());
 	settings.setValue("mls_dilation_iterations", _ui->spinBox_dilationSteps->value());
+	settings.setValue("mls_output_voxel_size", _ui->doubleSpinBox_mls_outputVoxelSize->value());
 
 	settings.setValue("gain", _ui->checkBox_gainCompensation->isChecked());
 	settings.setValue("gain_radius", _ui->doubleSpinBox_gainRadius->value());
@@ -472,6 +471,7 @@ void ExportCloudsDialog::loadSettings(QSettings & settings, const QString & grou
 	_ui->spinBox_randomPoints->setValue(settings.value("mls_point_density", _ui->spinBox_randomPoints->value()).toInt());
 	_ui->doubleSpinBox_dilationVoxelSize->setValue(settings.value("mls_dilation_voxel_size", _ui->doubleSpinBox_dilationVoxelSize->value()).toDouble());
 	_ui->spinBox_dilationSteps->setValue(settings.value("mls_dilation_iterations", _ui->spinBox_dilationSteps->value()).toInt());
+	_ui->doubleSpinBox_mls_outputVoxelSize->setValue(settings.value("mls_output_voxel_size", _ui->doubleSpinBox_mls_outputVoxelSize->value()).toInt());
 
 	_ui->checkBox_gainCompensation->setChecked(settings.value("gain", _ui->checkBox_gainCompensation->isChecked()).toBool());
 	_ui->doubleSpinBox_gainRadius->setValue(settings.value("gain_radius", _ui->doubleSpinBox_gainRadius->value()).toDouble());
@@ -599,10 +599,11 @@ void ExportCloudsDialog::restoreDefaults()
 	_ui->spinBox_polygonialOrder->setValue(2);
 	_ui->comboBox_upsamplingMethod->setCurrentIndex(0);
 	_ui->doubleSpinBox_sampleRadius->setValue(0.01);
-	_ui->doubleSpinBox_sampleStep->setValue(0.0);
-	_ui->spinBox_randomPoints->setValue(0);
-	_ui->doubleSpinBox_dilationVoxelSize->setValue(0.01);
-	_ui->spinBox_dilationSteps->setValue(0);
+	_ui->doubleSpinBox_sampleStep->setValue(0.005);
+	_ui->spinBox_randomPoints->setValue(10);
+	_ui->doubleSpinBox_dilationVoxelSize->setValue(0.005);
+	_ui->spinBox_dilationSteps->setValue(1);
+	_ui->doubleSpinBox_mls_outputVoxelSize->setValue(0);
 
 	_ui->checkBox_gainCompensation->setChecked(false);
 	_ui->doubleSpinBox_gainRadius->setValue(0.02);
@@ -1694,16 +1695,16 @@ bool ExportCloudsDialog::getExportedClouds(
 				if(_ui->checkBox_assemble->isChecked())
 				{
 					// Re-voxelize to make sure to have uniform density
-					if(_ui->doubleSpinBox_voxelSize_assembled->value())
+                    if(_ui->doubleSpinBox_mls_outputVoxelSize->value())
 					{
 						_progressDialog->appendText(tr("Voxelize cloud (%1 points, voxel size = %2 m)...")
 								.arg(cloudWithNormals->size())
-								.arg(_ui->doubleSpinBox_voxelSize_assembled->value()));
+								.arg(_ui->doubleSpinBox_mls_outputVoxelSize->value()));
 						QApplication::processEvents();
 
 						cloudWithNormals = util3d::voxelize(
 								cloudWithNormals,
-								_ui->doubleSpinBox_voxelSize_assembled->value());
+								_ui->doubleSpinBox_mls_outputVoxelSize->value());
 					}
 				
 					_progressDialog->appendText(tr("Update %1 normals with %2 camera views...").arg(cloudWithNormals->size()).arg(poses.size()));

guilib/src/GraphViewer.cpp

@@ -64,11 +64,16 @@ public:
 		QGraphicsEllipseItem(QRectF(-radius*100.0f,-radius*100.0f,radius*100.0f*2.0f,radius*100.0f*2.0f)),
 		_id(id),
 		_mapId(mapId),
-		_pose(pose)
+		_pose(pose),
+		_line(0)
 	{
 		this->setPos(-pose.y()*100.0f,-pose.x()*100.0f);
 		this->setBrush(pen().color());
 		this->setAcceptHoverEvents(true);
+		float r,p,yaw;
+		pose.getEulerAngles(r, p, yaw);
+		radius*=100.0f;
+		_line = new QGraphicsLineItem(0,0,-radius*sin(yaw),-radius*cos(yaw), this);
 	}
 	virtual ~NodeItem() {}
 
@@ -80,6 +85,17 @@ public:
 		QBrush b = this->brush();
 		b.setColor(color);
 		this->setBrush(b);
+
+		_line->setPen(QPen(QColor(255-color.red(), 255-color.green(), 255-color.blue())));
+	}
+
+	void setRadius(float radius)
+	{
+		float r,p,yaw;
+		_pose.getEulerAngles(r, p, yaw);
+		radius*=100.0f;
+		this->setRect(-radius, -radius, radius*2.0f, radius*2.0f);
+		_line->setLine(0,0,-radius*sin(yaw),-radius*cos(yaw));
 	}
 
 	int id() const {return _id;};
@@ -105,6 +121,7 @@ private:
 	int _id;
 	int _mapId;
 	Transform _pose;
+	QGraphicsLineItem * _line;
 };
 
 class NodeGPSItem: public NodeItem
@@ -822,30 +839,28 @@ void GraphViewer::updateMap(const cv::Mat & map8U, float resolution, float xMin,
 	}
 }
 
-void GraphViewer::updatePosterior(const std::map<int, float> & posterior, float fixedMax)
+void GraphViewer::updatePosterior(const std::map<int, float> & posterior, float max)
 {
 	//find max
-	float max = 0.0f;
-	for(std::map<int, float>::const_iterator iter = posterior.begin(); iter!=posterior.end(); ++iter)
+	if(max <= 0.0f)
 	{
-		if(iter->first > 0 && iter->second>max)
+		for(std::map<int, float>::const_iterator iter = posterior.begin(); iter!=posterior.end(); ++iter)
 		{
-			max = iter->second;
+			if(iter->first > 0 && iter->second>max)
+			{
+				max = iter->second;
+			}
 		}
 	}
 	if(max > 0.0f)
 	{
-		if(fixedMax > 0.0f && max < fixedMax)
-		{
-			max = fixedMax;
-		}
 		for(QMap<int, NodeItem*>::iterator iter = _nodeItems.begin(); iter!=_nodeItems.end(); ++iter)
 		{
 			std::map<int,float>::const_iterator jter = posterior.find(iter.key());
 			if(jter != posterior.end())
 			{
-				//UDEBUG("id=%d max=%f hyp=%f color = %f", iter.key(), max, jter->second, (1-jter->second/max)*240.0f/360.0f);
-				iter.value()->setColor(QColor::fromHsvF((1-jter->second/max)*240.0f/360.0f, 1, 1, 1)); //0=red 240=blue
+				float v = jter->second>max?max:jter->second;
+				iter.value()->setColor(QColor::fromHsvF((1-v/max)*240.0f/360.0f, 1, 1, 1)); //0=red 240=blue
 			}
 			else
 			{
@@ -1172,15 +1187,15 @@ void GraphViewer::setNodeRadius(float radius)
 	_nodeRadius = radius;
 	for(QMap<int, NodeItem*>::iterator iter=_nodeItems.begin(); iter!=_nodeItems.end(); ++iter)
 	{
-		iter.value()->setRect(-_nodeRadius*100.0f, -_nodeRadius*100.0f, _nodeRadius*100.0f*2.0f, _nodeRadius*100.0f*2.0f);
+		iter.value()->setRadius(_nodeRadius);
 	}
 	for(QMap<int, NodeItem*>::iterator iter=_gtNodeItems.begin(); iter!=_gtNodeItems.end(); ++iter)
 	{
-		iter.value()->setRect(-_nodeRadius*100.0f, -_nodeRadius*100.0f, _nodeRadius*100.0f*2.0f, _nodeRadius*100.0f*2.0f);
+		iter.value()->setRadius(_nodeRadius);
 	}
 	for(QMap<int, NodeItem*>::iterator iter=_gpsNodeItems.begin(); iter!=_gpsNodeItems.end(); ++iter)
 	{
-		iter.value()->setRect(-_nodeRadius*100.0f, -_nodeRadius*100.0f, _nodeRadius*100.0f*2.0f, _nodeRadius*100.0f*2.0f);
+		iter.value()->setRadius(_nodeRadius);
 	}
 }
 void GraphViewer::setLinkWidth(float width)

guilib/src/GuiLib.qrc

@@ -34,5 +34,6 @@
         <file>images/r200.png</file>
         <file>images/zr300.png</file>
         <file>images/d435.png</file>
+        <file>images/d415.png</file>
     </qresource>
 </RCC>

guilib/src/ImageView.cpp

@@ -38,6 +38,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #include <QVBoxLayout>
 #include <QGraphicsRectItem>
 #include "rtabmap/utilite/ULogger.h"
+#include "rtabmap/utilite/UCv2Qt.h"
 #include "rtabmap/gui/KeypointItem.h"
 #include "rtabmap/core/util2d.h"
 
@@ -58,10 +59,7 @@ public:
 	}
 	virtual ~LineItem()
 	{
-		if(_placeHolder)
-		{
-			delete _placeHolder;
-		}
+		delete _placeHolder;
 	}
 
 	void setColor(const QColor & color);
@@ -186,10 +184,39 @@ ImageView::ImageView(QWidget * parent) :
 	_graphicsViewMode = _menu->addAction(tr("Graphics view"));
 	_graphicsViewMode->setCheckable(true);
 	_graphicsViewMode->setChecked(false);
-	_graphicsViewScaled = _menu->addAction(tr("Scale image"));
+	_scaleMenu = _menu->addMenu("Scale image");
+	_scaleMenu->setEnabled(false);
+	_graphicsViewScaled = _scaleMenu->addAction(tr("Fit in view"));
 	_graphicsViewScaled->setCheckable(true);
 	_graphicsViewScaled->setChecked(true);
-	_graphicsViewScaled->setEnabled(false);
+	_graphicsViewScaledToHeight = _scaleMenu->addAction(tr("Fit height"));
+	_graphicsViewScaledToHeight->setCheckable(true);
+	_graphicsViewScaledToHeight->setChecked(false);
+	_graphicsViewNoScaling = _scaleMenu->addAction(tr("No scale"));
+	_graphicsViewNoScaling->setCheckable(true);
+	_graphicsViewNoScaling->setChecked(false);
+	QActionGroup * group = new QActionGroup(this);
+	group->addAction(_graphicsViewScaled);
+	group->addAction(_graphicsViewScaledToHeight);
+	group->addAction(_graphicsViewNoScaling);
+	QMenu * colorMap = _menu->addMenu("Depth color map");
+	_colorMapWhiteToBlack = colorMap->addAction(tr("White to black"));
+	_colorMapWhiteToBlack->setCheckable(true);
+	_colorMapWhiteToBlack->setChecked(true);
+	_colorMapBlackToWhite = colorMap->addAction(tr("Black to white"));
+	_colorMapBlackToWhite->setCheckable(true);
+	_colorMapBlackToWhite->setChecked(false);
+	_colorMapRedToBlue = colorMap->addAction(tr("Red to blue"));
+	_colorMapRedToBlue->setCheckable(true);
+	_colorMapRedToBlue->setChecked(false);
+	_colorMapBlueToRed = colorMap->addAction(tr("Blue to red"));
+	_colorMapBlueToRed->setCheckable(true);
+	_colorMapBlueToRed->setChecked(false);
+	group = new QActionGroup(this);
+	group->addAction(_colorMapWhiteToBlack);
+	group->addAction(_colorMapBlackToWhite);
+	group->addAction(_colorMapRedToBlue);
+	group->addAction(_colorMapBlueToRed);
 	_setAlpha = _menu->addAction(tr("Set transparency..."));
 	_saveImage = _menu->addAction(tr("Save picture..."));
 	_saveImage->setEnabled(false);
@@ -216,6 +243,8 @@ void ImageView::saveSettings(QSettings & settings, const QString & group) const
 	settings.setValue("bg_color", this->getDefaultBackgroundColor());
 	settings.setValue("graphics_view", this->isGraphicsViewMode());
 	settings.setValue("graphics_view_scale", this->isGraphicsViewScaled());
+	settings.setValue("graphics_view_scale_to_height", this->isGraphicsViewScaledToHeight());
+	settings.setValue("colormap", _colorMapWhiteToBlack->isChecked()?0:_colorMapBlackToWhite->isChecked()?1:_colorMapRedToBlue->isChecked()?2:3);
 	if(!group.isEmpty())
 	{
 		settings.endGroup();
@@ -237,6 +266,12 @@ void ImageView::loadSettings(QSettings & settings, const QString & group)
 	this->setDefaultBackgroundColor(settings.value("bg_color", this->getDefaultBackgroundColor()).value<QColor>());
 	this->setGraphicsViewMode(settings.value("graphics_view", this->isGraphicsViewMode()).toBool());
 	this->setGraphicsViewScaled(settings.value("graphics_view_scale", this->isGraphicsViewScaled()).toBool());
+	this->setGraphicsViewScaledToHeight(settings.value("graphics_view_scale_to_height", this->isGraphicsViewScaledToHeight()).toBool());
+	int colorMap = settings.value("colormap", 0).toInt();
+	_colorMapWhiteToBlack->setChecked(colorMap==0);
+	_colorMapBlackToWhite->setChecked(colorMap==1);
+	_colorMapRedToBlue->setChecked(colorMap==2);
+	_colorMapBlueToRed->setChecked(colorMap==3);
 	if(!group.isEmpty())
 	{
 		settings.endGroup();
@@ -273,6 +308,11 @@ bool ImageView::isGraphicsViewScaled() const
 	return _graphicsViewScaled->isChecked();
 }
 
+bool ImageView::isGraphicsViewScaledToHeight() const
+{
+	return _graphicsViewScaledToHeight->isChecked();
+}
+
 const QColor & ImageView::getDefaultBackgroundColor() const
 {
 	return _defaultBgColor;
@@ -365,7 +405,7 @@ void ImageView::setGraphicsViewMode(bool on)
 {
 	_graphicsViewMode->setChecked(on);
 	_graphicsView->setVisible(on);
-	_graphicsViewScaled->setEnabled(on);
+	_scaleMenu->setEnabled(on);
 
 	if(on)
 	{
@@ -405,6 +445,12 @@ void ImageView::setGraphicsViewMode(bool on)
 		{
 			_graphicsView->fitInView(_graphicsView->sceneRect(), Qt::KeepAspectRatio);
 		}
+		else if(_graphicsViewScaledToHeight->isChecked())
+		{
+			QRectF rect = _graphicsView->sceneRect();
+			rect.setWidth(1);
+			_graphicsView->fitInView(rect, Qt::KeepAspectRatio);
+		}
 		else
 		{
 			_graphicsView->resetTransform();
@@ -435,6 +481,27 @@ void ImageView::setGraphicsViewScaled(bool scaled)
 	}
 }
 
+void ImageView::setGraphicsViewScaledToHeight(bool scaled)
+{
+	_graphicsViewScaledToHeight->setChecked(scaled);
+
+	if(scaled)
+	{
+		QRectF rect = _graphicsView->sceneRect();
+		rect.setWidth(1);
+		_graphicsView->fitInView(rect, Qt::KeepAspectRatio);
+	}
+	else
+	{
+		_graphicsView->resetTransform();
+	}
+
+	if(!_graphicsView->isVisible())
+	{
+		this->update();
+	}
+}
+
 void ImageView::setDefaultBackgroundColor(const QColor & color)
 {
 	_defaultBgColor = color;
@@ -569,9 +636,18 @@ void ImageView::paintEvent(QPaintEvent *event)
 void ImageView::resizeEvent(QResizeEvent* event)
 {
 	QWidget::resizeEvent(event);
-	if(_graphicsView->isVisible() && _graphicsViewScaled->isChecked())
+	if(_graphicsView->isVisible())
 	{
-		_graphicsView->fitInView(_graphicsView->sceneRect(), Qt::KeepAspectRatio);
+		if(_graphicsViewScaled->isChecked())
+		{
+			_graphicsView->fitInView(_graphicsView->sceneRect(), Qt::KeepAspectRatio);
+		}
+		else if(_graphicsViewScaledToHeight->isChecked())
+		{
+			QRectF rect = _graphicsView->sceneRect();
+			rect.setWidth(1);
+			_graphicsView->fitInView(rect, Qt::KeepAspectRatio);
+		}
 	}
 }
 
@@ -635,6 +711,17 @@ void ImageView::contextMenuEvent(QContextMenuEvent * e)
 		this->setGraphicsViewScaled(_graphicsViewScaled->isChecked());
 		Q_EMIT configChanged();
 	}
+	else if(action == _graphicsViewScaledToHeight || action == _graphicsViewNoScaling)
+	{
+		this->setGraphicsViewScaledToHeight(_graphicsViewScaledToHeight->isChecked());
+		Q_EMIT configChanged();
+	}
+	else if(action == _colorMapBlackToWhite || action == _colorMapWhiteToBlack || action == _colorMapRedToBlue || action == _colorMapBlueToRed)
+	{
+		if(!_imageDepthCv.empty())
+			this->setImageDepth(_imageDepthCv);
+		Q_EMIT configChanged();
+	}
 	else if(action == _setAlpha)
 	{
 		bool ok = false;
@@ -808,6 +895,25 @@ void ImageView::setImage(const QImage & image)
 	}
 }
 
+void ImageView::setImageDepth(const cv::Mat & imageDepth)
+{
+	_imageDepthCv = imageDepth;
+	uCvQtDepthColorMap colorMap = uCvQtDepthWhiteToBlack;
+	if(_colorMapBlackToWhite->isChecked())
+	{
+		colorMap = uCvQtDepthBlackToWhite;
+	}
+	else if(_colorMapRedToBlue->isChecked())
+	{
+		colorMap = uCvQtDepthRedToBlue;
+	}
+	else if(_colorMapBlueToRed->isChecked())
+	{
+		colorMap = uCvQtDepthBlueToRed;
+	}
+	setImageDepth(uCvMat2QImage(_imageDepthCv, true, colorMap));
+}
+
 void ImageView::setImageDepth(const QImage & imageDepth)
 {
 	_imageDepth = QPixmap::fromImage(imageDepth);
@@ -936,6 +1042,12 @@ void ImageView::setSceneRect(const QRectF & rect)
 	{
 		_graphicsView->fitInView(_graphicsView->sceneRect(), Qt::KeepAspectRatio);
 	}
+	else if(_graphicsViewScaledToHeight->isChecked())
+	{
+		QRectF rect = _graphicsView->sceneRect();
+		rect.setWidth(1);
+		_graphicsView->fitInView(rect, Qt::KeepAspectRatio);
+	}
 	else
 	{
 		_graphicsView->resetTransform();

guilib/src/KeypointItem.cpp

@@ -49,10 +49,7 @@ KeypointItem::KeypointItem(int id, const cv::KeyPoint & kpt, float depth, const
 
 KeypointItem::~KeypointItem()
 {
-	if(_placeHolder)
-	{
-		delete _placeHolder;
-	}
+	delete _placeHolder;
 }
 
 void KeypointItem::setColor(const QColor & color)

guilib/src/MainWindow.cpp

@@ -429,6 +429,7 @@ MainWindow::MainWindow(PreferencesDialog * prefDialog, QWidget * parent, bool sh
 	connect(_ui->actionKinect_for_Windows_SDK_v2, SIGNAL(triggered()), this, SLOT(selectK4W2()));
 	connect(_ui->actionRealSense_R200, SIGNAL(triggered()), this, SLOT(selectRealSense()));
 	connect(_ui->actionRealSense_ZR300, SIGNAL(triggered()), this, SLOT(selectRealSense()));
+	connect(_ui->actionRealSense2_D415, SIGNAL(triggered()), this, SLOT(selectRealSense2()));
 	connect(_ui->actionRealSense2_D435, SIGNAL(triggered()), this, SLOT(selectRealSense2()));
 	connect(_ui->actionStereoDC1394, SIGNAL(triggered()), this, SLOT(selectStereoDC1394()));
 	connect(_ui->actionStereoFlyCapture2, SIGNAL(triggered()), this, SLOT(selectStereoFlyCapture2()));
@@ -444,6 +445,7 @@ MainWindow::MainWindow(PreferencesDialog * prefDialog, QWidget * parent, bool sh
 	_ui->actionKinect_for_Windows_SDK_v2->setEnabled(CameraK4W2::available());
 	_ui->actionRealSense_R200->setEnabled(CameraRealSense::available());
 	_ui->actionRealSense_ZR300->setEnabled(CameraRealSense::available());
+	_ui->actionRealSense2_D415->setEnabled(CameraRealSense2::available());
 	_ui->actionRealSense2_D435->setEnabled(CameraRealSense2::available());
 	_ui->actionStereoDC1394->setEnabled(CameraStereoDC1394::available());
 	_ui->actionStereoFlyCapture2->setEnabled(CameraStereoFlyCapture2::available());
@@ -1352,7 +1354,7 @@ void MainWindow::processOdometry(const rtabmap::OdometryEvent & odom, bool dataI
 				_odomImageShow = _ui->imageView_odometry->isImageShown();
 				_odomImageDepthShow = _ui->imageView_odometry->isImageDepthShown();
 			}
-			_ui->imageView_odometry->setImageDepth(uCvMat2QImage(odom.data().imageRaw()));
+			_ui->imageView_odometry->setImageDepth(odom.data().imageRaw());
 			_ui->imageView_odometry->setImageShown(true);
 			_ui->imageView_odometry->setImageDepthShown(true);
 		}
@@ -1368,7 +1370,7 @@ void MainWindow::processOdometry(const rtabmap::OdometryEvent & odom, bool dataI
 			_ui->imageView_odometry->setImage(uCvMat2QImage(odom.data().imageRaw()));
 			if(_ui->imageView_odometry->isImageDepthShown() && !odom.data().depthOrRightRaw().empty())
 			{
-				_ui->imageView_odometry->setImageDepth(uCvMat2QImage(odom.data().depthOrRightRaw()));
+				_ui->imageView_odometry->setImageDepth(odom.data().depthOrRightRaw());
 			}
 
 			if( odom.info().type == (int)Odometry::kTypeF2M ||
@@ -1739,36 +1741,32 @@ void MainWindow::processStats(const rtabmap::Statistics & stat)
 			}
 			_refIds.push_back(stat.refImageId());
 			_loopClosureIds.push_back(matchId);
+			if(matchId > 0)
+			{
+				_cachedLocalizationsCount[matchId] += 1.0f;
+			}
 
 			//update image views
 			{
 				UCvMat2QImageThread qimageThread(signature.sensorData().imageRaw());
 				UCvMat2QImageThread qimageLoopThread(loopSignature.sensorData().imageRaw());
-				UCvMat2QImageThread qdepthThread(signature.sensorData().depthOrRightRaw());
-				UCvMat2QImageThread qdepthLoopThread(loopSignature.sensorData().depthOrRightRaw());
 				qimageThread.start();
-				qdepthThread.start();
 				qimageLoopThread.start();
-				qdepthLoopThread.start();
 				qimageThread.join();
-				qdepthThread.join();
 				qimageLoopThread.join();
-				qdepthLoopThread.join();
 				QImage img = qimageThread.getQImage();
 				QImage lcImg = qimageLoopThread.getQImage();
-				QImage depth = qdepthThread.getQImage();
-				QImage lcDepth = qdepthLoopThread.getQImage();
 				UDEBUG("time= %d ms", time.restart());
 
 				if(!img.isNull())
 				{
 					_ui->imageView_source->setImage(img);
 				}
-				if(!depth.isNull())
+				if(!signature.sensorData().depthOrRightRaw().empty())
 				{
-					_ui->imageView_source->setImageDepth(depth);
+					_ui->imageView_source->setImageDepth(signature.sensorData().depthOrRightRaw());
 				}
-				if(img.isNull() && depth.isNull())
+				if(img.isNull() && signature.sensorData().depthOrRightRaw().empty())
 				{
 					QRect sceneRect;
 					if(signature.sensorData().cameraModels().size())
@@ -1792,9 +1790,9 @@ void MainWindow::processStats(const rtabmap::Statistics & stat)
 				{
 					_ui->imageView_loopClosure->setImage(lcImg);
 				}
-				if(!lcDepth.isNull())
+				if(!loopSignature.sensorData().depthOrRightRaw().empty())
 				{
-					_ui->imageView_loopClosure->setImageDepth(lcDepth);
+					_ui->imageView_loopClosure->setImageDepth(loopSignature.sensorData().depthOrRightRaw());
 				}
 				if(_ui->imageView_loopClosure->sceneRect().isNull())
 				{
@@ -1956,11 +1954,18 @@ void MainWindow::processStats(const rtabmap::Statistics & stat)
 			{
 				_ui->graphicsView_graphView->updatePosterior(stat.posterior());
 			}
-			else if(_preferencesDialog->isWordsCountGraphView() &&
-					_preferencesDialog->isRGBDMode() &&
-					_cachedWordsCount.size())
+			else if(_preferencesDialog->isRGBDMode())
 			{
-				_ui->graphicsView_graphView->updatePosterior(_cachedWordsCount, (float)_preferencesDialog->getKpMaxFeatures());
+				if(_preferencesDialog->isWordsCountGraphView() &&
+						_cachedWordsCount.size())
+				{
+					_ui->graphicsView_graphView->updatePosterior(_cachedWordsCount, (float)_preferencesDialog->getKpMaxFeatures());
+				}
+				else if(_preferencesDialog->isLocalizationsCountGraphView() &&
+						_cachedLocalizationsCount.size())
+				{
+					_ui->graphicsView_graphView->updatePosterior(_cachedLocalizationsCount, 1.0f);
+				}
 			}
 			// update local path on the graph view
 			_ui->graphicsView_graphView->updateLocalPath(stat.localPath());
@@ -2776,6 +2781,57 @@ std::pair<pcl::PointCloud<pcl::PointXYZRGB>::Ptr, pcl::IndicesPtr> MainWindow::c
 		cv::Mat image, depth;
 		SensorData data = iter->sensorData();
 		data.uncompressData(&image, &depth, 0);
+		ParametersMap allParameters = _preferencesDialog->getAllParameters();
+		bool rectifyOnlyFeatures = Parameters::defaultRtabmapRectifyOnlyFeatures();
+		bool imagesAlreadyRectified = Parameters::defaultRtabmapImagesAlreadyRectified();
+		Parameters::parse(allParameters, Parameters::kRtabmapRectifyOnlyFeatures(), rectifyOnlyFeatures);
+		Parameters::parse(allParameters, Parameters::kRtabmapImagesAlreadyRectified(), imagesAlreadyRectified);
+		if(rectifyOnlyFeatures && !imagesAlreadyRectified)
+		{
+			if(data.cameraModels().size())
+			{
+				UTimer time;
+				// Note that only RGB image is rectified, the depth image is assumed to be already registered to rectified RGB camera.
+				UASSERT(int((data.imageRaw().cols/data.cameraModels().size())*data.cameraModels().size()) == data.imageRaw().cols);
+				int subImageWidth = data.imageRaw().cols/data.cameraModels().size();
+				cv::Mat rectifiedImages = data.imageRaw().clone();
+				bool initRectMaps = _rectCameraModels.empty();
+				if(initRectMaps)
+				{
+					_rectCameraModels.resize(data.cameraModels().size());
+				}
+				for(unsigned int i=0; i<data.cameraModels().size(); ++i)
+				{
+					if(data.cameraModels()[i].isValidForRectification())
+					{
+						if(initRectMaps)
+						{
+							_rectCameraModels[i] = data.cameraModels()[i];
+							if(!_rectCameraModels[i].isRectificationMapInitialized())
+							{
+								UWARN("Initializing rectification maps for camera %d (only done for the first image received)...", i);
+								_rectCameraModels[i].initRectificationMap();
+								UWARN("Initializing rectification maps for camera %d (only done for the first image received)... done!", i);
+							}
+						}
+						UASSERT(_rectCameraModels[i].imageWidth() == data.cameraModels()[i].imageWidth() &&
+								_rectCameraModels[i].imageHeight() == data.cameraModels()[i].imageHeight());
+						cv::Mat rectifiedImage = _rectCameraModels[i].rectifyImage(cv::Mat(data.imageRaw(), cv::Rect(subImageWidth*i, 0, subImageWidth, data.imageRaw().rows)));
+						rectifiedImage.copyTo(cv::Mat(rectifiedImages, cv::Rect(subImageWidth*i, 0, subImageWidth, data.imageRaw().rows)));
+					}
+					else
+					{
+						UWARN("Camera %d of data %d is not valid for rectification (%dx%d).",
+								i, data.id(),
+								data.cameraModels()[i].imageWidth(),
+								data.cameraModels()[i].imageHeight());
+					}
+				}
+				UINFO("Time rectification: %fs", time.ticks());
+				data.setImageRaw(rectifiedImages);
+				image = rectifiedImages;
+			}
+		}
 
 		pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud;
 		pcl::IndicesPtr indices(new std::vector<int>);
@@ -2787,7 +2843,7 @@ std::pair<pcl::PointCloud<pcl::PointXYZRGB>::Ptr, pcl::IndicesPtr> MainWindow::c
 				_preferencesDialog->getCloudMaxDepth(0),
 				_preferencesDialog->getCloudMinDepth(0),
 				indices.get(),
-				_preferencesDialog->getAllParameters(),
+				allParameters,
 				_preferencesDialog->getCloudRoiRatios(0));
 
 		// view point
@@ -4280,6 +4336,7 @@ void MainWindow::updateSelectSourceMenu()
 	_ui->actionKinect_for_Windows_SDK_v2->setChecked(_preferencesDialog->getSourceDriver() == PreferencesDialog::kSrcK4W2);
 	_ui->actionRealSense_R200->setChecked(_preferencesDialog->getSourceDriver() == PreferencesDialog::kSrcRealSense);
 	_ui->actionRealSense_ZR300->setChecked(_preferencesDialog->getSourceDriver() == PreferencesDialog::kSrcRealSense);
+	_ui->actionRealSense2_D415->setChecked(_preferencesDialog->getSourceDriver() == PreferencesDialog::kSrcRealSense2);
 	_ui->actionRealSense2_D435->setChecked(_preferencesDialog->getSourceDriver() == PreferencesDialog::kSrcRealSense2);
 	_ui->actionStereoDC1394->setChecked(_preferencesDialog->getSourceDriver() == PreferencesDialog::kSrcDC1394);
 	_ui->actionStereoFlyCapture2->setChecked(_preferencesDialog->getSourceDriver() == PreferencesDialog::kSrcFlyCapture2);
@@ -4500,7 +4557,7 @@ void MainWindow::openDatabase()
 	}
 }
 
-void MainWindow::openDatabase(const QString & path)
+void MainWindow::openDatabase(const QString & path, const ParametersMap & overridedParameters)
 {
 	if(_state != MainWindow::kIdle)
 	{
@@ -4536,6 +4593,8 @@ void MainWindow::openDatabase(const QString & path)
 					parameters.insert(*iter);
 				}
 
+				uInsert(parameters, overridedParameters);
+
 				ParametersMap currentParameters = _preferencesDialog->getAllParameters();
 				ParametersMap differentParameters;
 				for(ParametersMap::iterator iter=parameters.begin(); iter!=parameters.end(); ++iter)
@@ -6268,6 +6327,7 @@ void MainWindow::clearTheCache()
 	_ui->label_stats_loopClosuresRejected->setText("0");
 	_refIds.clear();
 	_loopClosureIds.clear();
+	_cachedLocalizationsCount.clear();
 	_ui->label_refId->clear();
 	_ui->label_matchId->clear();
 	_ui->graphicsView_graphView->clearAll();
@@ -6284,6 +6344,7 @@ void MainWindow::clearTheCache()
 	_octomap = new OctoMap(_preferencesDialog->getAllParameters());
 #endif
 	_occupancyGrid->clear();
+	_rectCameraModels.clear();
 }
 
 void MainWindow::openHelp()

guilib/src/OdometryViewer.cpp

@@ -418,7 +418,7 @@ void OdometryViewer::processData(const rtabmap::OdometryEvent & odom)
 				odomImageShow_ = imageView_->isImageShown();
 				odomImageDepthShow_ = imageView_->isImageDepthShown();
 			}
-			imageView_->setImageDepth(uCvMat2QImage(odom.data().imageRaw()));
+			imageView_->setImageDepth(odom.data().imageRaw());
 			imageView_->setImageShown(true);
 			imageView_->setImageDepthShown(true);
 		}
@@ -434,7 +434,7 @@ void OdometryViewer::processData(const rtabmap::OdometryEvent & odom)
 			imageView_->setImage(uCvMat2QImage(odom.data().imageRaw()));
 			if(imageView_->isImageDepthShown())
 			{
-				imageView_->setImageDepth(uCvMat2QImage(odom.data().depthOrRightRaw()));
+				imageView_->setImageDepth(odom.data().depthOrRightRaw());
 			}
 
 			if( odom.info().type == Odometry::kTypeF2M ||

guilib/src/PreferencesDialog.cpp

@@ -190,7 +190,7 @@ PreferencesDialog::PreferencesDialog(QWidget * parent) :
 		_ui->reextract_type->setItemData(0, 0, Qt::UserRole - 1);
 		_ui->reextract_type->setItemData(1, 0, Qt::UserRole - 1);
 
-#if CV_MAJOR_VERSION == 3
+#if CV_MAJOR_VERSION >= 3
 		_ui->comboBox_detector_strategy->setItemData(0, 0, Qt::UserRole - 1);
 		_ui->comboBox_detector_strategy->setItemData(1, 0, Qt::UserRole - 1);
 		_ui->comboBox_detector_strategy->setItemData(3, 0, Qt::UserRole - 1);
@@ -354,6 +354,7 @@ PreferencesDialog::PreferencesDialog(QWidget * parent) :
 	connect(_ui->checkBox_odom_onlyInliersShown, SIGNAL(stateChanged(int)), this, SLOT(makeObsoleteGeneralPanel()));
 	connect(_ui->radioButton_posteriorGraphView, SIGNAL(toggled(bool)), this, SLOT(makeObsoleteGeneralPanel()));
 	connect(_ui->radioButton_wordsGraphView, SIGNAL(toggled(bool)), this, SLOT(makeObsoleteGeneralPanel()));
+	connect(_ui->radioButton_localizationsGraphView, SIGNAL(toggled(bool)), this, SLOT(makeObsoleteGeneralPanel()));
 	connect(_ui->radioButton_nochangeGraphView, SIGNAL(toggled(bool)), this, SLOT(makeObsoleteGeneralPanel()));
 	connect(_ui->checkbox_odomDisabled, SIGNAL(stateChanged(int)), this, SLOT(makeObsoleteGeneralPanel()));
 	connect(_ui->odom_registration, SIGNAL(currentIndexChanged(int)), this, SLOT(makeObsoleteGeneralPanel()));
@@ -573,6 +574,7 @@ PreferencesDialog::PreferencesDialog(QWidget * parent) :
 	connect(_ui->comboBox_realsensePresetDepth, SIGNAL(currentIndexChanged(int)), this, SLOT(makeObsoleteSourcePanel()));
 	connect(_ui->checkbox_realsenseOdom, SIGNAL(stateChanged(int)), this, SLOT(makeObsoleteSourcePanel()));
 	connect(_ui->checkbox_realsenseDepthScaledToRGBSize, SIGNAL(stateChanged(int)), this, SLOT(makeObsoleteSourcePanel()));
+	connect(_ui->comboBox_realsenseRGBSource, SIGNAL(currentIndexChanged(int)), this, SLOT(makeObsoleteSourcePanel()));
 	connect(_ui->checkbox_rs2_emitter, SIGNAL(stateChanged(int)), this, SLOT(makeObsoleteSourcePanel()));
 	connect(_ui->checkbox_rs2_irDepth, SIGNAL(stateChanged(int)), this, SLOT(makeObsoleteSourcePanel()));
 
@@ -702,6 +704,7 @@ PreferencesDialog::PreferencesDialog(QWidget * parent) :
 	_ui->general_checkBox_startNewMapOnLoopClosure->setObjectName(Parameters::kRtabmapStartNewMapOnLoopClosure().c_str());
 	_ui->general_checkBox_startNewMapOnGoodSignature->setObjectName(Parameters::kRtabmapStartNewMapOnGoodSignature().c_str());
 	_ui->general_checkBox_imagesAlreadyRectified->setObjectName(Parameters::kRtabmapImagesAlreadyRectified().c_str());
+	_ui->general_checkBox_rectifyOnlyFeatures->setObjectName(Parameters::kRtabmapRectifyOnlyFeatures().c_str());
 	_ui->lineEdit_workingDirectory->setObjectName(Parameters::kRtabmapWorkingDirectory().c_str());
 	connect(_ui->toolButton_workingDirectory, SIGNAL(clicked()), this, SLOT(changeWorkingDirectory()));
 
@@ -732,6 +735,7 @@ PreferencesDialog::PreferencesDialog(QWidget * parent) :
 	_ui->general_spinBox_laserScanNormalK->setObjectName(Parameters::kMemLaserScanNormalK().c_str());
 	_ui->general_doubleSpinBox_laserScanNormalRadius->setObjectName(Parameters::kMemLaserScanNormalRadius().c_str());
 	_ui->checkBox_useOdomFeatures->setObjectName(Parameters::kMemUseOdomFeatures().c_str());
+	_ui->memCovOffDiagIgnored->setObjectName(Parameters::kMemCovOffDiagIgnored().c_str());
 
 	// Database
 	_ui->checkBox_dbInMemory->setObjectName(Parameters::kDbSqlite3InMemory().c_str());
@@ -992,7 +996,11 @@ PreferencesDialog::PreferencesDialog(QWidget * parent) :
 	_ui->doubleSpinBox_grid_minMapSize->setObjectName(Parameters::kGridGlobalMinSize().c_str());
 	_ui->spinBox_grid_maxNodes->setObjectName(Parameters::kGridGlobalMaxNodes().c_str());
 	_ui->doubleSpinBox_grid_footprintRadius->setObjectName(Parameters::kGridGlobalFootprintRadius().c_str());
-	_ui->doubleSpinBox_grid_octomapOccThr->setObjectName(Parameters::kGridGlobalOctoMapOccupancyThr().c_str());
+	_ui->doubleSpinBox_grid_occThr->setObjectName(Parameters::kGridGlobalOccupancyThr().c_str());
+	_ui->doubleSpinBox_grid_probHit->setObjectName(Parameters::kGridGlobalProbHit().c_str());
+	_ui->doubleSpinBox_grid_probMiss->setObjectName(Parameters::kGridGlobalProbMiss().c_str());
+	_ui->doubleSpinBox_grid_clampingMin->setObjectName(Parameters::kGridGlobalProbClampingMin().c_str());
+	_ui->doubleSpinBox_grid_clampingMax->setObjectName(Parameters::kGridGlobalProbClampingMax().c_str());
 	_ui->checkBox_grid_erode->setObjectName(Parameters::kGridGlobalEroded().c_str());
 
 	//Odometry
@@ -1488,6 +1496,7 @@ void PreferencesDialog::resetSettings(QGroupBox * groupBox)
 		_ui->checkBox_odom_onlyInliersShown->setChecked(false);
 		_ui->radioButton_posteriorGraphView->setChecked(true);
 		_ui->radioButton_wordsGraphView->setChecked(false);
+		_ui->radioButton_localizationsGraphView->setChecked(false);
 		_ui->radioButton_nochangeGraphView->setChecked(false);
 		_ui->checkbox_odomDisabled->setChecked(false);
 		_ui->checkbox_groundTruthAlign->setChecked(true);
@@ -1654,6 +1663,7 @@ void PreferencesDialog::resetSettings(QGroupBox * groupBox)
 		_ui->comboBox_realsensePresetDepth->setCurrentIndex(2);
 		_ui->checkbox_realsenseOdom->setChecked(false);
 		_ui->checkbox_realsenseDepthScaledToRGBSize->setChecked(false);
+		_ui->comboBox_realsenseRGBSource->setCurrentIndex(0);
 		_ui->checkbox_rs2_emitter->setChecked(true);
 		_ui->checkbox_rs2_irDepth->setChecked(false);
 		_ui->lineEdit_openniOniPath->clear();
@@ -1906,6 +1916,7 @@ void PreferencesDialog::readGuiSettings(const QString & filePath)
 	_ui->checkBox_odom_onlyInliersShown->setChecked(settings.value("odomOnlyInliersShown", _ui->checkBox_odom_onlyInliersShown->isChecked()).toBool());
 	_ui->radioButton_posteriorGraphView->setChecked(settings.value("posteriorGraphView", _ui->radioButton_posteriorGraphView->isChecked()).toBool());
 	_ui->radioButton_wordsGraphView->setChecked(settings.value("wordsGraphView", _ui->radioButton_wordsGraphView->isChecked()).toBool());
+	_ui->radioButton_localizationsGraphView->setChecked(settings.value("localizationsGraphView", _ui->radioButton_localizationsGraphView->isChecked()).toBool());
 	_ui->radioButton_nochangeGraphView->setChecked(settings.value("nochangeGraphView", _ui->radioButton_nochangeGraphView->isChecked()).toBool());
 	_ui->checkbox_odomDisabled->setChecked(settings.value("odomDisabled", _ui->checkbox_odomDisabled->isChecked()).toBool());
 	_ui->odom_registration->setCurrentIndex(settings.value("odomRegistration", _ui->odom_registration->currentIndex()).toInt());
@@ -2052,6 +2063,7 @@ void PreferencesDialog::readCameraSettings(const QString & filePath)
 	_ui->comboBox_realsensePresetDepth->setCurrentIndex(settings.value("presetDepth", _ui->comboBox_realsensePresetDepth->currentIndex()).toInt());
 	_ui->checkbox_realsenseOdom->setChecked(settings.value("odom", _ui->checkbox_realsenseOdom->isChecked()).toBool());
 	_ui->checkbox_realsenseDepthScaledToRGBSize->setChecked(settings.value("depthScaled", _ui->checkbox_realsenseDepthScaledToRGBSize->isChecked()).toBool());
+	_ui->comboBox_realsenseRGBSource->setCurrentIndex(settings.value("rgbSource", _ui->comboBox_realsenseRGBSource->currentIndex()).toInt());
 	settings.endGroup(); // RealSense
 
 	settings.beginGroup("RealSense2");
@@ -2326,6 +2338,7 @@ void PreferencesDialog::writeGuiSettings(const QString & filePath) const
 	settings.setValue("odomOnlyInliersShown", _ui->checkBox_odom_onlyInliersShown->isChecked());
 	settings.setValue("posteriorGraphView",   _ui->radioButton_posteriorGraphView->isChecked());
 	settings.setValue("wordsGraphView",       _ui->radioButton_wordsGraphView->isChecked());
+	settings.setValue("localizationsGraphView", _ui->radioButton_localizationsGraphView->isChecked());
 	settings.setValue("nochangeGraphView",    _ui->radioButton_nochangeGraphView->isChecked());
 	settings.setValue("odomDisabled",         _ui->checkbox_odomDisabled->isChecked());
 	settings.setValue("odomRegistration",     _ui->odom_registration->currentIndex());
@@ -2474,6 +2487,7 @@ void PreferencesDialog::writeCameraSettings(const QString & filePath) const
 	settings.setValue("presetDepth",         _ui->comboBox_realsensePresetDepth->currentIndex());
 	settings.setValue("odom",                _ui->checkbox_realsenseOdom->isChecked());
 	settings.setValue("depthScaled",         _ui->checkbox_realsenseDepthScaledToRGBSize->isChecked());
+	settings.setValue("rgbSource",           _ui->comboBox_realsenseRGBSource->currentIndex());
 	settings.endGroup(); // RealSense
 
 	settings.beginGroup("RealSense2");
@@ -4449,6 +4463,10 @@ bool PreferencesDialog::isWordsCountGraphView() const
 {
 	return _ui->radioButton_wordsGraphView->isChecked();
 }
+bool PreferencesDialog::isLocalizationsCountGraphView() const
+{
+	return _ui->radioButton_localizationsGraphView->isChecked();
+}
 bool PreferencesDialog::isOdomDisabled() const
 {
 	return _ui->checkbox_odomDisabled->isChecked();
@@ -5007,10 +5025,10 @@ Camera * PreferencesDialog::createCamera(bool useRawImages, bool useColor)
 	}
 	else if (driver == kSrcRealSense)
 	{
-		if(useRawImages)
+		if(useRawImages && _ui->comboBox_realsenseRGBSource->currentIndex()!=2)
 		{
 			QMessageBox::warning(this, tr("Calibration"),
-					tr("Using raw images for \"RealSense\" driver is not yet supported. "
+					tr("Using raw images for \"RealSense\" driver is not yet supported for color and infrared streams. "
 						"Factory calibration loaded from RealSense is used."), QMessageBox::Ok);
 			return 0;
 		}
@@ -5024,6 +5042,7 @@ Camera * PreferencesDialog::createCamera(bool useRawImages, bool useColor)
 				this->getGeneralInputRate(),
 				this->getSourceLocalTransform());
 			((CameraRealSense*)camera)->setDepthScaledToRGBSize(_ui->checkbox_realsenseDepthScaledToRGBSize->isChecked());
+			((CameraRealSense*)camera)->setRGBSource((CameraRealSense::RGBSource)_ui->comboBox_realsenseRGBSource->currentIndex());
 		}
 	}
 	else if (driver == kSrcRealSense2)
@@ -5810,7 +5829,7 @@ void PreferencesDialog::calibrate()
 		}
 
 		bool freenect2 = driver == kSrcFreenect2;
-		_calibrationDialog->setStereoMode(this->getSourceType() != kSrcRGB, freenect2?"rgb":"left", freenect2?"depth":"right"); // RGB+Depth or left+right
+		_calibrationDialog->setStereoMode(this->getSourceType() != kSrcRGB && driver != kSrcRealSense, freenect2?"rgb":"left", freenect2?"depth":"right"); // RGB+Depth or left+right
 		_calibrationDialog->setSwitchedImages(freenect2);
 		_calibrationDialog->setSavingDirectory(this->getCameraInfoDir());
 		_calibrationDialog->registerToEventsManager();

guilib/src/ui/DatabaseViewer.ui

@@ -61,8 +61,8 @@
           <rect>
            <x>0</x>
            <y>0</y>
-           <width>398</width>
-           <height>242</height>
+           <width>327</width>
+           <height>222</height>
           </rect>
          </property>
          <layout class="QGridLayout" name="gridLayout" columnstretch="0,1">
@@ -253,8 +253,8 @@
           <rect>
            <x>0</x>
            <y>0</y>
-           <width>397</width>
-           <height>242</height>
+           <width>327</width>
+           <height>222</height>
           </rect>
          </property>
          <layout class="QGridLayout" name="gridLayout_2" columnstretch="0,1">
@@ -729,6 +729,27 @@
      </item>
      <item>
       <layout class="QGridLayout" name="gridLayout_6" columnstretch="0,0">
+       <item row="4" column="1">
+        <layout class="QHBoxLayout" name="horizontalLayout_13">
+         <item>
+          <widget class="QLabel" name="label_variance">
+           <property name="text">
+            <string/>
+           </property>
+           <property name="textInteractionFlags">
+            <set>Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse</set>
+           </property>
+          </widget>
+         </item>
+         <item>
+          <widget class="QLineEdit" name="lineEdit_covariance">
+           <property name="readOnly">
+            <bool>true</bool>
+           </property>
+          </widget>
+         </item>
+        </layout>
+       </item>
        <item row="0" column="0">
         <widget class="QLabel" name="label">
          <property name="text">
@@ -803,31 +824,14 @@
          </property>
         </widget>
        </item>
-       <item row="4" column="0">
-        <widget class="QLabel" name="label_18">
-         <property name="text">
-          <string>σ (rot, trans)</string>
-         </property>
-        </widget>
-       </item>
-       <item row="4" column="1">
-        <widget class="QLabel" name="label_variance">
-         <property name="text">
-          <string/>
-         </property>
-         <property name="textInteractionFlags">
-          <set>Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse</set>
-         </property>
-        </widget>
-       </item>
-       <item row="5" column="0">
+       <item row="6" column="0">
         <widget class="QCheckBox" name="checkBox_showOptimized">
          <property name="text">
           <string>Optimized</string>
          </property>
         </widget>
        </item>
-       <item row="5" column="1">
+       <item row="6" column="1">
         <widget class="QLabel" name="label_constraint_opt">
          <property name="text">
           <string/>
@@ -837,6 +841,13 @@
          </property>
         </widget>
        </item>
+       <item row="4" column="0">
+        <widget class="QLabel" name="label_18">
+         <property name="text">
+          <string>σ (lin, ang), cov</string>
+         </property>
+        </widget>
+       </item>
       </layout>
      </item>
      <item>
@@ -1210,15 +1221,15 @@
      <item>
       <widget class="QToolBox" name="toolBox">
        <property name="currentIndex">
-        <number>0</number>
+        <number>1</number>
        </property>
        <widget class="QWidget" name="page_3">
         <property name="geometry">
          <rect>
           <x>0</x>
           <y>0</y>
-          <width>318</width>
-          <height>219</height>
+          <width>312</width>
+          <height>200</height>
          </rect>
         </property>
         <attribute name="label">
@@ -1378,9 +1389,9 @@
         <property name="geometry">
          <rect>
           <x>0</x>
-          <y>0</y>
-          <width>282</width>
-          <height>845</height>
+          <y>-167</y>
+          <width>441</width>
+          <height>764</height>
          </rect>
         </property>
         <attribute name="label">
@@ -1428,7 +1439,7 @@
              </property>
             </widget>
            </item>
-           <item row="11" column="1">
+           <item row="12" column="1">
             <widget class="QLabel" name="label_54">
              <property name="text">
               <string>Voxel size (for clouds and scans)</string>
@@ -1455,7 +1466,7 @@
              </property>
             </widget>
            </item>
-           <item row="11" column="0">
+           <item row="12" column="0">
             <widget class="QDoubleSpinBox" name="doubleSpinBox_voxelSize">
              <property name="suffix">
               <string> m</string>
@@ -1513,6 +1524,25 @@
              </property>
             </widget>
            </item>
+           <item row="7" column="0">
+            <widget class="QComboBox" name="comboBox_octomap_rendering_type">
+             <item>
+              <property name="text">
+               <string>Point Cloud</string>
+              </property>
+             </item>
+             <item>
+              <property name="text">
+               <string>Cube</string>
+              </property>
+             </item>
+             <item>
+              <property name="text">
+               <string>Volume</string>
+              </property>
+             </item>
+            </widget>
+           </item>
            <item row="8" column="1">
             <widget class="QLabel" name="label_octomap_depth">
              <property name="text">
@@ -1533,25 +1563,6 @@
              </property>
             </widget>
            </item>
-           <item row="7" column="0">
-            <widget class="QComboBox" name="comboBox_octomap_rendering_type">
-             <item>
-              <property name="text">
-               <string>Point Cloud</string>
-              </property>
-             </item>
-             <item>
-              <property name="text">
-               <string>Cube</string>
-              </property>
-             </item>
-             <item>
-              <property name="text">
-               <string>Volume</string>
-              </property>
-             </item>
-            </widget>
-           </item>
            <item row="0" column="1">
             <widget class="QLabel" name="label_octomap_empty">
              <property name="text">
@@ -1646,6 +1657,32 @@
              </item>
             </layout>
            </item>
+           <item row="11" column="1">
+            <widget class="QLabel" name="label_56">
+             <property name="text">
+              <string>Decimation (for images)</string>
+             </property>
+             <property name="wordWrap">
+              <bool>true</bool>
+             </property>
+            </widget>
+           </item>
+           <item row="11" column="0">
+            <widget class="QSpinBox" name="spinBox_decimation">
+             <property name="prefix">
+              <string/>
+             </property>
+             <property name="minimum">
+              <number>1</number>
+             </property>
+             <property name="maximum">
+              <number>16</number>
+             </property>
+             <property name="value">
+              <number>16</number>
+             </property>
+            </widget>
+           </item>
           </layout>
          </item>
          <item>
@@ -1887,8 +1924,8 @@
          <rect>
           <x>0</x>
           <y>0</y>
-          <width>205</width>
-          <height>117</height>
+          <width>200</width>
+          <height>111</height>
          </rect>
         </property>
         <attribute name="label">
@@ -1987,8 +2024,8 @@
          <rect>
           <x>0</x>
           <y>0</y>
-          <width>185</width>
-          <height>487</height>
+          <width>179</width>
+          <height>452</height>
          </rect>
         </property>
         <attribute name="label">

guilib/src/ui/calibrationDialog.ui

@@ -145,7 +145,7 @@
          <rect>
           <x>0</x>
           <y>0</y>
-          <width>353</width>
+          <width>379</width>
           <height>816</height>
          </rect>
         </property>
@@ -172,7 +172,7 @@
             <item row="3" column="0">
              <widget class="QLabel" name="label_15">
               <property name="toolTip">
-               <string>Number of inner squares on the board</string>
+               <string/>
               </property>
               <property name="text">
                <string>Square size</string>
@@ -228,7 +228,7 @@
             <item row="0" column="0">
              <widget class="QLabel" name="label_21">
               <property name="toolTip">
-               <string>Number of inner squares on the board</string>
+               <string/>
               </property>
               <property name="text">
                <string>Download</string>
@@ -264,7 +264,7 @@
             <item row="4" column="0">
              <widget class="QLabel" name="label_22">
               <property name="toolTip">
-               <string>Number of inner squares on the board</string>
+               <string>If &gt; 1, image is scaled down to help board detection on high definition images</string>
               </property>
               <property name="text">
                <string>Max scale</string>
@@ -281,7 +281,14 @@
            </property>
            <layout class="QVBoxLayout" name="verticalLayout_6">
             <item>
-             <layout class="QGridLayout" name="gridLayout" columnstretch="0,1,1">
+             <layout class="QGridLayout" name="gridLayout" columnstretch="0,0,0">
+              <item row="5" column="0">
+               <widget class="QLabel" name="label_4">
+                <property name="text">
+                 <string>Skew</string>
+                </property>
+               </widget>
+              </item>
               <item row="0" column="0">
                <widget class="QLabel" name="label_19">
                 <property name="text">
@@ -417,13 +424,6 @@
                 </property>
                </widget>
               </item>
-              <item row="5" column="0">
-               <widget class="QLabel" name="label_4">
-                <property name="text">
-                 <string>Skew</string>
-                </property>
-               </widget>
-              </item>
               <item row="5" column="1">
                <widget class="QProgressBar" name="progressBar_skew">
                 <property name="value">
@@ -444,7 +444,7 @@
              </layout>
             </item>
             <item>
-             <layout class="QHBoxLayout" name="horizontalLayout_5" stretch="1,0">
+             <layout class="QHBoxLayout" name="horizontalLayout_5" stretch="1,0,0">
               <item>
                <widget class="QPushButton" name="pushButton_calibrate">
                 <property name="text">
@@ -452,6 +452,13 @@
                 </property>
                </widget>
               </item>
+              <item>
+               <widget class="QCheckBox" name="checkBox_fisheye">
+                <property name="text">
+                 <string>Fish eye</string>
+                </property>
+               </widget>
+              </item>
               <item>
                <widget class="QCheckBox" name="checkBox_unlock">
                 <property name="text">
@@ -467,6 +474,13 @@
                <string>Camera intrinsic parameters</string>
               </property>
               <layout class="QGridLayout" name="gridLayout_2">
+               <item row="8" column="2">
+                <widget class="QLineEdit" name="lineEdit_P_2">
+                 <property name="readOnly">
+                  <bool>true</bool>
+                 </property>
+                </widget>
+               </item>
                <item row="0" column="1">
                 <widget class="QLabel" name="label_fx">
                  <property name="text">
@@ -681,13 +695,6 @@
                  </property>
                 </widget>
                </item>
-               <item row="8" column="2">
-                <widget class="QLineEdit" name="lineEdit_P_2">
-                 <property name="readOnly">
-                  <bool>true</bool>
-                 </property>
-                </widget>
-               </item>
                <item row="4" column="0">
                 <widget class="QLabel" name="label_baseline_name">
                  <property name="text">

guilib/src/ui/exportCloudsDialog.ui

@@ -23,9 +23,9 @@
       <property name="geometry">
        <rect>
         <x>0</x>
-        <y>0</y>
-        <width>778</width>
-        <height>4697</height>
+        <y>-1112</y>
+        <width>773</width>
+        <height>4339</height>
        </rect>
       </property>
       <layout class="QVBoxLayout" name="verticalLayout_13">
@@ -890,12 +890,29 @@ Guidelines: 4 times the voxel size, 0.025 for voxel=0.</string>
               </property>
              </widget>
             </item>
+            <item row="3" column="1">
+             <widget class="QLabel" name="label_90">
+              <property name="text">
+               <string>Output voxel size. Used to filter superposed points after upsampling.</string>
+              </property>
+              <property name="wordWrap">
+               <bool>true</bool>
+              </property>
+             </widget>
+            </item>
+            <item row="3" column="0">
+             <widget class="QDoubleSpinBox" name="doubleSpinBox_mls_outputVoxelSize">
+              <property name="decimals">
+               <number>3</number>
+              </property>
+             </widget>
+            </item>
            </layout>
           </item>
           <item>
            <widget class="QStackedWidget" name="stackedWidget_upsampling">
             <property name="currentIndex">
-             <number>0</number>
+             <number>4</number>
             </property>
             <widget class="QWidget" name="page">
              <layout class="QVBoxLayout" name="verticalLayout_3">
@@ -1000,7 +1017,11 @@ Guidelines: 4 times the voxel size, 0.025 for voxel=0.</string>
                     </widget>
                    </item>
                    <item row="0" column="0">
-                    <widget class="QDoubleSpinBox" name="doubleSpinBox_sampleRadius"/>
+                    <widget class="QDoubleSpinBox" name="doubleSpinBox_sampleRadius">
+                     <property name="decimals">
+                      <number>3</number>
+                     </property>
+                    </widget>
                    </item>
                    <item row="0" column="1">
                     <widget class="QLabel" name="label_7">
@@ -1013,7 +1034,11 @@ Guidelines: 4 times the voxel size, 0.025 for voxel=0.</string>
                     </widget>
                    </item>
                    <item row="1" column="0">
-                    <widget class="QDoubleSpinBox" name="doubleSpinBox_sampleStep"/>
+                    <widget class="QDoubleSpinBox" name="doubleSpinBox_sampleStep">
+                     <property name="decimals">
+                      <number>3</number>
+                     </property>
+                    </widget>
                    </item>
                   </layout>
                  </item>
@@ -1119,7 +1144,11 @@ Guidelines: 4 times the voxel size, 0.025 for voxel=0.</string>
                     </widget>
                    </item>
                    <item row="0" column="0">
-                    <widget class="QDoubleSpinBox" name="doubleSpinBox_dilationVoxelSize"/>
+                    <widget class="QDoubleSpinBox" name="doubleSpinBox_dilationVoxelSize">
+                     <property name="decimals">
+                      <number>3</number>
+                     </property>
+                    </widget>
                    </item>
                    <item row="1" column="1">
                     <widget class="QLabel" name="label_11">

guilib/src/ui/mainWindow.ui

@@ -197,12 +197,23 @@
        </property>
        <addaction name="actionRealSense2_D435"/>
       </widget>
+      <widget class="QMenu" name="menuRealSense_D415">
+       <property name="title">
+        <string>RealSense D415</string>
+       </property>
+       <property name="icon">
+        <iconset resource="../GuiLib.qrc">
+         <normaloff>:/images/d415.png</normaloff>:/images/d415.png</iconset>
+       </property>
+       <addaction name="actionRealSense2_D415"/>
+      </widget>
       <addaction name="menuKinect_for_Xbox_360"/>
       <addaction name="menuXtion_PRO_LIVE"/>
       <addaction name="menuSense_3D_scanner"/>
       <addaction name="menuKinect_v2"/>
       <addaction name="menuRealSense_R200"/>
       <addaction name="menuRealSense_ZR300"/>
+      <addaction name="menuRealSense_D415"/>
       <addaction name="menuRealSense_D435"/>
      </widget>
      <widget class="QMenu" name="menuStereo_camera">
@@ -331,16 +342,7 @@
      <property name="spacing">
       <number>0</number>
      </property>
-     <property name="leftMargin">
-      <number>0</number>
-     </property>
-     <property name="topMargin">
-      <number>0</number>
-     </property>
-     <property name="rightMargin">
-      <number>0</number>
-     </property>
-     <property name="bottomMargin">
+     <property name="margin">
       <number>0</number>
      </property>
      <item>
@@ -378,16 +380,7 @@
      <property name="spacing">
       <number>0</number>
      </property>
-     <property name="leftMargin">
-      <number>0</number>
-     </property>
-     <property name="topMargin">
-      <number>0</number>
-     </property>
-     <property name="rightMargin">
-      <number>0</number>
-     </property>
-     <property name="bottomMargin">
+     <property name="margin">
       <number>0</number>
      </property>
      <item>
@@ -630,16 +623,7 @@
      <property name="spacing">
       <number>0</number>
      </property>
-     <property name="leftMargin">
-      <number>0</number>
-     </property>
-     <property name="topMargin">
-      <number>0</number>
-     </property>
-     <property name="rightMargin">
-      <number>0</number>
-     </property>
-     <property name="bottomMargin">
+     <property name="margin">
       <number>0</number>
      </property>
      <item>
@@ -660,16 +644,7 @@
      <property name="spacing">
       <number>0</number>
      </property>
-     <property name="leftMargin">
-      <number>0</number>
-     </property>
-     <property name="topMargin">
-      <number>0</number>
-     </property>
-     <property name="rightMargin">
-      <number>0</number>
-     </property>
-     <property name="bottomMargin">
+     <property name="margin">
       <number>0</number>
      </property>
      <item>
@@ -690,16 +665,7 @@
      <property name="spacing">
       <number>0</number>
      </property>
-     <property name="leftMargin">
-      <number>0</number>
-     </property>
-     <property name="topMargin">
-      <number>0</number>
-     </property>
-     <property name="rightMargin">
-      <number>0</number>
-     </property>
-     <property name="bottomMargin">
+     <property name="margin">
       <number>0</number>
      </property>
      <item>
@@ -720,16 +686,7 @@
      <property name="spacing">
       <number>0</number>
      </property>
-     <property name="leftMargin">
-      <number>0</number>
-     </property>
-     <property name="topMargin">
-      <number>0</number>
-     </property>
-     <property name="rightMargin">
-      <number>0</number>
-     </property>
-     <property name="bottomMargin">
+     <property name="margin">
       <number>0</number>
      </property>
     </layout>
@@ -747,16 +704,7 @@
      <property name="spacing">
       <number>0</number>
      </property>
-     <property name="leftMargin">
-      <number>0</number>
-     </property>
-     <property name="topMargin">
-      <number>0</number>
-     </property>
-     <property name="rightMargin">
-      <number>0</number>
-     </property>
-     <property name="bottomMargin">
+     <property name="margin">
       <number>0</number>
      </property>
     </layout>
@@ -777,16 +725,7 @@
      <property name="spacing">
       <number>0</number>
      </property>
-     <property name="leftMargin">
-      <number>0</number>
-     </property>
-     <property name="topMargin">
-      <number>0</number>
-     </property>
-     <property name="rightMargin">
-      <number>0</number>
-     </property>
-     <property name="bottomMargin">
+     <property name="margin">
       <number>0</number>
      </property>
      <item>
@@ -807,16 +746,7 @@
      <property name="spacing">
       <number>0</number>
      </property>
-     <property name="leftMargin">
-      <number>0</number>
-     </property>
-     <property name="topMargin">
-      <number>0</number>
-     </property>
-     <property name="rightMargin">
-      <number>0</number>
-     </property>
-     <property name="bottomMargin">
+     <property name="margin">
       <number>0</number>
      </property>
      <item>
@@ -840,16 +770,7 @@
      <property name="spacing">
       <number>0</number>
      </property>
-     <property name="leftMargin">
-      <number>0</number>
-     </property>
-     <property name="topMargin">
-      <number>0</number>
-     </property>
-     <property name="rightMargin">
-      <number>0</number>
-     </property>
-     <property name="bottomMargin">
+     <property name="margin">
       <number>0</number>
      </property>
      <item>
@@ -919,16 +840,7 @@
      <property name="spacing">
       <number>0</number>
      </property>
-     <property name="leftMargin">
-      <number>0</number>
-     </property>
-     <property name="topMargin">
-      <number>0</number>
-     </property>
-     <property name="rightMargin">
-      <number>0</number>
-     </property>
-     <property name="bottomMargin">
+     <property name="margin">
       <number>0</number>
      </property>
      <item>
@@ -1485,6 +1397,14 @@
     <string>RealSense2</string>
    </property>
   </action>
+  <action name="actionRealSense2_D415">
+   <property name="checkable">
+    <bool>true</bool>
+   </property>
+   <property name="text">
+    <string>RealSense2</string>
+   </property>
+  </action>
  </widget>
  <customwidgets>
   <customwidget>

guilib/src/ui/preferencesDialog.ui

@@ -94,25 +94,16 @@
           <property name="geometry">
            <rect>
             <x>0</x>
-            <y>-1144</y>
+            <y>-287</y>
             <width>681</width>
-            <height>2811</height>
+            <height>2943</height>
            </rect>
           </property>
           <layout class="QVBoxLayout" name="verticalLayout_16">
            <property name="spacing">
             <number>0</number>
            </property>
-           <property name="leftMargin">
-            <number>0</number>
-           </property>
-           <property name="topMargin">
-            <number>0</number>
-           </property>
-           <property name="rightMargin">
-            <number>0</number>
-           </property>
-           <property name="bottomMargin">
+           <property name="margin">
             <number>0</number>
            </property>
            <item>
@@ -126,7 +117,7 @@
                 <enum>QFrame::Raised</enum>
                </property>
                <property name="currentIndex">
-                <number>18</number>
+                <number>5</number>
                </property>
                <widget class="QWidget" name="page_22">
                 <layout class="QVBoxLayout" name="verticalLayout_29" stretch="0,1">
@@ -309,6 +300,49 @@
                       </item>
                      </layout>
                     </item>
+                    <item>
+                     <widget class="QGroupBox" name="groupBox_2">
+                      <property name="title">
+                       <string>Save/Load Settings</string>
+                      </property>
+                      <layout class="QHBoxLayout" name="horizontalLayout_9">
+                       <item>
+                        <widget class="QPushButton" name="pushButton_loadConfig">
+                         <property name="text">
+                          <string>Load settings (*.ini) ...</string>
+                         </property>
+                        </widget>
+                       </item>
+                       <item>
+                        <widget class="QPushButton" name="pushButton_saveConfig">
+                         <property name="text">
+                          <string>Save settings (*.ini) ...</string>
+                         </property>
+                        </widget>
+                       </item>
+                       <item>
+                        <widget class="QPushButton" name="pushButton_resetConfig">
+                         <property name="text">
+                          <string>Reset all settings</string>
+                         </property>
+                        </widget>
+                       </item>
+                       <item>
+                        <spacer name="horizontalSpacer_4">
+                         <property name="orientation">
+                          <enum>Qt::Horizontal</enum>
+                         </property>
+                         <property name="sizeHint" stdset="0">
+                          <size>
+                           <width>40</width>
+                           <height>20</height>
+                          </size>
+                         </property>
+                        </spacer>
+                       </item>
+                      </layout>
+                     </widget>
+                    </item>
                     <item>
                      <widget class="QGroupBox" name="groupBox_5">
                       <property name="title">
@@ -384,7 +418,16 @@
                          </property>
                         </widget>
                        </item>
-                       <item row="3" column="1">
+                      </layout>
+                     </widget>
+                    </item>
+                    <item>
+                     <widget class="QGroupBox" name="groupBox_29">
+                      <property name="title">
+                       <string>Graph View</string>
+                      </property>
+                      <layout class="QGridLayout" name="gridLayout_104" columnstretch="0,1">
+                       <item row="0" column="1">
                         <widget class="QLabel" name="label_222">
                          <property name="text">
                           <string>Show loop closure hypotheses on the graph view. Nodes are colorized from blue to red depending on the highest hypothesis (which is red). Posterior hypotheses should be published. </string>
@@ -397,8 +440,8 @@
                          </property>
                         </widget>
                        </item>
-                       <item row="3" column="0">
-                        <widget class="QRadioButton" name="radioButton_posteriorGraphView">
+                       <item row="1" column="0">
+                        <widget class="QRadioButton" name="radioButton_wordsGraphView">
                          <property name="text">
                           <string/>
                          </property>
@@ -407,7 +450,17 @@
                          </property>
                         </widget>
                        </item>
-                       <item row="4" column="1">
+                       <item row="0" column="0">
+                        <widget class="QRadioButton" name="radioButton_posteriorGraphView">
+                         <property name="text">
+                          <string/>
+                         </property>
+                         <property name="checked">
+                          <bool>true</bool>
+                         </property>
+                        </widget>
+                       </item>
+                       <item row="1" column="1">
                         <widget class="QLabel" name="label_478">
                          <property name="text">
                           <string>Show 3D visual words count on the graph view. Nodes are colorized from blue to red depending on the maximum visual features extracted (which is red).</string>
@@ -420,24 +473,14 @@
                          </property>
                         </widget>
                        </item>
-                       <item row="4" column="0">
-                        <widget class="QRadioButton" name="radioButton_wordsGraphView">
-                         <property name="text">
-                          <string/>
-                         </property>
-                         <property name="checked">
-                          <bool>true</bool>
-                         </property>
-                        </widget>
-                       </item>
-                       <item row="5" column="0">
+                       <item row="3" column="0">
                         <widget class="QRadioButton" name="radioButton_nochangeGraphView">
                          <property name="text">
                           <string/>
                          </property>
                         </widget>
                        </item>
-                       <item row="5" column="1">
+                       <item row="3" column="1">
                         <widget class="QLabel" name="label_479">
                          <property name="text">
                           <string>No change of graph color.</string>
@@ -450,7 +493,38 @@
                          </property>
                         </widget>
                        </item>
+                       <item row="2" column="1">
+                        <widget class="QLabel" name="label_481">
+                         <property name="text">
+                          <string>Show nodes on which loop closures or localizations happened in red.</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="QRadioButton" name="radioButton_localizationsGraphView">
+                         <property name="text">
+                          <string/>
+                         </property>
+                         <property name="checked">
+                          <bool>false</bool>
+                         </property>
+                        </widget>
+                       </item>
                       </layout>
+                      <zorder>radioButton_posteriorGraphView</zorder>
+                      <zorder>radioButton_wordsGraphView</zorder>
+                      <zorder>label_478</zorder>
+                      <zorder>label_222</zorder>
+                      <zorder>label_479</zorder>
+                      <zorder>radioButton_nochangeGraphView</zorder>
+                      <zorder>label_481</zorder>
+                      <zorder>radioButton_localizationsGraphView</zorder>
                      </widget>
                     </item>
                     <item>
@@ -509,49 +583,6 @@ Show a yellow background when the number of odometry inliers goes under this thr
                       </layout>
                      </widget>
                     </item>
-                    <item>
-                     <widget class="QGroupBox" name="groupBox_2">
-                      <property name="title">
-                       <string>Save/Load Settings</string>
-                      </property>
-                      <layout class="QHBoxLayout" name="horizontalLayout_9">
-                       <item>
-                        <widget class="QPushButton" name="pushButton_loadConfig">
-                         <property name="text">
-                          <string>Load settings (*.ini) ...</string>
-                         </property>
-                        </widget>
-                       </item>
-                       <item>
-                        <widget class="QPushButton" name="pushButton_saveConfig">
-                         <property name="text">
-                          <string>Save settings (*.ini) ...</string>
-                         </property>
-                        </widget>
-                       </item>
-                       <item>
-                        <widget class="QPushButton" name="pushButton_resetConfig">
-                         <property name="text">
-                          <string>Reset all settings</string>
-                         </property>
-                        </widget>
-                       </item>
-                       <item>
-                        <spacer name="horizontalSpacer_4">
-                         <property name="orientation">
-                          <enum>Qt::Horizontal</enum>
-                         </property>
-                         <property name="sizeHint" stdset="0">
-                          <size>
-                           <width>40</width>
-                           <height>20</height>
-                          </size>
-                         </property>
-                        </spacer>
-                       </item>
-                      </layout>
-                     </widget>
-                    </item>
                    </layout>
                   </widget>
                  </item>
@@ -3569,7 +3600,7 @@ when using the file type, logs are saved in LogRtabmap.txt (located in the worki
                                   <string>RealSense</string>
                                  </property>
                                  <layout class="QGridLayout" name="gridLayout_73" columnstretch="0,1">
-                                  <item row="4" column="0">
+                                  <item row="5" column="0">
                                    <spacer name="verticalSpacer_51">
                                     <property name="orientation">
                                      <enum>Qt::Vertical</enum>
@@ -3675,6 +3706,16 @@ when using the file type, logs are saved in LogRtabmap.txt (located in the worki
                                     </property>
                                    </widget>
                                   </item>
+                                  <item row="3" column="0">
+                                   <widget class="QCheckBox" name="checkbox_realsenseDepthScaledToRGBSize">
+                                    <property name="text">
+                                     <string/>
+                                    </property>
+                                    <property name="checked">
+                                     <bool>false</bool>
+                                    </property>
+                                   </widget>
+                                  </item>
                                   <item row="3" column="1">
                                    <widget class="QLabel" name="label_realsenseOdom_2">
                                     <property name="text">
@@ -3688,13 +3729,38 @@ when using the file type, logs are saved in LogRtabmap.txt (located in the worki
                                     </property>
                                    </widget>
                                   </item>
-                                  <item row="3" column="0">
-                                   <widget class="QCheckBox" name="checkbox_realsenseDepthScaledToRGBSize">
-                                    <property name="text">
-                                     <string/>
+                                  <item row="4" column="0">
+                                   <widget class="QComboBox" name="comboBox_realsenseRGBSource">
+                                    <property name="sizeAdjustPolicy">
+                                     <enum>QComboBox::AdjustToContents</enum>
                                     </property>
-                                    <property name="checked">
-                                     <bool>false</bool>
+                                    <item>
+                                     <property name="text">
+                                      <string>Color</string>
+                                     </property>
+                                    </item>
+                                    <item>
+                                     <property name="text">
+                                      <string>Infrared</string>
+                                     </property>
+                                    </item>
+                                    <item>
+                                     <property name="text">
+                                      <string>Fisheye</string>
+                                     </property>
+                                    </item>
+                                   </widget>
+                                  </item>
+                                  <item row="4" column="1">
+                                   <widget class="QLabel" name="label_realsenseOdom_3">
+                                    <property name="text">
+                                     <string>RGB camera source. For fisheye has depth available only if it is calibrated.</string>
+                                    </property>
+                                    <property name="wordWrap">
+                                     <bool>true</bool>
+                                    </property>
+                                    <property name="textInteractionFlags">
+                                     <set>Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse</set>
                                     </property>
                                    </widget>
                                   </item>
@@ -5053,16 +5119,7 @@ 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="leftMargin">
-                        <number>0</number>
-                       </property>
-                       <property name="topMargin">
-                        <number>0</number>
-                       </property>
-                       <property name="rightMargin">
-                        <number>0</number>
-                       </property>
-                       <property name="bottomMargin">
+                       <property name="margin">
                         <number>0</number>
                        </property>
                        <item>
@@ -6250,6 +6307,19 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag
                    <layout class="QVBoxLayout" name="verticalLayout_15">
                     <item>
                      <layout class="QGridLayout" name="gridLayout_43" columnstretch="0,1">
+                      <item row="1" column="0">
+                       <widget class="QDoubleSpinBox" name="general_doubleSpinBox_detectionRate">
+                        <property name="suffix">
+                         <string> Hz</string>
+                        </property>
+                        <property name="decimals">
+                         <number>3</number>
+                        </property>
+                        <property name="value">
+                         <double>1.000000000000000</double>
+                        </property>
+                       </widget>
+                      </item>
                       <item row="0" column="0">
                        <widget class="QCheckBox" name="general_checkBox_SLAM_mode">
                         <property name="text">
@@ -6343,19 +6413,6 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag
                         </property>
                        </widget>
                       </item>
-                      <item row="1" column="0">
-                       <widget class="QDoubleSpinBox" name="general_doubleSpinBox_detectionRate">
-                        <property name="suffix">
-                         <string> Hz</string>
-                        </property>
-                        <property name="decimals">
-                         <number>3</number>
-                        </property>
-                        <property name="value">
-                         <double>1.000000000000000</double>
-                        </property>
-                       </widget>
-                      </item>
                       <item row="3" column="1">
                        <widget class="QLabel" name="label_165">
                         <property name="text">
@@ -6415,6 +6472,29 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag
                         </property>
                        </widget>
                       </item>
+                      <item row="7" column="1">
+                       <widget class="QLabel" name="label_498">
+                        <property name="text">
+                         <string>Rectify only features. If images are not already rectified (see parameter above) and this parameter is true, the whole RGB image will not be rectified, only the features. Warning: As projection of RGB-D image to point cloud is assuming that images are rectified, the generated point cloud map will have wrong colors if this parameter is true.</string>
+                        </property>
+                        <property name="wordWrap">
+                         <bool>true</bool>
+                        </property>
+                        <property name="textInteractionFlags">
+                         <set>Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse</set>
+                        </property>
+                       </widget>
+                      </item>
+                      <item row="7" column="0">
+                       <widget class="QCheckBox" name="general_checkBox_rectifyOnlyFeatures">
+                        <property name="text">
+                         <string/>
+                        </property>
+                        <property name="checked">
+                         <bool>true</bool>
+                        </property>
+                       </widget>
+                      </item>
                      </layout>
                     </item>
                     <item>
@@ -8790,19 +8870,6 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag
                          </property>
                         </widget>
                        </item>
-                       <item row="11" column="1">
-                        <widget class="QLabel" name="label_space2">
-                         <property name="text">
-                          <string>Local radius for nodes selection in the local map. This parameter is used in some approaches of the sub-panels.</string>
-                         </property>
-                         <property name="wordWrap">
-                          <bool>true</bool>
-                         </property>
-                         <property name="textInteractionFlags">
-                          <set>Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse</set>
-                         </property>
-                        </widget>
-                       </item>
                        <item row="6" column="1">
                         <widget class="QLabel" name="label_scanMatching">
                          <property name="text">
@@ -8816,29 +8883,6 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag
                          </property>
                         </widget>
                        </item>
-                       <item row="9" column="1">
-                        <widget class="QLabel" name="label_scanMatching_3">
-                         <property name="text">
-                          <string>Maximum local locations retrieved (0=disabled) near the current pose in the local map or on the current planned path (those on the planned path have priority).</string>
-                         </property>
-                         <property name="wordWrap">
-                          <bool>true</bool>
-                         </property>
-                         <property name="textInteractionFlags">
-                          <set>Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse</set>
-                         </property>
-                        </widget>
-                       </item>
-                       <item row="11" column="0">
-                        <widget class="QDoubleSpinBox" name="localDetection_radius">
-                         <property name="suffix">
-                          <string> m</string>
-                         </property>
-                         <property name="value">
-                          <double>1.000000000000000</double>
-                         </property>
-                        </widget>
-                       </item>
                        <item row="1" column="1">
                         <widget class="QLabel" name="label_152">
                          <property name="text">
@@ -8852,32 +8896,6 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag
                          </property>
                         </widget>
                        </item>
-                       <item row="6" column="0">
-                        <widget class="QCheckBox" name="odomScanHistory">
-                         <property name="text">
-                          <string/>
-                         </property>
-                        </widget>
-                       </item>
-                       <item row="10" column="0">
-                        <widget class="QDoubleSpinBox" name="rgdb_localImmunizationRatio">
-                         <property name="suffix">
-                          <string/>
-                         </property>
-                         <property name="decimals">
-                          <number>2</number>
-                         </property>
-                         <property name="maximum">
-                          <double>1.000000000000000</double>
-                         </property>
-                         <property name="singleStep">
-                          <double>0.100000000000000</double>
-                         </property>
-                         <property name="value">
-                          <double>0.100000000000000</double>
-                         </property>
-                        </widget>
-                       </item>
                        <item row="0" column="0">
                         <widget class="QDoubleSpinBox" name="rgdb_linearUpdate">
                          <property name="suffix">
@@ -8891,7 +8909,7 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag
                          </property>
                         </widget>
                        </item>
-                       <item row="9" column="0">
+                       <item row="10" column="0">
                         <widget class="QSpinBox" name="spinBox_maxLocalLocationsRetrieved"/>
                        </item>
                        <item row="0" column="1">
@@ -8907,19 +8925,6 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag
                          </property>
                         </widget>
                        </item>
-                       <item row="5" column="1">
-                        <widget class="QLabel" name="label_163">
-                         <property name="text">
-                          <string>Odometry change detected that triggers a new map (0 means whatever the odometry change, the detector will still link the new pose in the current map). Also by default, when an odometry with Identity transformation is detected, a new map is automatically created. </string>
-                         </property>
-                         <property name="wordWrap">
-                          <bool>true</bool>
-                         </property>
-                         <property name="textInteractionFlags">
-                          <set>Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse</set>
-                         </property>
-                        </widget>
-                       </item>
                        <item row="1" column="0">
                         <widget class="QDoubleSpinBox" name="rgdb_angularUpdate">
                          <property name="suffix">
@@ -8962,7 +8967,7 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag
                          </property>
                         </widget>
                        </item>
-                       <item row="10" column="1">
+                       <item row="11" column="1">
                         <widget class="QLabel" name="label_scanMatching_5">
                          <property name="text">
                           <string>Ratio of working memory for which local nodes are immunized from transfer.</string>
@@ -9001,6 +9006,108 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag
                          </property>
                         </widget>
                        </item>
+                       <item row="4" column="0">
+                        <widget class="QCheckBox" name="rgbd_savedLocalizationIgnored">
+                         <property name="text">
+                          <string/>
+                         </property>
+                        </widget>
+                       </item>
+                       <item row="4" column="1">
+                        <widget class="QLabel" name="label_scanMatching_2">
+                         <property name="text">
+                          <string>When loading a database, ignore last saved localization pose from previous session. If true, RTAB-Map won't assume it is restarting from the same place than where it shut down previously.</string>
+                         </property>
+                         <property name="wordWrap">
+                          <bool>true</bool>
+                         </property>
+                         <property name="textInteractionFlags">
+                          <set>Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse</set>
+                         </property>
+                        </widget>
+                       </item>
+                       <item row="8" column="0">
+                        <widget class="QCheckBox" name="loopClosure_bunlde">
+                         <property name="text">
+                          <string/>
+                         </property>
+                        </widget>
+                       </item>
+                       <item row="12" column="1">
+                        <widget class="QLabel" name="label_space2">
+                         <property name="text">
+                          <string>Local radius for nodes selection in the local map. This parameter is used in some approaches of the sub-panels.</string>
+                         </property>
+                         <property name="wordWrap">
+                          <bool>true</bool>
+                         </property>
+                         <property name="textInteractionFlags">
+                          <set>Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse</set>
+                         </property>
+                        </widget>
+                       </item>
+                       <item row="10" column="1">
+                        <widget class="QLabel" name="label_scanMatching_3">
+                         <property name="text">
+                          <string>Maximum local locations retrieved (0=disabled) near the current pose in the local map or on the current planned path (those on the planned path have priority).</string>
+                         </property>
+                         <property name="wordWrap">
+                          <bool>true</bool>
+                         </property>
+                         <property name="textInteractionFlags">
+                          <set>Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse</set>
+                         </property>
+                        </widget>
+                       </item>
+                       <item row="12" column="0">
+                        <widget class="QDoubleSpinBox" name="localDetection_radius">
+                         <property name="suffix">
+                          <string> m</string>
+                         </property>
+                         <property name="value">
+                          <double>1.000000000000000</double>
+                         </property>
+                        </widget>
+                       </item>
+                       <item row="6" column="0">
+                        <widget class="QCheckBox" name="odomScanHistory">
+                         <property name="text">
+                          <string/>
+                         </property>
+                        </widget>
+                       </item>
+                       <item row="11" column="0">
+                        <widget class="QDoubleSpinBox" name="rgdb_localImmunizationRatio">
+                         <property name="suffix">
+                          <string/>
+                         </property>
+                         <property name="decimals">
+                          <number>2</number>
+                         </property>
+                         <property name="maximum">
+                          <double>1.000000000000000</double>
+                         </property>
+                         <property name="singleStep">
+                          <double>0.100000000000000</double>
+                         </property>
+                         <property name="value">
+                          <double>0.100000000000000</double>
+                         </property>
+                        </widget>
+                       </item>
+                       <item row="5" column="1">
+                        <widget class="QLabel" name="label_163">
+                         <property name="text">
+                          <string>Odometry change detected that triggers a new map (0 means whatever the odometry change, the detector will still link the new pose in the current map). Also by default, when an odometry with Identity transformation is detected, a new map is automatically created. </string>
+                         </property>
+                         <property name="wordWrap">
+                          <bool>true</bool>
+                         </property>
+                         <property name="textInteractionFlags">
+                          <set>Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse</set>
+                         </property>
+                        </widget>
+                       </item>
                        <item row="3" column="1">
                         <widget class="QLabel" name="label_433">
                          <property name="text">
@@ -9014,13 +9121,6 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag
                          </property>
                         </widget>
                        </item>
-                       <item row="4" column="0">
-                        <widget class="QCheckBox" name="rgbd_savedLocalizationIgnored">
-                         <property name="text">
-                          <string/>
-                         </property>
-                        </widget>
-                       </item>
                        <item row="3" column="0">
                         <widget class="QDoubleSpinBox" name="rgdb_angularSpeedUpdate">
                          <property name="suffix">
@@ -9037,19 +9137,6 @@ 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_2">
-                         <property name="text">
-                          <string>When loading a database, ignore last saved localization pose from previous session. If true, RTAB-Map won't assume it is restarting from the same place than where it shut down previously.</string>
-                         </property>
-                         <property name="wordWrap">
-                          <bool>true</bool>
-                         </property>
-                         <property name="textInteractionFlags">
-                          <set>Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse</set>
-                         </property>
-                        </widget>
-                       </item>
                        <item row="8" column="1">
                         <widget class="QLabel" name="label_scanMatching_10">
                          <property name="text">
@@ -9063,8 +9150,21 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag
                          </property>
                         </widget>
                        </item>
-                       <item row="8" column="0">
-                        <widget class="QCheckBox" name="loopClosure_bunlde">
+                       <item row="9" column="1">
+                        <widget class="QLabel" name="label_scanMatching_11">
+                         <property name="text">
+                          <string>Ignore off diagonal values of the odometry covariance matrix.</string>
+                         </property>
+                         <property name="wordWrap">
+                          <bool>true</bool>
+                         </property>
+                         <property name="textInteractionFlags">
+                          <set>Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse</set>
+                         </property>
+                        </widget>
+                       </item>
+                       <item row="9" column="0">
+                        <widget class="QCheckBox" name="memCovOffDiagIgnored">
                          <property name="text">
                           <string/>
                          </property>
@@ -10363,6 +10463,19 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag
                    <layout class="QVBoxLayout" name="verticalLayout_115">
                     <item>
                      <layout class="QGridLayout" name="gridLayout_88" columnstretch="0,1">
+                      <item row="8" column="1">
+                       <widget class="QLabel" name="label_484">
+                        <property name="text">
+                         <string>Probability clamping threshold minimum.</string>
+                        </property>
+                        <property name="wordWrap">
+                         <bool>true</bool>
+                        </property>
+                        <property name="textInteractionFlags">
+                         <set>Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse</set>
+                        </property>
+                       </widget>
+                      </item>
                       <item row="0" column="0">
                        <widget class="QCheckBox" name="checkBox_grid_fullUpdate">
                         <property name="text">
@@ -10386,7 +10499,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="10" column="1">
                        <widget class="QLabel" name="label_224">
                         <property name="text">
                          <string>Erode obstacle cells.</string>
@@ -10399,6 +10512,19 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag
                         </property>
                        </widget>
                       </item>
+                      <item row="6" column="1">
+                       <widget class="QLabel" name="label_482">
+                        <property name="text">
+                         <string>Probability of a hit.</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="1">
                        <widget class="QLabel" name="label_319">
                         <property name="text">
@@ -10412,7 +10538,7 @@ 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="10" column="0">
                        <widget class="QCheckBox" name="checkBox_grid_erode">
                         <property name="text">
                          <string/>
@@ -10534,7 +10660,7 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag
                       <item row="5" column="1">
                        <widget class="QLabel" name="label_457">
                         <property name="text">
-                         <string>OctoMap occupancy threshold.</string>
+                         <string>Occupancy threshold.</string>
                         </property>
                         <property name="wordWrap">
                          <bool>true</bool>
@@ -10545,7 +10671,7 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag
                        </widget>
                       </item>
                       <item row="5" column="0">
-                       <widget class="QDoubleSpinBox" name="doubleSpinBox_grid_octomapOccThr">
+                       <widget class="QDoubleSpinBox" name="doubleSpinBox_grid_occThr">
                         <property name="suffix">
                          <string/>
                         </property>
@@ -10563,6 +10689,114 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag
                         </property>
                        </widget>
                       </item>
+                      <item row="7" column="1">
+                       <widget class="QLabel" name="label_483">
+                        <property name="text">
+                         <string>Probability of a miss.</string>
+                        </property>
+                        <property name="wordWrap">
+                         <bool>true</bool>
+                        </property>
+                        <property name="textInteractionFlags">
+                         <set>Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse</set>
+                        </property>
+                       </widget>
+                      </item>
+                      <item row="6" column="0">
+                       <widget class="QDoubleSpinBox" name="doubleSpinBox_grid_probHit">
+                        <property name="suffix">
+                         <string/>
+                        </property>
+                        <property name="minimum">
+                         <double>0.500000000000000</double>
+                        </property>
+                        <property name="maximum">
+                         <double>1.000000000000000</double>
+                        </property>
+                        <property name="singleStep">
+                         <double>0.100000000000000</double>
+                        </property>
+                        <property name="value">
+                         <double>0.500000000000000</double>
+                        </property>
+                       </widget>
+                      </item>
+                      <item row="7" column="0">
+                       <widget class="QDoubleSpinBox" name="doubleSpinBox_grid_probMiss">
+                        <property name="suffix">
+                         <string/>
+                        </property>
+                        <property name="minimum">
+                         <double>0.000000000000000</double>
+                        </property>
+                        <property name="maximum">
+                         <double>0.500000000000000</double>
+                        </property>
+                        <property name="singleStep">
+                         <double>0.100000000000000</double>
+                        </property>
+                        <property name="value">
+                         <double>0.500000000000000</double>
+                        </property>
+                       </widget>
+                      </item>
+                      <item row="9" column="1">
+                       <widget class="QLabel" name="label_485">
+                        <property name="text">
+                         <string>Probability clamping threshold maximum.</string>
+                        </property>
+                        <property name="wordWrap">
+                         <bool>true</bool>
+                        </property>
+                        <property name="textInteractionFlags">
+                         <set>Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse</set>
+                        </property>
+                       </widget>
+                      </item>
+                      <item row="8" column="0">
+                       <widget class="QDoubleSpinBox" name="doubleSpinBox_grid_clampingMin">
+                        <property name="suffix">
+                         <string/>
+                        </property>
+                        <property name="decimals">
+                         <number>4</number>
+                        </property>
+                        <property name="minimum">
+                         <double>0.000000000000000</double>
+                        </property>
+                        <property name="maximum">
+                         <double>1.000000000000000</double>
+                        </property>
+                        <property name="singleStep">
+                         <double>0.100000000000000</double>
+                        </property>
+                        <property name="value">
+                         <double>0.500000000000000</double>
+                        </property>
+                       </widget>
+                      </item>
+                      <item row="9" column="0">
+                       <widget class="QDoubleSpinBox" name="doubleSpinBox_grid_clampingMax">
+                        <property name="suffix">
+                         <string/>
+                        </property>
+                        <property name="decimals">
+                         <number>4</number>
+                        </property>
+                        <property name="minimum">
+                         <double>0.000000000000000</double>
+                        </property>
+                        <property name="maximum">
+                         <double>1.000000000000000</double>
+                        </property>
+                        <property name="singleStep">
+                         <double>0.100000000000000</double>
+                        </property>
+                        <property name="value">
+                         <double>0.500000000000000</double>
+                        </property>
+                       </widget>
+                      </item>
                      </layout>
                     </item>
                    </layout>
@@ -15013,7 +15247,7 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag
                          <property name="text">
                           <string>NNDR ratio
 (A matching pair is accepted, if its distance is closer than X times the distance of the second nearest neighbor)
-Lower the ratio -&gt; higher the precision. 0 means disabled, matching the nearest.</string>
+Lower the ratio -&gt; higher the precision.</string>
                          </property>
                          <property name="wordWrap">
                           <bool>true</bool>
@@ -15082,16 +15316,7 @@ Lower the ratio -&gt; higher the precision. 0 means disabled, matching the neare
                         <property name="spacing">
                          <number>0</number>
                         </property>
-                        <property name="leftMargin">
-                         <number>0</number>
-                        </property>
-                        <property name="topMargin">
-                         <number>0</number>
-                        </property>
-                        <property name="rightMargin">
-                         <number>0</number>
-                        </property>
-                        <property name="bottomMargin">
+                        <property name="margin">
                          <number>0</number>
                         </property>
                         <item>
@@ -15171,16 +15396,7 @@ Lower the ratio -&gt; higher the precision. 0 means disabled, matching the neare
                         <property name="spacing">
                          <number>0</number>
                         </property>
-                        <property name="leftMargin">
-                         <number>0</number>
-                        </property>
-                        <property name="topMargin">
-                         <number>0</number>
-                        </property>
-                        <property name="rightMargin">
-                         <number>0</number>
-                        </property>
-                        <property name="bottomMargin">
+                        <property name="margin">
                          <number>0</number>
                         </property>
                         <item>
@@ -15292,16 +15508,7 @@ Lower the ratio -&gt; higher the precision. 0 means disabled, matching the neare
                         <property name="spacing">
                          <number>0</number>
                         </property>
-                        <property name="leftMargin">
-                         <number>0</number>
-                        </property>
-                        <property name="topMargin">
-                         <number>0</number>
-                        </property>
-                        <property name="rightMargin">
-                         <number>0</number>
-                        </property>
-                        <property name="bottomMargin">
+                        <property name="margin">
                          <number>0</number>
                         </property>
                         <item>

package.xml

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

tools/Camera/main.cpp

@@ -32,6 +32,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #include "rtabmap/utilite/UDirectory.h"
 #include "rtabmap/utilite/UConversion.h"
 #include <opencv2/highgui/highgui.hpp>
+#include <opencv2/highgui/highgui_c.h>
 #include <stdio.h>
 
 void showUsage()

tools/CameraRGBD/main.cpp

@@ -37,6 +37,10 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #include "rtabmap/utilite/UConversion.h"
 #include <opencv2/highgui/highgui.hpp>
 #include <opencv2/imgproc/imgproc.hpp>
+#include <opencv2/imgproc/types_c.h>
+#if CV_MAJOR_VERSION >= 3
+#include <opencv2/videoio/videoio_c.h>
+#endif
 #include <pcl/visualization/cloud_viewer.h>
 #include <stdio.h>
 #include <signal.h>

tools/EpipolarGeometry/main.cpp

@@ -83,7 +83,7 @@ public:
 		view1_->setImageDepthShown(true);
 
 		view1_->setImage(uCvMat2QImage(image1));
-		view1_->setImageDepth(uCvMat2QImage(image2));
+		view1_->setImageDepth(image2);
 
 		drawKeypoints(words1, words2, status);
 	}

tools/EurocDataset/CMakeLists.txt

@@ -5,10 +5,12 @@ FIND_PACKAGE(yaml-cpp QUIET)
 IF(NOT yaml-cpp_FOUND)
    find_package(PkgConfig QUIET)
    IF(PKG_CONFIG_FOUND)
-      pkg_check_modules(yaml_cpp REQUIRED yaml-cpp)
-      SET(YAML_CPP_LIBRARIES ${yaml_cpp_LIBRARIES})
-      SET(YAML_CPP_INCLUDE_DIRS ${yaml_cpp_INCLUDEDIR})
-      SET(yaml-cpp_FOUND ${yaml_cpp_FOUND})
+      pkg_check_modules(yaml_cpp QUIET yaml-cpp)
+	  IF(yaml_cpp_FOUND)
+         SET(YAML_CPP_LIBRARIES ${yaml_cpp_LIBRARIES})
+         SET(YAML_CPP_INCLUDE_DIRS ${yaml_cpp_INCLUDEDIR})
+         SET(yaml-cpp_FOUND ${yaml_cpp_FOUND})
+	  ENDIF(yaml_cpp_FOUND)
    ENDIF(PKG_CONFIG_FOUND)
 ENDIF(NOT yaml-cpp_FOUND)
 

tools/ImagesJoiner/main.cpp

@@ -138,7 +138,7 @@ int main(int argc, char * argv[])
 
 			cv::Mat roiA(targetImage, cv::Rect( 0, 0, sizeA.width, sizeA.height ));
 			imageA.copyTo(roiA);
-			cv::Mat roiB( targetImage, cvRect( sizeA.width, 0, sizeB.width, sizeB.height ) );
+			cv::Mat roiB( targetImage, cv::Rect( sizeA.width, 0, sizeB.width, sizeB.height ) );
 			imageB.copyTo(roiB);
 
 			if(!cv::imwrite(targetFilePath.c_str(), targetImage))

tools/StereoEval/main.cpp

@@ -37,6 +37,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #include <rtabmap/utilite/UConversion.h>
 #include <rtabmap/utilite/UTimer.h>
 #include <rtabmap/utilite/UMath.h>
+#include <opencv2/imgproc/types_c.h>
 #include <fstream>
 #include <string>