Fixed 2 cmake minor warnings on standard build

Fix laserscan from depth

CMakeLists.txt

@@ -20,8 +20,8 @@ SET(CMAKE_MODULE_PATH "${PROJECT_SOURCE_DIR}/cmake_modules")
 # VERSION
 #######################
 SET(RTABMAP_MAJOR_VERSION 0)
-SET(RTABMAP_MINOR_VERSION 16)
+SET(RTABMAP_MINOR_VERSION 17)
-SET(RTABMAP_PATCH_VERSION 3)
+SET(RTABMAP_PATCH_VERSION 0)
 SET(RTABMAP_VERSION
   ${RTABMAP_MAJOR_VERSION}.${RTABMAP_MINOR_VERSION}.${RTABMAP_PATCH_VERSION})
   
@@ -113,15 +113,13 @@ SET( CMAKE_ARCHIVE_OUTPUT_DIRECTORY_DEBUG "${CMAKE_ARCHIVE_OUTPUT_DIRECTORY}")
 SET( CMAKE_ARCHIVE_OUTPUT_DIRECTORY_RELEASE "${CMAKE_ARCHIVE_OUTPUT_DIRECTORY}")
 
 ####### INSTALL DIR #######
-set(INSTALL_INCLUDE_DIR include/${PROJECT_PREFIX}-${RTABMAP_MAJOR_VERSION}.${RTABMAP_MINOR_VERSION} CACHE PATH
+set(INSTALL_INCLUDE_DIR include/${PROJECT_PREFIX}-${RTABMAP_MAJOR_VERSION}.${RTABMAP_MINOR_VERSION})
-  "Installation directory for header files")
 if(WIN32 AND NOT CYGWIN)
   set(DEF_INSTALL_CMAKE_DIR CMake)
 else()
   set(DEF_INSTALL_CMAKE_DIR ${CMAKE_INSTALL_LIBDIR}/${PROJECT_PREFIX}-${RTABMAP_MAJOR_VERSION}.${RTABMAP_MINOR_VERSION})
 endif()
-set(INSTALL_CMAKE_DIR ${DEF_INSTALL_CMAKE_DIR} CACHE PATH
+set(INSTALL_CMAKE_DIR ${DEF_INSTALL_CMAKE_DIR})
-  "Installation directory for CMake files")
   
 ####### BUILD OPTIONS #######
 
@@ -163,6 +161,7 @@ option(WITH_REALSENSE     "Include RealSense support"            ON)
 option(WITH_REALSENSE_SLAM "Include RealSenseSlam support"       ON)
 option(WITH_OCTOMAP       "Include Octomap support"              ON)
 option(WITH_CPUTSDF       "Include CPUTSDF support"              ON)
+option(WITH_OPENCHISEL    "Include open_chisel support"          ON)
 option(WITH_FOVIS         "Include FOVIS support"                ON)
 option(WITH_VISO2         "Include VISO2 support"                ON)
 option(WITH_DVO           "Include DVO support"                  ON)
@@ -171,11 +170,18 @@ option(WITH_OKVIS         "Include OKVIS support"                ON)
 option(PCL_OMP            "With PCL OMP implementations"         ON)
 
 FIND_PACKAGE(OpenCV REQUIRED QUIET)
+
 IF(WITH_QT)
 FIND_PACKAGE(PCL 1.7 REQUIRED QUIET COMPONENTS common io kdtree search surface filters registration sample_consensus segmentation visualization)
 ELSE()
 FIND_PACKAGE(PCL 1.7 REQUIRED QUIET COMPONENTS common io kdtree search surface filters registration sample_consensus segmentation )
 ENDIF()
+if("${PCL_DEFINITIONS}" MATCHES "-march=native")
+  MESSAGE(WARNING "PCL definitions contain \"-march=native\", make sure all libraries using Eigen are also compiled with that flag to avoid some segmentation faults (with gdb referring to some Eigen functions).")
+else()
+  MESSAGE(STATUS "PCL definitions don't contain \"-march=native\", make sure all libraries using Eigen are also compiled without that flag to avoid some segmentation faults (with gdb referring to some Eigen functions).")
+endif()
+
 FIND_PACKAGE(ZLIB REQUIRED QUIET)
 
 if(NOT "${PCL_LIBRARIES}" STREQUAL "")
@@ -267,12 +273,12 @@ IF(WITH_FREENECT2)
     ENDIF(freenect2_FOUND)
 ENDIF(WITH_FREENECT2)
 
-IF(WITH_K4W2)
+IF(WITH_K4W2 AND WIN32)
     FIND_PACKAGE(KinectSDK2 QUIET)
 	IF(KinectSDK2_FOUND)
        MESSAGE(STATUS "Found Kinect for Windows 2: ${KinectSDK2_INCLUDE_DIRS}")
     ENDIF(KinectSDK2_FOUND)
-ENDIF(WITH_K4W2)
+ENDIF(WITH_K4W2 AND WIN32)
 
 # IF PCL depends on OpenNI2 (already found), ignore WITH_OPENNI2
 IF(WITH_OPENNI2 OR OpenNI2_FOUND)
@@ -382,6 +388,13 @@ IF(WITH_CPUTSDF)
     ENDIF(CPUTSDF_FOUND)
 ENDIF(WITH_CPUTSDF)
 
+IF(WITH_OPENCHISEL)
+    find_package(open_chisel QUIET)
+    if(open_chisel_FOUND)
+        MESSAGE(STATUS "Found open_chisel: ${open_chisel_INCLUDE_DIRS}")
+    endif(open_chisel_FOUND)
+ENDIF(WITH_OPENCHISEL)
+
 IF(WITH_FOVIS)
     FIND_PACKAGE(libfovis QUIET)
     IF(libfovis_FOUND)
@@ -407,7 +420,6 @@ IF(WITH_OKVIS)
     FIND_PACKAGE(okvis 1.1 QUIET)
     IF(okvis_FOUND)
        MESSAGE(STATUS "Found okvis: ${OKVIS_INCLUDE_DIRS}")
-       MESSAGE(WARNING "Don't forget to build okvis and its dependencies without \"-march=native\" to avoid crash when okvis starts.")
        find_package(brisk 2 REQUIRED)
        MESSAGE(STATUS "Found brisk: ${BRISK_INCLUDE_DIRS}")
        find_package(opengv REQUIRED)
@@ -429,12 +441,11 @@ IF(WITH_ORB_SLAM2 AND NOT G2O_FOUND)
           MESSAGE(STATUS "Found Pangolin: ${Pangolin_INCLUDE_DIRS}")
           SET(ORB_SLAM2_INCLUDE_DIRS ${ORB_SLAM2_INCLUDE_DIRS} ${Pangolin_INCLUDE_DIRS})
           SET(ORB_SLAM2_LIBRARIES ${ORB_SLAM2_LIBRARIES} ${Pangolin_LIBRARIES})
-          MESSAGE(WARNING "Don't forget to build ORB_SLAM2 (and included g2o) without \"-march=native\" to avoid crash when ORB_SLAM2 starts.")
        ENDIF()
     ENDIF(ORB_SLAM2_FOUND)
 ENDIF(WITH_ORB_SLAM2 AND NOT G2O_FOUND)
 
-IF(G2O_FOUND OR GTSAM_FOUND OR ZED_FOUND OR ANDROID OR RealSense_FOUND OR ORB_SLAM2_FOUND OR okvis_FOUND)
+IF(G2O_FOUND OR GTSAM_FOUND OR ZED_FOUND OR ANDROID OR RealSense_FOUND OR ORB_SLAM2_FOUND OR okvis_FOUND OR open_chisel_FOUND)
    #Newest versions require std11
    IF(NOT MSVC)
      include(CheckCXXCompilerFlag)
@@ -448,7 +459,7 @@ IF(G2O_FOUND OR GTSAM_FOUND OR ZED_FOUND OR ANDROID OR RealSense_FOUND OR ORB_SL
         message(STATUS "The compiler ${CMAKE_CXX_COMPILER} has no C++11 support. Please use a different C++ compiler if you want to use g2o or gtsam (set \"-DWITH_G2O=OFF -DWITH_GTSAM=OFF\" to build without g2o and gtsam).")
       ENDIF()
    ENDIF()
-ENDIF(G2O_FOUND OR GTSAM_FOUND OR ZED_FOUND OR ANDROID OR RealSense_FOUND OR ORB_SLAM2_FOUND OR okvis_FOUND)
+ENDIF(G2O_FOUND OR GTSAM_FOUND OR ZED_FOUND OR ANDROID OR RealSense_FOUND OR ORB_SLAM2_FOUND OR okvis_FOUND OR open_chisel_FOUND)
 
 ####### OSX BUNDLE CMAKE_INSTALL_PREFIX #######
 IF(APPLE AND BUILD_AS_BUNDLE)
@@ -567,6 +578,11 @@ IF(NOT CPUTSDF_FOUND)
 ELSE()
    SET(CONF_DEPENDENCIES ${CONF_DEPENDENCIES} ${CPUTSDF_LIBRARIES}) 
 ENDIF()
+IF(NOT open_chisel_FOUND)
+   SET(OPENCHISEL "//")
+ELSE()
+   SET(CONF_DEPENDENCIES ${CONF_DEPENDENCIES} ${open_chisel_LIBRARIES}) 
+ENDIF()
 IF(NOT libfovis_FOUND)
    SET(FOVIS "//")
 ELSE()
@@ -783,6 +799,7 @@ IF(APPLE)
 MESSAGE(STATUS "  BUILD_AS_BUNDLE =      ${BUILD_AS_BUNDLE}")
 ENDIF(APPLE)
 MESSAGE(STATUS "  CMAKE_CXX_FLAGS = ${CMAKE_CXX_FLAGS}")
+MESSAGE(STATUS "  PCL_DEFINITIONS = ${PCL_DEFINITIONS}")
 
 IF(OpenCV_FOUND)
   IF(OpenCV_VERSION_MAJOR EQUAL 2)
@@ -939,6 +956,14 @@ ELSE()
 MESSAGE(STATUS "  With CPUTSDF              = NO (CPUTSDF not found)")
 ENDIF()
 
+IF(open_chisel_FOUND)
+MESSAGE(STATUS "  With OpenChisel           = YES (License: ???)")
+ELSEIF(NOT WITH_OPENCHISEL)
+MESSAGE(STATUS "  With OpenChisel           = NO (WITH_OPENCHISEL=OFF)")
+ELSE()
+MESSAGE(STATUS "  With OpenChisel           = NO (open_chisel not found)")
+ENDIF()
+
 IF(libfovis_FOUND)
 MESSAGE(STATUS "  With libfovis             = YES (License: GPLv2)")
 ELSEIF(NOT WITH_FOVIS)

README.md

@@ -7,7 +7,7 @@ rtabmap ![Analytics](https://ga-beacon.appspot.com/UA-56986679-3/github-main?pix
 [![License][license-image]][license]
 Linux: [![Build Status](https://travis-ci.org/introlab/rtabmap.svg?branch=master)](https://travis-ci.org/introlab/rtabmap) Windows: [![Build status](https://ci.appveyor.com/api/projects/status/hr73xspix9oqa26h/branch/master?svg=true)](https://ci.appveyor.com/project/matlabbe/rtabmap/branch/master)
 
-[release-image]: https://img.shields.io/badge/release-0.14.0-green.svg?style=flat
+[release-image]: https://img.shields.io/badge/release-0.16.3-green.svg?style=flat
 [releases]: https://github.com/introlab/rtabmap/releases
 
 [license-image]: https://img.shields.io/badge/license-BSD-green.svg?style=flat

Version.h.in

@@ -57,6 +57,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 @REALSENSESLAM@#define RTABMAP_REALSENSE_SLAM
 @OCTOMAP@#define RTABMAP_OCTOMAP
 @CPUTSDF@#define RTABMAP_CPUTSDF
+@OPENCHISEL@#define RTABMAP_OPENCHISEL
 @FOVIS@#define RTABMAP_FOVIS
 @VISO2@#define RTABMAP_VISO2
 @DVO@#define RTABMAP_DVO

app/android/jni/RTABMapApp.cpp

@@ -335,25 +335,18 @@ int RTABMapApp::openDatabase(const std::string & databasePath, bool databaseInMe
 	std::vector<std::vector<Eigen::Vector2f> > texCoords;
 #endif
 	cv::Mat textures;
-	std::map<int, rtabmap::Transform> optPoses;
 	if(!databaseSource.empty())
 	{
 		UEventsManager::post(new rtabmap::RtabmapEventInit(rtabmap::RtabmapEventInit::kInfo, "Loading optimized cloud/mesh..."));
 		rtabmap::DBDriver * driver = rtabmap::DBDriver::create();
 		if(driver->openConnection(databaseSource))
 		{
-			cloudMat = driver->loadOptimizedMesh(&optPoses, &polygons, &texCoords, &textures);
+			cloudMat = driver->loadOptimizedMesh(&polygons, &texCoords, &textures);
 			if(!cloudMat.empty())
 			{
 				LOGI("Open: Found optimized mesh! Visualizing it.");
 				optMesh_ = rtabmap::util3d::assembleTextureMesh(cloudMat, polygons, texCoords, textures, true);
 				optTexture_ = textures;
-				if(optPoses.size())
-				{
-					// just take the last as reference
-					optRefId_ = optPoses.rbegin()->first;
-					optRefPose_ = new rtabmap::Transform(optPoses.rbegin()->second);
-				}
 				if(!optTexture_.empty())
 				{
 					LOGI("Open: Texture mesh: %dx%d.", optTexture_.cols, optTexture_.rows);
@@ -592,6 +585,14 @@ int RTABMapApp::openDatabase(const std::string & databasePath, bool databaseInMe
 
 	rtabmap_->setOptimizedPoses(poses);
 
+	// for optimized mesh
+	if(poses.size())
+	{
+		// just take the last as reference
+		optRefId_ = poses.rbegin()->first;
+		optRefPose_ = new rtabmap::Transform(poses.rbegin()->second);
+	}
+
 	if(camera_)
 	{
 		camera_->resetOrigin();
@@ -2713,7 +2714,7 @@ bool RTABMapApp::exportMesh(
 						}
 						boost::mutex::scoped_lock  lock(rtabmapMutex_);
 
-						rtabmap_->getMemory()->saveOptimizedMesh(cloudMat, poses, polygons);
+						rtabmap_->getMemory()->saveOptimizedMesh(cloudMat, polygons);
 						success = true;
 					}
 				}
@@ -2734,7 +2735,7 @@ bool RTABMapApp::exportMesh(
 						}
 					}
 					boost::mutex::scoped_lock  lock(rtabmapMutex_);
-					rtabmap_->getMemory()->saveOptimizedMesh(cloudMat, poses, polygons, textureMesh->tex_coordinates, globalTextures);
+					rtabmap_->getMemory()->saveOptimizedMesh(cloudMat, polygons, textureMesh->tex_coordinates, globalTextures);
 					success = true;
 				}
 				else
@@ -2857,7 +2858,7 @@ bool RTABMapApp::exportMesh(
 				{
 					cv::Mat cloudMat = rtabmap::compressData2(rtabmap::util3d::laserScanFromPointCloud(*mergedClouds)); // for database
 					boost::mutex::scoped_lock  lock(rtabmapMutex_);
-					rtabmap_->getMemory()->saveOptimizedMesh(cloudMat, poses);
+					rtabmap_->getMemory()->saveOptimizedMesh(cloudMat);
 					success = true;
 				}
 			}
@@ -2887,6 +2888,20 @@ bool RTABMapApp::exportMesh(
 	}
 	exporting_ = false;
 
+	optRefId_ = 0;
+	if(optRefPose_)
+	{
+		delete optRefPose_;
+		optRefPose_ = 0;
+	}
+	if(success && poses.size())
+	{
+		// for optimized mesh
+		// just take the last as reference
+		optRefId_ = poses.rbegin()->first;
+		optRefPose_ = new rtabmap::Transform(poses.rbegin()->second);
+	}
+
 	return success;
 }
 
@@ -2895,12 +2910,6 @@ bool RTABMapApp::postExportation(bool visualize)
 	LOGI("postExportation(visualize=%d)", visualize?1:0);
 	optMesh_.reset(new pcl::TextureMesh);
 	optTexture_ = cv::Mat();
-	optRefId_ = 0;
-	if(optRefPose_)
-	{
-		delete optRefPose_;
-		optRefPose_ = 0;
-	}
 	exportedMeshUpdated_ = false;
 
 	if(visualize)
@@ -2914,23 +2923,15 @@ bool RTABMapApp::postExportation(bool visualize)
 		std::vector<std::vector<Eigen::Vector2f> > texCoords;
 #endif
 		cv::Mat textures;
-		std::map<int, rtabmap::Transform> optPoses;
 		if(rtabmap_ && rtabmap_->getMemory())
 		{
-			cloudMat = rtabmap_->getMemory()->loadOptimizedMesh(&optPoses, &polygons, &texCoords, &textures);
+			cloudMat = rtabmap_->getMemory()->loadOptimizedMesh(&polygons, &texCoords, &textures);
 			if(!cloudMat.empty())
 			{
 				LOGI("postExportation: Found optimized mesh! Visualizing it.");
 				optMesh_ = rtabmap::util3d::assembleTextureMesh(cloudMat, polygons, texCoords, textures, true);
 				optTexture_ = textures;
 
-				if(optPoses.size())
-				{
-					// just take the last as reference
-					optRefId_ = optPoses.rbegin()->first;
-					optRefPose_ = new rtabmap::Transform(optPoses.rbegin()->second);
-				}
-
 				boost::mutex::scoped_lock  lock(renderingMutex_);
 				visualizingMesh_ = true;
 				exportedMeshUpdated_ = true;
@@ -2975,10 +2976,9 @@ bool RTABMapApp::writeExportedMesh(const std::string & directory, const std::str
 	std::vector<std::vector<Eigen::Vector2f> > texCoords;
 #endif
 	cv::Mat textures;
-	std::map<int, rtabmap::Transform> optPoses;
 	if(rtabmap_ && rtabmap_->getMemory())
 	{
-		cloudMat = rtabmap_->getMemory()->loadOptimizedMesh(&optPoses, &polygons, &texCoords, &textures);
+		cloudMat = rtabmap_->getMemory()->loadOptimizedMesh(&polygons, &texCoords, &textures);
 		if(!cloudMat.empty())
 		{
 			LOGI("writeExportedMesh: Found optimized mesh!");

cmake_modules/FindKinectSDK2.cmake

@@ -180,4 +180,4 @@ if(KinectSDK2_FOUND)
   endif()
 endif()
 
-message(STATUS "KinectSDK2_FOUND : ${KinectSDK2_FOUND}")
+message(STATUS "KinectSDK2_FOUND : ${KinectSDK2_FOUND}")

corelib/include/rtabmap/core/DBDriver.h

@@ -102,9 +102,12 @@ public:
 	void addStatistics(const Statistics & statistics) const;
 	void savePreviewImage(const cv::Mat & image) const;
 	cv::Mat loadPreviewImage() const;
+	void saveOptimizedPoses(const std::map<int, Transform> & optimizedPoses, const Transform & lastlocalizationPose) const;
+	std::map<int, Transform> loadOptimizedPoses(Transform * lastlocalizationPose) const;
+	void save2DMap(const cv::Mat & map, float xMin, float yMin, float cellSize) const;
+	cv::Mat load2DMap(float & xMin, float & yMin, float & cellSize) const;
 	void saveOptimizedMesh(
 			const cv::Mat & cloud,
-			const std::map<int, Transform> & poses = std::map<int, Transform>(), // if we want to do localization afterward using optimized mesh
 			const std::vector<std::vector<std::vector<unsigned int> > > & polygons = std::vector<std::vector<std::vector<unsigned int> > >(),      // Textures -> polygons -> vertices
 #if PCL_VERSION_COMPARE(>=, 1, 8, 0)
 			const std::vector<std::vector<Eigen::Vector2f, Eigen::aligned_allocator<Eigen::Vector2f> > > & texCoords = std::vector<std::vector<Eigen::Vector2f, Eigen::aligned_allocator<Eigen::Vector2f> > >(), // Textures -> uv coords for each vertex of the polygons
@@ -113,7 +116,6 @@ public:
 #endif
 			const cv::Mat & textures = cv::Mat()) const; // concatenated textures (assuming square textures with all same size);
 	cv::Mat loadOptimizedMesh(
-			std::map<int, Transform> * poses = 0,
 			std::vector<std::vector<std::vector<unsigned int> > > * polygons = 0,
 #if PCL_VERSION_COMPARE(>=, 1, 8, 0)
 			std::vector<std::vector<Eigen::Vector2f, Eigen::aligned_allocator<Eigen::Vector2f> > > * texCoords = 0,
@@ -230,9 +232,12 @@ private:
 	virtual void addStatisticsQuery(const Statistics & statistics) const = 0;
 	virtual void savePreviewImageQuery(const cv::Mat & image) const = 0;
 	virtual cv::Mat loadPreviewImageQuery() const = 0;
+	virtual void saveOptimizedPosesQuery(const std::map<int, Transform> & optimizedPoses, const Transform & lastlocalizationPose) const = 0;
+	virtual std::map<int, Transform> loadOptimizedPosesQuery(Transform * lastlocalizationPose) const = 0;
+	virtual void save2DMapQuery(const cv::Mat & map, float xMin, float yMin, float cellSize) const = 0;
+	virtual cv::Mat load2DMapQuery(float & xMin, float & yMin, float & cellSize) const = 0;
 	virtual void saveOptimizedMeshQuery(
 				const cv::Mat & cloud,
-				const std::map<int, Transform> & poses,
 				const std::vector<std::vector<std::vector<unsigned int> > > & polygons,
 #if PCL_VERSION_COMPARE(>=, 1, 8, 0)
 				const std::vector<std::vector<Eigen::Vector2f, Eigen::aligned_allocator<Eigen::Vector2f> > > & texCoords,
@@ -241,7 +246,6 @@ private:
 #endif
 				const cv::Mat & textures) const = 0;
 	virtual cv::Mat loadOptimizedMeshQuery(
-				std::map<int, Transform> * poses,
 				std::vector<std::vector<std::vector<unsigned int> > > * polygons,
 #if PCL_VERSION_COMPARE(>=, 1, 8, 0)
 				std::vector<std::vector<Eigen::Vector2f, Eigen::aligned_allocator<Eigen::Vector2f> > > * texCoords,

corelib/include/rtabmap/core/Memory.h

@@ -95,9 +95,12 @@ public:
 	void saveStatistics(const Statistics & statistics);
 	void savePreviewImage(const cv::Mat & image) const;
 	cv::Mat loadPreviewImage() const;
+	void saveOptimizedPoses(const std::map<int, Transform> & optimizedPoses, const Transform & lastlocalizationPose) const;
+	std::map<int, Transform> loadOptimizedPoses(Transform * lastlocalizationPose) const;
+	void save2DMap(const cv::Mat & map, float xMin, float yMin, float cellSize) const;
+	cv::Mat load2DMap(float & xMin, float & yMin, float & cellSize) const;
 	void saveOptimizedMesh(
 			const cv::Mat & cloud,
-			const std::map<int, Transform> & poses = std::map<int, Transform>(), // if we want to do localization afterward using optimized mesh
 			const std::vector<std::vector<std::vector<unsigned int> > > & polygons = std::vector<std::vector<std::vector<unsigned int> > >(),      // Textures -> polygons -> vertices
 #if PCL_VERSION_COMPARE(>=, 1, 8, 0)
 			const std::vector<std::vector<Eigen::Vector2f, Eigen::aligned_allocator<Eigen::Vector2f> > > & texCoords = std::vector<std::vector<Eigen::Vector2f, Eigen::aligned_allocator<Eigen::Vector2f> > >(), // Textures -> uv coords for each vertex of the polygons
@@ -106,7 +109,6 @@ public:
 #endif
 			const cv::Mat & textures = cv::Mat()) const; // concatenated textures (assuming square textures with all same size)
 	cv::Mat loadOptimizedMesh(
-			std::map<int, Transform> * poses = 0,
 			std::vector<std::vector<std::vector<unsigned int> > > * polygons = 0,
 #if PCL_VERSION_COMPARE(>=, 1, 8, 0)
 			std::vector<std::vector<Eigen::Vector2f, Eigen::aligned_allocator<Eigen::Vector2f> > > * texCoords = 0,

corelib/include/rtabmap/core/OccupancyGrid.h

@@ -42,6 +42,7 @@ class RTABMAP_EXP OccupancyGrid
 public:
 	OccupancyGrid(const ParametersMap & parameters = ParametersMap());
 	void parseParameters(const ParametersMap & parameters);
+	void setMap(const cv::Mat & map, float xMin, float yMin, float cellSize, const std::map<int, Transform> & poses);
 	void setCellSize(float cellSize);
 	float getCellSize() const {return cellSize_;}
 	void setCloudAssembling(bool enabled);
@@ -118,7 +119,6 @@ private:
 	float noiseFilteringRadius_;
 	int noiseFilteringMinNeighbors_;
 	bool scan2dUnknownSpaceFilled_;
-	double scan2dMaxUnknownSpaceFilledRange_;
 	bool rayTracing_;
 	bool fullUpdate_;
 	float minMapSize_;

corelib/include/rtabmap/core/OdometryOkvis.h

@@ -52,8 +52,10 @@ private:
 
 private:
 	std::string configFilename_;
+#ifdef RTABMAP_OKVIS
 	OkvisCallbackHandler * okvisCallbackHandler_;
 	okvis::ThreadedKFVio * okvisEstimator_;
+#endif
 	ParametersMap okvisParameters_;
 	IMU lastImu_; // only used for initialization
 	int imagesProcessed_;

corelib/include/rtabmap/core/Parameters.h

@@ -336,6 +336,7 @@ class RTABMAP_EXP Parameters
     RTABMAP_PARAM(RGBD, NewMapOdomChangeDistance, float, 0,    "A new map is created if a change of odometry translation greater than X m is detected (0 m = disabled).");
     RTABMAP_PARAM(RGBD, OptimizeFromGraphEnd,     bool, false, "Optimize graph from the newest node. If false, the graph is optimized from the oldest node of the current graph (this adds an overhead computation to detect to oldest node of the current graph, but it can be useful to preserve the map referential from the oldest node). Warning when set to false: when some nodes are transferred, the first referential of the local map may change, resulting in momentary changes in robot/map position (which are annoying in teleoperation).");
     RTABMAP_PARAM(RGBD, OptimizeMaxError,         float, 1.0,   uFormat("Reject loop closures if optimization error ratio is greater than this value (0=disabled). Ratio is computed as absolute error over standard deviation of each link. This will help to detect when a wrong loop closure is added to the graph. Not compatible with \"%s\" if enabled.", kOptimizerRobust().c_str()));
+    RTABMAP_PARAM(RGBD, SavedLocalizationIgnored, bool, false, "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.");
     RTABMAP_PARAM(RGBD, GoalReachedRadius,        float, 0.5,  "Goal reached radius (m).");
     RTABMAP_PARAM(RGBD, PlanStuckIterations,      int, 0,      "Mark the current goal node on the path as unreachable if it is not updated after X iterations (0=disabled). If all upcoming nodes on the path are unreachabled, the plan fails.");
     RTABMAP_PARAM(RGBD, PlanLinearVelocity,       float, 0,    "Linear velocity (m/sec) used to compute path weights.");
@@ -550,11 +551,19 @@ class RTABMAP_EXP Parameters
     RTABMAP_PARAM(Icp, MaxRotation,               float, 0.78,  "Maximum ICP rotation correction accepted (rad).");
     RTABMAP_PARAM(Icp, VoxelSize,                 float, 0.0,   "Uniform sampling voxel size (0=disabled).");
     RTABMAP_PARAM(Icp, DownsamplingStep,          int, 1,       "Downsampling step size (1=no sampling). This is done before uniform sampling.");
+#ifdef RTABMAP_POINTMATCHER
     RTABMAP_PARAM(Icp, MaxCorrespondenceDistance, float, 0.1,   "Max distance for point correspondences.");
+#else
+    RTABMAP_PARAM(Icp, MaxCorrespondenceDistance, float, 0.05,  "Max distance for point correspondences.");
+#endif
     RTABMAP_PARAM(Icp, Iterations,                int, 30,      "Max iterations.");
     RTABMAP_PARAM(Icp, Epsilon,                   float, 0,     "Set the transformation epsilon (maximum allowable difference between two consecutive transformations) in order for an optimization to be considered as having converged to the final solution.");
     RTABMAP_PARAM(Icp, CorrespondenceRatio,       float, 0.1,   "Ratio of matching correspondences to accept the transform.");
+#ifdef RTABMAP_POINTMATCHER
+    RTABMAP_PARAM(Icp, PointToPlane,              bool, true,   "Use point to plane ICP.");
+#else
     RTABMAP_PARAM(Icp, PointToPlane,              bool, false,  "Use point to plane ICP.");
+#endif
     RTABMAP_PARAM(Icp, PointToPlaneK,             int, 5,       "Number of neighbors to compute normals for point to plane if the cloud doesn't have already normals.");
     RTABMAP_PARAM(Icp, PointToPlaneRadius,        float, 1.0,   "Search radius to compute normals for point to plane if the cloud doesn't have already normals.");
     RTABMAP_PARAM(Icp, PointToPlaneMinComplexity, float, 0.02,  "Minimum structural complexity (0.0=low, 1.0=high) of the scan to do point to plane registration, otherwise point to point registration is done instead.");
@@ -621,8 +630,7 @@ class RTABMAP_EXP Parameters
     RTABMAP_PARAM(Grid, GroundIsObstacle,           bool,   false,   uFormat("[%s=true] Ground segmentation (%s) is ignored, all points are obstacles. Use this only if you want an OctoMap with ground identified as an obstacle (e.g., with an UAV).", kGrid3D().c_str(), kGridNormalsSegmentation().c_str()));
     RTABMAP_PARAM(Grid, NoiseFilteringRadius,       float,   0.0,    "Noise filtering radius (0=disabled). Done after segmentation.");
     RTABMAP_PARAM(Grid, NoiseFilteringMinNeighbors, int,     5,      "Noise filtering minimum neighbors.");
-    RTABMAP_PARAM(Grid, Scan2dUnknownSpaceFilled,   bool,    false,  "Unknown space filled. Only used with 2D laser scans.");
+    RTABMAP_PARAM(Grid, Scan2dUnknownSpaceFilled,   bool,    false,  uFormat("Unknown space filled. Only used with 2D laser scans. Use %s to set maximum range if laser scan max range is to set.", kGridRangeMax().c_str()));
-    RTABMAP_PARAM(Grid, Scan2dMaxFilledRange,       float,   4.0,    "Unknown space filled maximum range. If 0, the laser scan maximum range is used.");
     RTABMAP_PARAM(Grid, RayTracing,                 bool,   false,   uFormat("Ray tracing is done for each occupied cell, filling unknown space between the sensor and occupied cells. If %s=true, RTAB-Map should be built with OctoMap support, otherwise 3D ray tracing is ignored.", kGrid3D().c_str()));
 
     RTABMAP_PARAM(GridGlobal, FullUpdate,           bool,   true,    "When the graph is changed, the whole map will be reconstructed instead of moving individually each cells of the map. Also, data added to cache won't be released after updating the map. This process is longer but more robust to drift that would erase some parts of the map when it should not.");

corelib/include/rtabmap/core/Rtabmap.h

@@ -129,6 +129,7 @@ public:
 	float getTimeThreshold() const {return _maxTimeAllowed;} // in ms
 	void setTimeThreshold(float maxTimeAllowed); // in ms
 
+	void setInitialPose(const Transform & initialPose);
 	int triggerNewMap();
 	bool labelLocation(int id, const std::string & label);
 	/**
@@ -253,6 +254,7 @@ private:
 	int _pathStuckIterations;
 	float _pathLinearVelocity;
 	float _pathAngularVelocity;
+	bool _savedLocalizationIgnored;
 
 	std::pair<int, float> _loopClosureHypothesis;
 	std::pair<int, float> _highestHypothesis;

corelib/include/rtabmap/core/util3d.h

@@ -228,6 +228,7 @@ cv::Mat RTABMAP_EXP laserScan2dFromPointCloud(const pcl::PointCloud<pcl::PointXY
 cv::Mat RTABMAP_EXP laserScan2dFromPointCloud(const pcl::PointCloud<pcl::PointXYZINormal> & cloud, const Transform & transform = Transform(), bool filterNaNs = true);
 cv::Mat RTABMAP_EXP laserScan2dFromPointCloud(const pcl::PointCloud<pcl::PointXYZI> & cloud, const pcl::PointCloud<pcl::Normal> & normals, const Transform & transform = Transform(), bool filterNaNs = true);
 
+pcl::PCLPointCloud2::Ptr RTABMAP_EXP laserScanToPointCloud2(const LaserScan & laserScan, const Transform & transform = Transform());
 // For 2d laserScan, z is set to null.
 pcl::PointCloud<pcl::PointXYZ>::Ptr RTABMAP_EXP laserScanToPointCloud(const LaserScan & laserScan, const Transform & transform = Transform());
 // For laserScan without normals, normals are set to null.

corelib/include/rtabmap/core/util3d_mapping.h

@@ -114,7 +114,8 @@ void RTABMAP_EXP rayTrace(const cv::Point2i & start,
 		cv::Mat & grid,
 		bool stopOnObstacle);
 
-cv::Mat RTABMAP_EXP convertMap2Image8U(const cv::Mat & map8S);
+cv::Mat RTABMAP_EXP convertMap2Image8U(const cv::Mat & map8S, bool pgmFormat = false);
+cv::Mat RTABMAP_EXP convertImage8U2Map(const cv::Mat & map8U, bool pgmFormat = false);
 
 cv::Mat RTABMAP_EXP erodeMap(const cv::Mat & map);
 

corelib/include/rtabmap/core/util3d_surface.h

@@ -175,6 +175,15 @@ pcl::TextureMesh::Ptr RTABMAP_EXP concatenateTextureMeshes(
 void RTABMAP_EXP concatenateTextureMaterials(
 		pcl::TextureMesh & mesh, const cv::Size & imageSize, int textureSize, int maxTextures, float & scale, std::vector<bool> * materialsKept=0);
 
+std::vector<std::vector<unsigned int> > RTABMAP_EXP convertPolygonsFromPCL(
+		const std::vector<pcl::Vertices> & polygons);
+std::vector<std::vector<std::vector<unsigned int> > > RTABMAP_EXP convertPolygonsFromPCL(
+		const std::vector<std::vector<pcl::Vertices> > & polygons);
+std::vector<pcl::Vertices> RTABMAP_EXP convertPolygonsToPCL(
+		const std::vector<std::vector<unsigned int> > & polygons);
+std::vector<std::vector<pcl::Vertices> > RTABMAP_EXP convertPolygonsToPCL(
+		const std::vector<std::vector<std::vector<unsigned int> > > & tex_polygons);
+
 pcl::TextureMesh::Ptr RTABMAP_EXP assembleTextureMesh(
 		const cv::Mat & cloudMat,
 		const std::vector<std::vector<std::vector<unsigned int> > > & polygons,
@@ -194,6 +203,24 @@ pcl::PolygonMesh::Ptr RTABMAP_EXP assemblePolygonMesh(
  * Merge all textures in the mesh into "textureCount" textures of size "textureSize".
  * @return merged textures corresponding to new materials set in TextureMesh (height=textureSize, width=textureSize*materials)
  */
+cv::Mat RTABMAP_EXP mergeTextures(
+		pcl::TextureMesh & mesh,
+		const std::map<int, cv::Mat> & images, // raw or compressed, can be empty if memory or dbDriver should be used
+		const std::map<int, CameraModel> & calibrations, // Should match images
+		const Memory * memory = 0,             // Should be set if images are not set
+		const DBDriver * dbDriver = 0,         // Should be set if images and memory are not set
+		int textureSize = 4096,
+		int textureCount = 1,
+		const std::vector<std::map<int, pcl::PointXY> > & vertexToPixels = std::vector<std::map<int, pcl::PointXY> >(), // needed for parameters below
+		bool gainCompensation = true,
+		float gainBeta = 10.0f,
+		bool gainRGB = true,                 //Do gain compensation on each channel
+		bool blending = true,
+		int blendingDecimation = 0,          //0=auto depending on projected polygon size and texture size
+		int brightnessContrastRatioLow = 0,  //0=disabled, values between 0 and 100
+		int brightnessContrastRatioHigh = 0, //0=disabled, values between 0 and 100
+		bool exposureFusion = false,         //Exposure fusion can be used only with OpenCV3
+		const ProgressState * state = 0);
 cv::Mat RTABMAP_EXP mergeTextures(
 		pcl::TextureMesh & mesh,
 		const std::map<int, cv::Mat> & images, // raw or compressed, can be empty if memory or dbDriver should be used
@@ -213,6 +240,8 @@ cv::Mat RTABMAP_EXP mergeTextures(
 		bool exposureFusion = false,         //Exposure fusion can be used only with OpenCV3
 		const ProgressState * state = 0);
 
+void RTABMAP_EXP fixTextureMeshForVisualization(pcl::TextureMesh & textureMesh);
+
 cv::Mat RTABMAP_EXP computeNormals(
 		const cv::Mat & laserScan,
 		int searchK,

corelib/src/CMakeLists.txt

@@ -96,9 +96,12 @@ IF(OpenCV_VERSION_MAJOR EQUAL 2)
 SET(SRC_FILES
     ${SRC_FILES}
     opencv/Orb.cpp
-    opencv/solvepnp.cpp
 )
 ENDIF(OpenCV_VERSION_MAJOR EQUAL 2)
+SET(SRC_FILES
+    ${SRC_FILES}
+    opencv/solvepnp.cpp
+)
 
 # to get includes in visual studio
 IF(MSVC)
@@ -238,25 +241,6 @@ IF(G2O_FOUND)
 	ENDIF(WITH_VERTIGO)
 ENDIF(G2O_FOUND)
 
-IF(GTSAM_FOUND)
-	IF(GTSAM_INCLUDE_DIR)
-		SET(INCLUDE_DIRS 
-			${GTSAM_INCLUDE_DIR} # place it in front to use Eigen installed by GTSAM
-			${INCLUDE_DIRS} 
-		)
-	ELSE()
-		SET(INCLUDE_DIRS 
-			${GTSAM_INCLUDE_DIRS} # cmake standard
-			${INCLUDE_DIRS} 
-		)
-	ENDIF()
-	add_definitions("-DGTSAM_IMPORT_STATIC")
-	SET(LIBRARIES
-		${LIBRARIES}
-		gtsam
-	)
-ENDIF(GTSAM_FOUND)
-
 IF(cvsba_FOUND)
 	SET(INCLUDE_DIRS 
 		${INCLUDE_DIRS} 
@@ -376,6 +360,29 @@ IF(ORB_SLAM2_FOUND)
 	)
 ENDIF(ORB_SLAM2_FOUND)
 
+IF(GTSAM_FOUND)
+	# Make sure GTSAM is built with system Eigen, not the included one in its package
+	IF(GTSAM_INCLUDE_DIR)
+		SET(INCLUDE_DIRS 
+			${INCLUDE_DIRS} 
+			${GTSAM_INCLUDE_DIR} 
+		)
+	ELSE()
+		SET(INCLUDE_DIRS 
+			${INCLUDE_DIRS} 
+			${GTSAM_INCLUDE_DIRS}
+		)
+	ENDIF()
+	IF(WIN32)
+		# GTSAM should be built in STATIC on Windows to avoid "error C2338: THIS_METHOD_IS_ONLY_FOR_1x1_EXPRESSIONS" when building GTSAM
+		add_definitions("-DGTSAM_IMPORT_STATIC") 
+	ENDIF(WIN32)
+	SET(LIBRARIES
+		${LIBRARIES}
+		gtsam # Windows: Place static libs at the end
+	)
+ENDIF(GTSAM_FOUND)
+
 ####################################
 # Generate resources files
 ####################################

corelib/src/CameraThread.cpp

@@ -325,7 +325,7 @@ void CameraThread::postUpdate(SensorData * dataPtr, CameraInfo * info) const
 		!data.depthRaw().empty())
 	{
 		UDEBUG("");
-		if(data.laserScanRaw().size())
+		if(data.laserScanRaw().isEmpty())
 		{
 			UASSERT(_scanDecimation >= 1);
 			UTimer timer;

corelib/src/DBDriver.cpp

@@ -1061,9 +1061,37 @@ cv::Mat DBDriver::loadPreviewImage() const
 	return image;
 }
 
+void DBDriver::saveOptimizedPoses(const std::map<int, Transform> & optimizedPoses, const Transform & lastlocalizationPose) const
+{
+	_dbSafeAccessMutex.lock();
+	saveOptimizedPosesQuery(optimizedPoses, lastlocalizationPose);
+	_dbSafeAccessMutex.unlock();
+}
+std::map<int, Transform> DBDriver::loadOptimizedPoses(Transform * lastlocalizationPose) const
+{
+	_dbSafeAccessMutex.lock();
+	std::map<int, Transform> poses = loadOptimizedPosesQuery(lastlocalizationPose);
+	_dbSafeAccessMutex.unlock();
+	return poses;
+}
+
+void DBDriver::save2DMap(const cv::Mat & map, float xMin, float yMin, float cellSize) const
+{
+	_dbSafeAccessMutex.lock();
+	save2DMapQuery(map, xMin, yMin, cellSize);
+	_dbSafeAccessMutex.unlock();
+}
+
+cv::Mat DBDriver::load2DMap(float & xMin, float & yMin, float & cellSize) const
+{
+	_dbSafeAccessMutex.lock();
+	cv::Mat map = load2DMapQuery(xMin, yMin, cellSize);
+	_dbSafeAccessMutex.unlock();
+	return map;
+}
+
 void DBDriver::saveOptimizedMesh(
 			const cv::Mat & cloud,
-			const std::map<int, Transform> & poses,
 			const std::vector<std::vector<std::vector<unsigned int> > > & polygons,
 #if PCL_VERSION_COMPARE(>=, 1, 8, 0)
 			const std::vector<std::vector<Eigen::Vector2f, Eigen::aligned_allocator<Eigen::Vector2f> > > & texCoords,
@@ -1073,12 +1101,11 @@ void DBDriver::saveOptimizedMesh(
 			const cv::Mat & textures) const
 {
 	_dbSafeAccessMutex.lock();
-	saveOptimizedMeshQuery(cloud, poses, polygons, texCoords, textures);
+	saveOptimizedMeshQuery(cloud, polygons, texCoords, textures);
 	_dbSafeAccessMutex.unlock();
 }
 
 cv::Mat DBDriver::loadOptimizedMesh(
-				std::map<int, Transform> * poses,
 				std::vector<std::vector<std::vector<unsigned int> > > * polygons,
 #if PCL_VERSION_COMPARE(>=, 1, 8, 0)
 				std::vector<std::vector<Eigen::Vector2f, Eigen::aligned_allocator<Eigen::Vector2f> > > * texCoords,
@@ -1088,7 +1115,7 @@ cv::Mat DBDriver::loadOptimizedMesh(
 				cv::Mat * textures) const
 {
 	_dbSafeAccessMutex.lock();
-	cv::Mat cloud = loadOptimizedMeshQuery(poses, polygons, texCoords, textures);
+	cv::Mat cloud = loadOptimizedMeshQuery(polygons, texCoords, textures);
 	_dbSafeAccessMutex.unlock();
 	return cloud;
 }

corelib/src/DBDriverSqlite3.cpp

@@ -4147,16 +4147,19 @@ void DBDriverSqlite3::addStatisticsQuery(const Statistics & statistics) const
 				}
 
 				cv::Mat compressedWmState;
-				if(uStrNumCmp(this->getDatabaseVersion(), "0.16.2") >= 0 && !statistics.wmState().empty())
+				if(uStrNumCmp(this->getDatabaseVersion(), "0.16.2") >= 0)
 				{
-					compressedWmState = compressData2(cv::Mat(1, statistics.wmState().size(), CV_32SC1, (void *)statistics.wmState().data()));
+					if(!statistics.wmState().empty())
-					rc = sqlite3_bind_blob(ppStmt, index++, compressedWmState.data, compressedWmState.cols, SQLITE_STATIC);
+					{
-					UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
+						compressedWmState = compressData2(cv::Mat(1, statistics.wmState().size(), CV_32SC1, (void *)statistics.wmState().data()));
-				}
+						rc = sqlite3_bind_blob(ppStmt, index++, compressedWmState.data, compressedWmState.cols, SQLITE_STATIC);
-				else
+						UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
-				{
+					}
-					rc = sqlite3_bind_null(ppStmt, index++);
+					else
-					UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
+					{
+						rc = sqlite3_bind_null(ppStmt, index++);
+						UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
+					}
 				}
 
 				//step
@@ -4277,9 +4280,274 @@ cv::Mat DBDriverSqlite3::loadPreviewImageQuery() const
 	return image;
 }
 
+void DBDriverSqlite3::saveOptimizedPosesQuery(const std::map<int, Transform> & poses, const Transform & lastlocalizationPose) const
+{
+	UDEBUG("");
+	if(_ppDb && uStrNumCmp(_version, "0.17.0") >= 0)
+	{
+		UTimer timer;
+		timer.start();
+		int rc = SQLITE_OK;
+		sqlite3_stmt * ppStmt = 0;
+		std::string query;
+
+		// Update table Admin
+		query = uFormat("UPDATE Admin SET opt_ids=?, opt_poses=?, opt_last_localization=?, time_enter = DATETIME('NOW') WHERE version='%s';", _version.c_str());
+		rc = sqlite3_prepare_v2(_ppDb, query.c_str(), -1, &ppStmt, 0);
+		UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
+
+		int index = 1;
+
+		// opt ids and poses
+		cv::Mat compressedIds;
+		cv::Mat compressedPoses;
+		if(poses.empty())
+		{
+			rc = sqlite3_bind_null(ppStmt, index++);
+			UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
+			rc = sqlite3_bind_null(ppStmt, index++);
+			UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
+		}
+		else
+		{
+			std::vector<int> serializedIds(poses.size());
+			std::vector<float> serializedPoses(poses.size()*12);
+			int i=0;
+			for(std::map<int, Transform>::const_iterator iter=poses.begin(); iter!=poses.end(); ++iter)
+			{
+				serializedIds[i] = iter->first;
+				memcpy(serializedPoses.data()+(12*i), iter->second.data(), 12*sizeof(float));
+				++i;
+			}
+
+			compressedIds = compressData2(cv::Mat(1,serializedIds.size(), CV_32SC1, serializedIds.data()));
+			compressedPoses = compressData2(cv::Mat(1,serializedPoses.size(), CV_32FC1, serializedPoses.data()));
+
+			rc = sqlite3_bind_blob(ppStmt, index++, compressedIds.data, compressedIds.cols, SQLITE_STATIC);
+			UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
+			rc = sqlite3_bind_blob(ppStmt, index++, compressedPoses.data, compressedPoses.cols, SQLITE_STATIC);
+			UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
+		}
+
+		if(lastlocalizationPose.isNull())
+		{
+			rc = sqlite3_bind_null(ppStmt, index++);
+			UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
+		}
+		else
+		{
+			UDEBUG("lastlocalizationPose=%s", lastlocalizationPose.prettyPrint().c_str());
+			rc = sqlite3_bind_blob(ppStmt, index++, lastlocalizationPose.data(), lastlocalizationPose.size()*sizeof(float), SQLITE_STATIC);
+			UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
+		}
+
+		//execute query
+		rc=sqlite3_step(ppStmt);
+		UASSERT_MSG(rc == SQLITE_DONE, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
+
+		// Finalize (delete) the statement
+		rc = sqlite3_finalize(ppStmt);
+		UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
+
+		UDEBUG("Time=%fs", timer.ticks());
+	}
+}
+
+std::map<int, Transform> DBDriverSqlite3::loadOptimizedPosesQuery(Transform * lastlocalizationPose) const
+{
+	UDEBUG("");
+	std::map<int, Transform> poses;
+	if(_ppDb && uStrNumCmp(_version, "0.17.0") >= 0)
+	{
+		UTimer timer;
+		timer.start();
+		int rc = SQLITE_OK;
+		sqlite3_stmt * ppStmt = 0;
+		std::stringstream query;
+
+		query << "SELECT opt_ids, opt_poses, opt_last_localization "
+			  << "FROM Admin "
+			  << "WHERE version='" << _version.c_str()
+			  <<"';";
+
+		rc = sqlite3_prepare_v2(_ppDb, query.str().c_str(), -1, &ppStmt, 0);
+		UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
+
+		// Process the result if one
+		rc = sqlite3_step(ppStmt);
+		UASSERT_MSG(rc == SQLITE_ROW, uFormat("DB error (%s): Not found first Admin row: query=\"%s\"", _version.c_str(), query.str().c_str()).c_str());
+		if(rc == SQLITE_ROW)
+		{
+			const void * data = 0;
+			int dataSize = 0;
+			int index = 0;
+
+			//opt_poses
+			cv::Mat serializedIds;
+			data = sqlite3_column_blob(ppStmt, index);
+			dataSize = sqlite3_column_bytes(ppStmt, index++);
+			if(dataSize>0 && data)
+			{
+				serializedIds = uncompressData(cv::Mat(1, dataSize, CV_8UC1, (void *)data));
+				UDEBUG("serializedIds=%d", serializedIds.cols);
+			}
+
+			data = sqlite3_column_blob(ppStmt, index);
+			dataSize = sqlite3_column_bytes(ppStmt, index++);
+			if(dataSize>0 && data)
+			{
+				cv::Mat serializedPoses = uncompressData(cv::Mat(1, dataSize, CV_8UC1, (void *)data));
+				UDEBUG("serializedPoses=%d", serializedPoses.cols);
+
+				UASSERT(serializedIds.cols == serializedPoses.cols/12);
+				UASSERT(serializedPoses.type() == CV_32FC1);
+				UASSERT(serializedIds.type() == CV_32SC1);
+				for(int i=0; i<serializedIds.cols; ++i)
+				{
+					Transform t(serializedPoses.at<float>(i*12), serializedPoses.at<float>(i*12+1), serializedPoses.at<float>(i*12+2), serializedPoses.at<float>(i*12+3),
+							serializedPoses.at<float>(i*12+4), serializedPoses.at<float>(i*12+5), serializedPoses.at<float>(i*12+6), serializedPoses.at<float>(i*12+7),
+							serializedPoses.at<float>(i*12+8), serializedPoses.at<float>(i*12+9), serializedPoses.at<float>(i*12+10), serializedPoses.at<float>(i*12+11));
+					poses.insert(std::make_pair(serializedIds.at<int>(i), t));
+					UDEBUG("Optimized pose %d: %s", serializedIds.at<int>(i), t.prettyPrint().c_str());
+				}
+			}
+
+			data = sqlite3_column_blob(ppStmt, index); // ground_truth_pose
+			dataSize = sqlite3_column_bytes(ppStmt, index++);
+			if(lastlocalizationPose)
+			{
+				if((unsigned int)dataSize == lastlocalizationPose->size()*sizeof(float) && data)
+				{
+					memcpy(lastlocalizationPose->data(), data, dataSize);
+				}
+				UDEBUG("lastlocalizationPose=%s", lastlocalizationPose->prettyPrint().c_str());
+			}
+
+			rc = sqlite3_step(ppStmt); // next result...
+		}
+		UASSERT_MSG(rc == SQLITE_DONE, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
+
+		// Finalize (delete) the statement
+		rc = sqlite3_finalize(ppStmt);
+		UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
+		ULOGGER_DEBUG("Time=%fs", timer.ticks());
+
+	}
+	return poses;
+}
+
+void DBDriverSqlite3::save2DMapQuery(const cv::Mat & map, float xMin, float yMin, float cellSize) const
+{
+	UDEBUG("");
+	if(_ppDb && uStrNumCmp(_version, "0.17.0") >= 0)
+	{
+		UTimer timer;
+		timer.start();
+		int rc = SQLITE_OK;
+		sqlite3_stmt * ppStmt = 0;
+		std::string query;
+
+		// Update table Admin
+		query = uFormat("UPDATE Admin SET opt_map=?, opt_map_x_min=?, opt_map_y_min=?, opt_map_resolution=?, time_enter = DATETIME('NOW') WHERE version='%s';", _version.c_str());
+		rc = sqlite3_prepare_v2(_ppDb, query.c_str(), -1, &ppStmt, 0);
+		UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
+
+		int index = 1;
+
+		// opt ids and poses
+		cv::Mat compressedMap;
+		if(map.empty())
+		{
+			rc = sqlite3_bind_null(ppStmt, index++);
+			UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
+		}
+		else
+		{
+			compressedMap = compressData2(map);
+
+			rc = sqlite3_bind_blob(ppStmt, index++, compressedMap.data, compressedMap.cols, SQLITE_STATIC);
+			UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
+		}
+
+		rc = sqlite3_bind_double(ppStmt, index++, xMin);
+		UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
+		rc = sqlite3_bind_double(ppStmt, index++, yMin);
+		UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
+		rc = sqlite3_bind_double(ppStmt, index++, cellSize);
+		UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
+
+		//execute query
+		rc=sqlite3_step(ppStmt);
+		UASSERT_MSG(rc == SQLITE_DONE, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
+
+		// Finalize (delete) the statement
+		rc = sqlite3_finalize(ppStmt);
+		UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
+
+		UDEBUG("Time=%fs", timer.ticks());
+	}
+}
+
+cv::Mat DBDriverSqlite3::load2DMapQuery(float & xMin, float & yMin, float & cellSize) const
+{
+	UDEBUG("");
+	cv::Mat map;
+	if(_ppDb && uStrNumCmp(_version, "0.17.0") >= 0)
+	{
+		UTimer timer;
+		timer.start();
+		int rc = SQLITE_OK;
+		sqlite3_stmt * ppStmt = 0;
+		std::stringstream query;
+
+		query << "SELECT  opt_map, opt_map_x_min, opt_map_y_min, opt_map_resolution "
+			  << "FROM Admin "
+			  << "WHERE version='" << _version.c_str()
+			  <<"';";
+
+		rc = sqlite3_prepare_v2(_ppDb, query.str().c_str(), -1, &ppStmt, 0);
+		UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
+
+		// Process the result if one
+		rc = sqlite3_step(ppStmt);
+		UASSERT_MSG(rc == SQLITE_ROW, uFormat("DB error (%s): Not found first Admin row: query=\"%s\"", _version.c_str(), query.str().c_str()).c_str());
+		if(rc == SQLITE_ROW)
+		{
+			const void * data = 0;
+			int dataSize = 0;
+			int index = 0;
+
+			//opt_map
+			data = sqlite3_column_blob(ppStmt, index);
+			dataSize = sqlite3_column_bytes(ppStmt, index++);
+			if(dataSize>0 && data)
+			{
+				map = uncompressData(cv::Mat(1, dataSize, CV_8UC1, (void *)data));
+				UDEBUG("map=%d/%d", map.cols, map.rows);
+			}
+
+			xMin = sqlite3_column_double(ppStmt, index++);
+			UDEBUG("xMin=%f", xMin);
+			yMin = sqlite3_column_double(ppStmt, index++);
+			UDEBUG("yMin=%f", yMin);
+			cellSize = sqlite3_column_double(ppStmt, index++);
+			UDEBUG("cellSize=%f", cellSize);
+
+			rc = sqlite3_step(ppStmt); // next result...
+		}
+		UASSERT_MSG(rc == SQLITE_DONE, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
+
+		// Finalize (delete) the statement
+		rc = sqlite3_finalize(ppStmt);
+		UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
+		ULOGGER_DEBUG("Time=%fs", timer.ticks());
+
+	}
+	return map;
+}
+
 void DBDriverSqlite3::saveOptimizedMeshQuery(
 			const cv::Mat & cloud,
-			const std::map<int, Transform> & poses,
 			const std::vector<std::vector<std::vector<unsigned int> > > & polygons,
 #if PCL_VERSION_COMPARE(>=, 1, 8, 0)
 			const std::vector<std::vector<Eigen::Vector2f, Eigen::aligned_allocator<Eigen::Vector2f> > > & texCoords,
@@ -4298,14 +4566,14 @@ void DBDriverSqlite3::saveOptimizedMeshQuery(
 		std::string query;
 
 		// Update table Admin
-		query = uFormat("UPDATE Admin SET opt_cloud=?, opt_ids=?, opt_poses=?, opt_polygons_size=?, opt_polygons=?, opt_tex_coords=?, opt_tex_materials=?, time_enter = DATETIME('NOW') WHERE version='%s';", _version.c_str());
+		query = uFormat("UPDATE Admin SET opt_cloud=?, opt_polygons_size=?, opt_polygons=?, opt_tex_coords=?, opt_tex_materials=?, time_enter = DATETIME('NOW') WHERE version='%s';", _version.c_str());
 		rc = sqlite3_prepare_v2(_ppDb, query.c_str(), -1, &ppStmt, 0);
 		UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
 
 		if(cloud.empty())
 		{
 			// set all fields to null
-			for(int i=1; i<=7; ++i)
+			for(int i=1; i<=5; ++i)
 			{
 				rc = sqlite3_bind_null(ppStmt, i);
 				UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
@@ -4336,39 +4604,9 @@ void DBDriverSqlite3::saveOptimizedMeshQuery(
 			UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
 
 			// opt ids and poses
-			cv::Mat compressedIds;
-			cv::Mat compressedPoses;
 			cv::Mat compressedPolygons;
 			cv::Mat compressedTexCoords;
 			cv::Mat compressedTextures;
-			if(poses.empty())
-			{
-				rc = sqlite3_bind_null(ppStmt, index++);
-				UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
-				rc = sqlite3_bind_null(ppStmt, index++);
-				UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
-			}
-			else
-			{
-				std::vector<int> serializedIds(poses.size());
-				std::vector<float> serializedPoses(poses.size()*12);
-				int i=0;
-				for(std::map<int, Transform>::const_iterator iter=poses.begin(); iter!=poses.end(); ++iter)
-				{
-					serializedIds[i] = iter->first;
-					memcpy(serializedPoses.data()+(12*i), iter->second.data(), 12*sizeof(float));
-					++i;
-				}
-
-				compressedIds = compressData2(cv::Mat(1,serializedIds.size(), CV_32SC1, serializedIds.data()));
-				compressedPoses = compressData2(cv::Mat(1,serializedPoses.size(), CV_32FC1, serializedPoses.data()));
-
-				rc = sqlite3_bind_blob(ppStmt, index++, compressedIds.data, compressedIds.cols, SQLITE_STATIC);
-				UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
-				rc = sqlite3_bind_blob(ppStmt, index++, compressedPoses.data, compressedPoses.cols, SQLITE_STATIC);
-				UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
-			}
-
 			// polygons
 			if(polygons.empty())
 			{
@@ -4505,7 +4743,6 @@ void DBDriverSqlite3::saveOptimizedMeshQuery(
 }
 
 cv::Mat DBDriverSqlite3::loadOptimizedMeshQuery(
-			std::map<int, Transform> * poses,
 			std::vector<std::vector<std::vector<unsigned int> > > * polygons,
 #if PCL_VERSION_COMPARE(>=, 1, 8, 0)
 			std::vector<std::vector<Eigen::Vector2f, Eigen::aligned_allocator<Eigen::Vector2f> > > * texCoords,
@@ -4524,7 +4761,7 @@ cv::Mat DBDriverSqlite3::loadOptimizedMeshQuery(
 		sqlite3_stmt * ppStmt = 0;
 		std::stringstream query;
 
-		query << "SELECT opt_cloud, opt_ids, opt_poses, opt_polygons_size, opt_polygons, opt_tex_coords, opt_tex_materials "
+		query << "SELECT opt_cloud, opt_polygons_size, opt_polygons, opt_tex_coords, opt_tex_materials "
 			  << "FROM Admin "
 			  << "WHERE version='" << _version.c_str()
 			  <<"';";
@@ -4550,39 +4787,6 @@ cv::Mat DBDriverSqlite3::loadOptimizedMeshQuery(
 			}
 			UDEBUG("Cloud=%d points", cloud.cols);
 
-			//opt_poses
-			cv::Mat serializedIds;
-			data = sqlite3_column_blob(ppStmt, index);
-			dataSize = sqlite3_column_bytes(ppStmt, index++);
-			if(dataSize>0 && data)
-			{
-				serializedIds = uncompressData(cv::Mat(1, dataSize, CV_8UC1, (void *)data));
-				UDEBUG("serializedIds=%d", serializedIds.cols);
-			}
-
-			data = sqlite3_column_blob(ppStmt, index);
-			dataSize = sqlite3_column_bytes(ppStmt, index++);
-			if(dataSize>0 && data)
-			{
-				cv::Mat serializedPoses = uncompressData(cv::Mat(1, dataSize, CV_8UC1, (void *)data));
-				UDEBUG("serializedPoses=%d", serializedPoses.cols);
-
-				if(poses)
-				{
-					UASSERT(serializedIds.cols == serializedPoses.cols/12);
-					UASSERT(serializedPoses.type() == CV_32FC1);
-					UASSERT(serializedIds.type() == CV_32SC1);
-					for(int i=0; i<serializedIds.cols; ++i)
-					{
-						Transform t(serializedPoses.at<float>(i*12), serializedPoses.at<float>(i*12+1), serializedPoses.at<float>(i*12+2), serializedPoses.at<float>(i*12+3),
-								serializedPoses.at<float>(i*12+4), serializedPoses.at<float>(i*12+5), serializedPoses.at<float>(i*12+6), serializedPoses.at<float>(i*12+7),
-								serializedPoses.at<float>(i*12+8), serializedPoses.at<float>(i*12+9), serializedPoses.at<float>(i*12+10), serializedPoses.at<float>(i*12+11));
-						poses->insert(std::make_pair(serializedIds.at<int>(i), t));
-						UDEBUG("Optimized pose %d: %s", serializedIds.at<int>(i), t.prettyPrint().c_str());
-					}
-				}
-			}
-
 			//opt_polygons_size
 			int polygonSize = sqlite3_column_int(ppStmt, index++);
 			UDEBUG("polygonSize=%d", polygonSize);

corelib/src/DBDriverSqlite3.h

@@ -101,9 +101,12 @@ private:
 	virtual void addStatisticsQuery(const Statistics & statistics) const;
 	virtual void savePreviewImageQuery(const cv::Mat & image) const;
 	virtual cv::Mat loadPreviewImageQuery() const;
+	virtual void saveOptimizedPosesQuery(const std::map<int, Transform> & optimizedPoses, const Transform & lastlocalizationPose) const;
+	virtual std::map<int, Transform> loadOptimizedPosesQuery(Transform * lastlocalizationPose) const;
+	virtual void save2DMapQuery(const cv::Mat & map, float xMin, float yMin, float cellSize) const;
+	virtual cv::Mat load2DMapQuery(float & xMin, float & yMin, float & cellSize) const;
 	virtual void saveOptimizedMeshQuery(
 			const cv::Mat & cloud,
-			const std::map<int, Transform> & poses,
 			const std::vector<std::vector<std::vector<unsigned int> > > & polygons,
 #if PCL_VERSION_COMPARE(>=, 1, 8, 0)
 			const std::vector<std::vector<Eigen::Vector2f, Eigen::aligned_allocator<Eigen::Vector2f> > > & texCoords,
@@ -112,7 +115,6 @@ private:
 #endif
 			const cv::Mat & textures) const;
 	virtual cv::Mat loadOptimizedMeshQuery(
-			std::map<int, Transform> * poses,
 			std::vector<std::vector<std::vector<unsigned int> > > * polygons,
 #if PCL_VERSION_COMPARE(>=, 1, 8, 0)
 			std::vector<std::vector<Eigen::Vector2f, Eigen::aligned_allocator<Eigen::Vector2f> > > * texCoords,

corelib/src/Memory.cpp

@@ -126,7 +126,7 @@ Memory::Memory(const ParametersMap & parameters) :
 	{
 		UWARN(	"%s is >=0.5, which sets correspondence ratio for proximity detection using "
 			"laser scans to 100% (2 x Ratio). You may lower the ratio to accept proximity "
-			"detection with not full scans overlapping.");
+			"detection with not full scans overlapping.", Parameters::kIcpCorrespondenceRatio().c_str());
 	}
 	_registrationIcpMulti = new RegistrationIcp(paramsMulti);
 
@@ -361,7 +361,7 @@ void Memory::close(bool databaseSaved, bool postInitClosingEvents, const std::st
 	if(postInitClosingEvents) UEventsManager::post(new RtabmapEventInit(RtabmapEventInit::kClosing));
 
 	bool databaseNameChanged = false;
-	if(databaseSaved)
+	if(databaseSaved && _dbDriver)
 	{
 		databaseNameChanged = ouputDatabasePath.size() && _dbDriver->getUrl().size() && _dbDriver->getUrl().compare(ouputDatabasePath) != 0?true:false;
 	}
@@ -584,7 +584,7 @@ void Memory::parseParameters(const ParametersMap & parameters)
 			{
 				UWARN(	"%s is >=0.5, which sets correspondence ratio for proximity detection using "
 					"laser scans to 100% (2 x Ratio). You may lower the ratio to accept proximity "
-					"detection with not full scans overlapping.");
+					"detection with not full scans overlapping.", Parameters::kIcpCorrespondenceRatio().c_str());
 			}
 			_registrationIcpMulti->parseParameters(paramsMulti);
 		}
@@ -1756,9 +1756,42 @@ cv::Mat Memory::loadPreviewImage() const
 	return cv::Mat();
 }
 
+void Memory::saveOptimizedPoses(const std::map<int, Transform> & optimizedPoses, const Transform & lastlocalizationPose) const
+{
+	if(_dbDriver)
+	{
+		_dbDriver->saveOptimizedPoses(optimizedPoses, lastlocalizationPose);
+	}
+}
+
+std::map<int, Transform> Memory::loadOptimizedPoses(Transform * lastlocalizationPose) const
+{
+	if(_dbDriver)
+	{
+		return _dbDriver->loadOptimizedPoses(lastlocalizationPose);
+	}
+	return std::map<int, Transform>();
+}
+
+void Memory::save2DMap(const cv::Mat & map, float xMin, float yMin, float cellSize) const
+{
+	if(_dbDriver)
+	{
+		_dbDriver->save2DMap(map, xMin, yMin, cellSize);
+	}
+}
+
+cv::Mat Memory::load2DMap(float & xMin, float & yMin, float & cellSize) const
+{
+	if(_dbDriver)
+	{
+		return _dbDriver->load2DMap(xMin, yMin, cellSize);
+	}
+	return cv::Mat();
+}
+
 void Memory::saveOptimizedMesh(
 		const cv::Mat & cloud,
-		const std::map<int, Transform> & poses,
 		const std::vector<std::vector<std::vector<unsigned int> > > & polygons,
 #if PCL_VERSION_COMPARE(>=, 1, 8, 0)
 		const std::vector<std::vector<Eigen::Vector2f, Eigen::aligned_allocator<Eigen::Vector2f> > > & texCoords,
@@ -1769,12 +1802,11 @@ void Memory::saveOptimizedMesh(
 {
 	if(_dbDriver)
 	{
-		_dbDriver->saveOptimizedMesh(cloud, poses, polygons, texCoords, textures);
+		_dbDriver->saveOptimizedMesh(cloud, polygons, texCoords, textures);
 	}
 }
 
 cv::Mat Memory::loadOptimizedMesh(
-			std::map<int, Transform> * poses,
 			std::vector<std::vector<std::vector<unsigned int> > > * polygons,
 #if PCL_VERSION_COMPARE(>=, 1, 8, 0)
 			std::vector<std::vector<Eigen::Vector2f, Eigen::aligned_allocator<Eigen::Vector2f> > > * texCoords,
@@ -1785,7 +1817,7 @@ cv::Mat Memory::loadOptimizedMesh(
 {
 	if(_dbDriver)
 	{
-		return _dbDriver->loadOptimizedMesh(poses, polygons, texCoords, textures);
+		return _dbDriver->loadOptimizedMesh(polygons, texCoords, textures);
 	}
 	return cv::Mat();
 }

corelib/src/OccupancyGrid.cpp

@@ -68,7 +68,6 @@ OccupancyGrid::OccupancyGrid(const ParametersMap & parameters) :
 	noiseFilteringRadius_(Parameters::defaultGridNoiseFilteringRadius()),
 	noiseFilteringMinNeighbors_(Parameters::defaultGridNoiseFilteringMinNeighbors()),
 	scan2dUnknownSpaceFilled_(Parameters::defaultGridScan2dUnknownSpaceFilled()),
-	scan2dMaxUnknownSpaceFilledRange_(Parameters::defaultGridScan2dMaxFilledRange()),
 	rayTracing_(Parameters::defaultGridRayTracing()),
 	fullUpdate_(Parameters::defaultGridGlobalFullUpdate()),
 	minMapSize_(Parameters::defaultGridGlobalMinSize()),
@@ -125,7 +124,6 @@ void OccupancyGrid::parseParameters(const ParametersMap & parameters)
 	Parameters::parse(parameters, Parameters::kGridNoiseFilteringRadius(), noiseFilteringRadius_);
 	Parameters::parse(parameters, Parameters::kGridNoiseFilteringMinNeighbors(), noiseFilteringMinNeighbors_);
 	Parameters::parse(parameters, Parameters::kGridScan2dUnknownSpaceFilled(), scan2dUnknownSpaceFilled_);
-	Parameters::parse(parameters, Parameters::kGridScan2dMaxFilledRange(), scan2dMaxUnknownSpaceFilledRange_);
 	Parameters::parse(parameters, Parameters::kGridRayTracing(), rayTracing_);
 	Parameters::parse(parameters, Parameters::kGridGlobalFullUpdate(), fullUpdate_);
 	Parameters::parse(parameters, Parameters::kGridGlobalMinSize(), minMapSize_);
@@ -197,6 +195,24 @@ void OccupancyGrid::parseParameters(const ParametersMap & parameters)
 	}
 }
 
+void OccupancyGrid::setMap(const cv::Mat & map, float xMin, float yMin, float cellSize, const std::map<int, Transform> & poses)
+{
+	UDEBUG("map=%d/%d xMin=%f yMin=%f cellSize=%f poses=%d",
+			map.cols, map.rows, xMin, yMin, cellSize, (int)poses.size());
+	this->clear();
+	if(!poses.empty() && !map.empty())
+	{
+		UASSERT(cellSize > 0.0f);
+		UASSERT(map.type() == CV_8SC1);
+		map_ = map.clone();
+		mapInfo_ = cv::Mat::zeros(map.size(), CV_32FC3);
+		xMin_ = xMin;
+		yMin_ = yMin;
+		cellSize_ = cellSize;
+		addedNodes_ = poses;
+	}
+}
+
 void OccupancyGrid::setCellSize(float cellSize)
 {
 	UASSERT_MSG(cellSize > 0.0f, uFormat("Param name is \"%s\"", Parameters::kGridCellSize().c_str()).c_str());
@@ -241,11 +257,20 @@ void OccupancyGrid::createLocalMap(
 				node.sensorData().laserScanRaw().localTransform().z());
 
 		LaserScan scan = node.sensorData().laserScanRaw();
-		if(cloudMinDepth_ > 0.0f || cloudMaxDepth_ > 0.0f)
+		if(cloudMinDepth_ > 0.0f)
 		{
-			scan = util3d::rangeFiltering(scan, cloudMinDepth_, cloudMaxDepth_);
+			scan = util3d::rangeFiltering(scan, cloudMinDepth_, 0.0f);
 		}
 
+		float maxRange = cloudMaxDepth_;
+		if(cloudMaxDepth_>0.0f && node.sensorData().laserScanRaw().maxRange()>0.0f)
+		{
+			maxRange = cloudMaxDepth_ < node.sensorData().laserScanRaw().maxRange()?cloudMaxDepth_:node.sensorData().laserScanRaw().maxRange();
+		}
+		else if(scan2dUnknownSpaceFilled_ && node.sensorData().laserScanRaw().maxRange()>0.0f)
+		{
+			maxRange = node.sensorData().laserScanRaw().maxRange();
+		}
 		util3d::occupancy2DFromLaserScan(
 				util3d::transformLaserScan(scan, node.sensorData().laserScanRaw().localTransform()).data(),
 				cv::Mat(),
@@ -254,7 +279,7 @@ void OccupancyGrid::createLocalMap(
 				obstacleCells,
 				cellSize_,
 				scan2dUnknownSpaceFilled_,
-				node.sensorData().laserScanRaw().maxRange()>scan2dMaxUnknownSpaceFilledRange_?scan2dMaxUnknownSpaceFilledRange_:node.sensorData().laserScanRaw().maxRange());
+				maxRange);
 
 		UDEBUG("ground=%d obstacles=%d channels=%d", emptyCells.cols, obstacleCells.cols, obstacleCells.cols?obstacleCells.channels():emptyCells.channels());
 	}
@@ -515,7 +540,7 @@ void OccupancyGrid::createLocalMap(
 					obstacleCells,
 					cellSize_,
 					false, // don't fill unknown space
-					0);
+					cloudMaxDepth_);
 		}
 	}
 	UDEBUG("ground=%d obstacles=%d empty=%d, channels=%d", groundCells.cols, obstacleCells.cols, emptyCells.cols, obstacleCells.cols?obstacleCells.channels():groundCells.channels());
@@ -761,6 +786,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);
 			poses.push_back(*iter);
 		}
 	}
@@ -812,6 +838,8 @@ void OccupancyGrid::update(const std::map<int, Transform> & posesIn)
 			{
 				const std::pair<std::pair<cv::Mat, cv::Mat>, cv::Mat> & pair = cache_.at(iter->first);
 
+				UDEBUG("Adding grid %d: ground=%d obstacles=%d empty=%d", iter->first, pair.first.first.cols, pair.first.second.cols, pair.second.cols);
+
 				//ground
 				if(pair.first.first.cols)
 				{

corelib/src/OdometryOkvis.cpp

@@ -130,10 +130,8 @@ OdometryOkvis::OdometryOkvis(const ParametersMap & parameters) :
 	Odometry(parameters),
 #ifdef RTABMAP_OKVIS
 	okvisCallbackHandler_(new OkvisCallbackHandler),
-#else
-	okvisCallbackHandler_(0),
-#endif
 	okvisEstimator_(0),
+#endif
 	okvisParameters_(parameters),
 	imagesProcessed_(0)
 {

corelib/src/Optimizer.cpp

@@ -279,11 +279,14 @@ std::map<int, Transform> Optimizer::optimizeIncremental(
 	UDEBUG("Incremental optimization... poses=%d comstraints=%d", (int)poses.size(), (int)constraints.size());
 	for(std::map<int, Transform>::const_iterator iter=poses.begin(); iter!=poses.end(); ++iter)
 	{
+		incGraph.insert(*iter);
 		bool hasLoopClosure = false;
 		for(std::multimap<int, Link>::iterator jter=constraintsCpy.lower_bound(iter->first); jter!=constraintsCpy.end() && jter->first==iter->first; ++jter)
 		{
+			UDEBUG("%d: %d -> %d type=%d", iter->first, jter->second.from(), jter->second.to(), jter->second.type());
 			if(jter->second.type() == Link::kNeighbor || jter->second.type() == Link::kNeighborMerged)
 			{
+				UASSERT(uContains(incGraph, iter->first));
 				incGraph.insert(std::make_pair(jter->second.to(), incGraph.at(iter->first) * jter->second.transform()));
 				incGraphLinks.insert(*jter);
 			}
@@ -316,6 +319,8 @@ std::map<int, Transform> Optimizer::optimizeIncremental(
 	if(!incGraph.empty() && incGraph.size() == poses.size())
 	{
 		UASSERT(incGraphLinks.size() == constraints.size());
+		UASSERT(uContains(poses, rootId) && uContains(incGraph, rootId));
+		incGraph.at(rootId) = poses.at(rootId);
 		return this->optimize(rootId, incGraph, incGraphLinks, intermediateGraphes, finalError, iterationsDone);
 	}
 

corelib/src/Parameters.cpp

@@ -226,6 +226,9 @@ const std::map<std::string, std::pair<bool, std::string> > & Parameters::getRemo
 	{
 		// removed parameters
 
+		// 0.17.0
+		removedParameters_.insert(std::make_pair("Grid/Scan2dMaxFilledRange",     std::make_pair(false,  Parameters::kGridRangeMax())));
+
 		// 0.16.0
 		removedParameters_.insert(std::make_pair("Grid/ProjRayTracing",           std::make_pair(true,  Parameters::kGridRayTracing())));
 		removedParameters_.insert(std::make_pair("Grid/DepthMin",                 std::make_pair(true,  Parameters::kGridRangeMin())));

corelib/src/RegistrationIcp.cpp

@@ -423,7 +423,7 @@ void RegistrationIcp::parseParameters(const ParametersMap & parameters)
 #ifndef RTABMAP_POINTMATCHER
 	if(_libpointmatcher)
 	{
-		UWARN("Parameter %s is set to true but RTAB-MAp has not been built with libpointmatcher support. Setting to false.", Parameters::kIcpPM().c_str());
+		UWARN("Parameter %s is set to true but RTAB-Map has not been built with libpointmatcher support. Setting to false.", Parameters::kIcpPM().c_str());
 		_libpointmatcher = false;
 	}
 #else

corelib/src/Rtabmap.cpp

@@ -121,6 +121,7 @@ Rtabmap::Rtabmap() :
 	_pathStuckIterations(Parameters::defaultRGBDPlanStuckIterations()),
 	_pathLinearVelocity(Parameters::defaultRGBDPlanLinearVelocity()),
 	_pathAngularVelocity(Parameters::defaultRGBDPlanAngularVelocity()),
+	_savedLocalizationIgnored(Parameters::defaultRGBDSavedLocalizationIgnored()),
 	_loopClosureHypothesis(0,0.0f),
 	_highestHypothesis(0,0.0f),
 	_lastProcessTime(0.0),
@@ -309,6 +310,19 @@ void Rtabmap::init(const ParametersMap & parameters, const std::string & databas
 	// Parse all parameters
 	this->parseParameters(parameters);
 
+	Transform lastPose;
+	_optimizedPoses = _memory->loadOptimizedPoses(&lastPose);
+	if(_optimizedPoses.size())
+	{
+		if(!_savedLocalizationIgnored)
+		{
+			_lastLocalizationPose = lastPose;
+		}
+		std::map<int, Transform> tmp;
+		// Get just the links
+		_memory->getMetricConstraints(uKeysSet(_optimizedPoses), tmp, _constraints, false);
+	}
+
 	if(_databasePath.empty())
 	{
 		_statisticLogged = false;
@@ -337,11 +351,10 @@ void Rtabmap::close(bool databaseSaved, const std::string & ouputDatabasePath)
 	_loopClosureHypothesis = std::make_pair(0,0.0f);
 	_lastProcessTime = 0.0;
 	_someNodesHaveBeenTransferred = false;
-	_optimizedPoses.clear();
 	_constraints.clear();
 	_mapCorrection.setIdentity();
 	_mapCorrectionBackup.setNull();
-	_lastLocalizationPose.setNull();
+
 	_lastLocalizationNodeId = 0;
 	_distanceTravelled = 0.0f;
 	this->clearPath(0);
@@ -365,10 +378,14 @@ void Rtabmap::close(bool databaseSaved, const std::string & ouputDatabasePath)
 	}
 	if(_memory)
 	{
+		_memory->saveOptimizedPoses(_optimizedPoses, _lastLocalizationPose);
 		_memory->close(databaseSaved, true, ouputDatabasePath);
 		delete _memory;
 		_memory = 0;
 	}
+	_optimizedPoses.clear();
+	_lastLocalizationPose.setNull();
+
 	if(_bayesFilter)
 	{
 		delete _bayesFilter;
@@ -444,6 +461,7 @@ void Rtabmap::parseParameters(const ParametersMap & parameters)
 	Parameters::parse(parameters, Parameters::kRGBDPlanStuckIterations(), _pathStuckIterations);
 	Parameters::parse(parameters, Parameters::kRGBDPlanLinearVelocity(), _pathLinearVelocity);
 	Parameters::parse(parameters, Parameters::kRGBDPlanAngularVelocity(), _pathAngularVelocity);
+	Parameters::parse(parameters, Parameters::kRGBDSavedLocalizationIgnored(), _savedLocalizationIgnored);
 
 	UASSERT(_rgbdLinearUpdate >= 0.0f);
 	UASSERT(_rgbdAngularUpdate >= 0.0f);
@@ -644,6 +662,24 @@ Transform Rtabmap::getPose(int locationId) const
 	return Transform();
 }
 
+void Rtabmap::setInitialPose(const Transform & initialPose)
+{
+	if(_memory)
+	{
+		if(!_memory->isIncremental())
+		{
+			_lastLocalizationPose = initialPose;
+			_lastLocalizationNodeId = 0;
+			_mapCorrection.setIdentity();
+			_mapCorrectionBackup.setNull();
+		}
+		else
+		{
+			UWARN("Initial pose can only be set in localization mode (%s=false), ignoring it...", Parameters::kMemIncrementalMemory().c_str());
+		}
+	}
+}
+
 int Rtabmap::triggerNewMap()
 {
 	int mapId = -1;
@@ -655,8 +691,6 @@ int Rtabmap::triggerNewMap()
 		_optimizedPoses.clear();
 		_constraints.clear();
 		_lastLocalizationNodeId = 0;
-		_mapCorrection.setIdentity();
-		_mapCorrectionBackup.setNull();
 
 		if(_bayesFilter)
 		{
@@ -937,10 +971,24 @@ bool Rtabmap::process(
 	bool fakeOdom = false;
 	if(_rgbdSlamMode)
 	{
-		if(!_memory->isIncremental() && !odomPose.isNull() && !_mapCorrectionBackup.isNull())
+		if(!_memory->isIncremental() && !odomPose.isNull())
 		{
-			_mapCorrection = _mapCorrectionBackup;
+			if(!_mapCorrectionBackup.isNull())
-			_mapCorrectionBackup.setNull();
+			{
+				_mapCorrection = _mapCorrectionBackup;
+				_mapCorrectionBackup.setNull();
+			}
+			else if(_optimizedPoses.size() && _mapCorrection.isIdentity() && !_lastLocalizationPose.isNull() && _lastLocalizationNodeId == 0)
+			{
+				// Localization mode, set map->odom so that odom is moved back to last saved localization
+				_mapCorrection = _lastLocalizationPose * odomPose.inverse();
+				_lastLocalizationNodeId = graph::findNearestNode(_optimizedPoses, _lastLocalizationPose);
+				UWARN("Update map correction based on last localization saved in database! correction = %s, nearest id = %d of last pose = %s, odom = %s",
+						_mapCorrection.prettyPrint().c_str(),
+						_lastLocalizationNodeId,
+						_lastLocalizationPose.prettyPrint().c_str(),
+						odomPose.prettyPrint().c_str());
+			}
 		}
 
 		if(odomPose.isNull())
@@ -2241,10 +2289,18 @@ bool Rtabmap::process(
 			
 			// if _optimizeFromGraphEnd parameter just changed state, don't use optimized poses as guess
 			float normMapCorrection = _mapCorrection.getNormSquared(); // use distance for identity detection
-			if((normMapCorrection > 0.001f && _optimizeFromGraphEnd) ||
+			if((normMapCorrection > 0.000001f && _optimizeFromGraphEnd) ||
-				(normMapCorrection < 0.001f && !_optimizeFromGraphEnd))
+				(normMapCorrection < 0.000001f && !_optimizeFromGraphEnd))
 			{
-				poses.clear();
+				for(std::multimap<int, Link>::iterator iter=_constraints.begin(); iter!=_constraints.end(); ++iter)
+				{
+					if(iter->second.type() != Link::kNeighbor && iter->second.type() != Link::kVirtualClosure)
+					{
+						UWARN("Optimization: clearing guess poses as %s may have changed state, now %s (normMapCorrection=%f)", Parameters::kRGBDOptimizeFromGraphEnd().c_str(), _optimizeFromGraphEnd?"true":"false", normMapCorrection);
+						poses.clear();
+						break;
+					}
+				}
 			}
 
 			std::multimap<int, Link> constraints;
@@ -2269,7 +2325,8 @@ bool Rtabmap::process(
 			else if(_memory->isIncremental() && // FIXME: not tested in localization mode, so do it only in mapping mode
 			  _optimizationMaxLinearError > 0.0f &&
 			  loopClosureLinksAdded.size() &&
-			  optimizationIterations > 0)
+			  optimizationIterations > 0 &&
+			  constraints.size())
 			{
 				const Link * maxLinearLink = 0;
 				for(std::multimap<int, Link>::iterator iter=constraints.begin(); iter!=constraints.end(); ++iter)
@@ -2294,34 +2351,34 @@ bool Rtabmap::process(
 				if(maxLinearLink)
 				{
 					UINFO("Max optimization error = %f m (link %d->%d, var=%f, %f)", maxLinearError, maxLinearLink->from(), maxLinearLink->to(), maxLinearLink->transVariance(), maxLinearError/sqrt(maxLinearLink->transVariance()));
-				}
 
-				float stddev = sqrt(maxLinearLink->transVariance());
+					float stddev = sqrt(maxLinearLink->transVariance());
-				maxLinearErrorRatio = maxLinearError/stddev;
+					maxLinearErrorRatio = maxLinearError/stddev;
-				if(maxLinearErrorRatio > _optimizationMaxLinearError)
+					if(maxLinearErrorRatio > _optimizationMaxLinearError)
-				{
-					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.",
-						  (int)loopClosureLinksAdded.size(),
-						  maxLinearErrorRatio,
-						  maxLinearLink->from(),
-						  maxLinearLink->to(),
-						  maxLinearLink->type(),
-						  maxLinearError,
-						  stddev,
-						  _optimizationMaxLinearError);
-					for(std::list<std::pair<int, int> >::iterator iter=loopClosureLinksAdded.begin(); iter!=loopClosureLinksAdded.end(); ++iter)
 					{
-						_memory->removeLink(iter->first, iter->second);
+						UWARN("Rejecting all added loop closures (%d) in this "
-						UWARN("Loop closure %d->%d rejected!", iter->first, iter->second);
+							  "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.",
+							  (int)loopClosureLinksAdded.size(),
+							  maxLinearErrorRatio,
+							  maxLinearLink->from(),
+							  maxLinearLink->to(),
+							  maxLinearLink->type(),
+							  maxLinearError,
+							  stddev,
+							  _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;
 					}
-					updateConstraints = false;
-					_loopClosureHypothesis.first = 0;
-					lastProximitySpaceClosureId = 0;
-					rejectedHypothesis = true;
 				}
 			}
 
@@ -3284,24 +3341,24 @@ std::map<int, Transform> Rtabmap::optimizeGraph(
 
 		if(!poses.empty() && optimizedPoses.empty() && guessPoses.empty())
 		{
-			UERROR("Optimization has failed, trying incremental optimization instead, this may take a while (poses=%d, links=%d)...", (int)poses.size(), (int)edgeConstraints.size());
+			UWARN("Optimization has failed, trying incremental optimization instead, this may take a while (poses=%d, links=%d)...", (int)poses.size(), (int)edgeConstraints.size());
 			optimizedPoses = _graphOptimizer->optimizeIncremental(fromId, poses, edgeConstraints, 0, error, iterationsDone);
 
 			if(optimizedPoses.empty())
 			{
 				if(!_graphOptimizer->isCovarianceIgnored() || _graphOptimizer->type() != Optimizer::kTypeTORO)
 				{
-					UERROR("Incremental optimization also failed. You may try changing parameters to %s=0 and %s=true.",
+					UWARN("Incremental optimization also failed. You may try changing parameters to %s=0 and %s=true.",
 							Parameters::kOptimizerStrategy().c_str(), Parameters::kOptimizerVarianceIgnored().c_str());
 				}
 				else
 				{
-					UERROR("Incremental optimization also failed.");
+					UWARN("Incremental optimization also failed.");
 				}
 			}
 			else
 			{
-				UERROR("Incremental optimization succeeded!");
+				UWARN("Incremental optimization succeeded!");
 			}
 		}
 	}

corelib/src/resources/DatabaseSchema.sql.in

@@ -112,6 +112,7 @@ CREATE TABLE Admin (
 	opt_cloud BLOB,          -- compressed data
 	opt_ids BLOB,            -- Node ids used to generate the optimized cloud/mesh
 	opt_poses BLOB,          -- compressed N*3x4 float 
+	opt_last_localization BLOB, -- 3x4 float
 	opt_polygons_size INTEGER, -- e.g., 3
 	opt_polygons BLOB,       -- compressed data [length_v0, i0,i1,i3, length_v1, i0,i1,i3]
 	opt_tex_coords BLOB,     -- compressed data [length_v0, u0,v0,u1,v1,u2,v2, length_v1, u0,v0,u1,v1,u2,v2]
@@ -119,6 +120,7 @@ CREATE TABLE Admin (
 	opt_map BLOB,            -- compressed CV_8SC1 occupancy grid
 	opt_map_x_min FLOAT,
 	opt_map_y_min FLOAT, 
+	opt_map_resolution FLOAT, 
 
 	time_enter DATE
 );

corelib/src/util3d.cpp

@@ -2285,6 +2285,45 @@ cv::Mat laserScan2dFromPointCloud(const pcl::PointCloud<pcl::PointXYZI> & cloud,
 	return laserScan(cv::Range::all(), cv::Range(0,oi));
 }
 
+pcl::PCLPointCloud2::Ptr laserScanToPointCloud2(const LaserScan & laserScan, const Transform & transform)
+{
+	pcl::PCLPointCloud2::Ptr cloud(new pcl::PCLPointCloud2);
+	if(laserScan.isEmpty())
+	{
+		return cloud;
+	}
+
+	if(laserScan.format() == LaserScan::kXY || laserScan.format() == LaserScan::kXYZ)
+	{
+		pcl::toPCLPointCloud2(*laserScanToPointCloud(laserScan, transform), *cloud);
+	}
+	else if(laserScan.format() == LaserScan::kXYI || laserScan.format() == LaserScan::kXYZI)
+	{
+		pcl::toPCLPointCloud2(*laserScanToPointCloudI(laserScan, transform), *cloud);
+	}
+	else if(laserScan.format() == LaserScan::kXYNormal || laserScan.format() == LaserScan::kXYZNormal)
+	{
+		pcl::toPCLPointCloud2(*laserScanToPointCloudNormal(laserScan, transform), *cloud);
+	}
+	else if(laserScan.format() == LaserScan::kXYINormal || laserScan.format() == LaserScan::kXYZINormal)
+	{
+		pcl::toPCLPointCloud2(*laserScanToPointCloudINormal(laserScan, transform), *cloud);
+	}
+	else if(laserScan.format() == LaserScan::kXYZRGB)
+	{
+		pcl::toPCLPointCloud2(*laserScanToPointCloudRGB(laserScan, transform), *cloud);
+	}
+	else if(laserScan.format() == LaserScan::kXYZRGBNormal)
+	{
+		pcl::toPCLPointCloud2(*laserScanToPointCloudRGBNormal(laserScan, transform), *cloud);
+	}
+	else
+	{
+		UERROR("Unknown conversion from LaserScan format %d to PointCloud2.", laserScan.format());
+	}
+	return cloud;
+}
+
 pcl::PointCloud<pcl::PointXYZ>::Ptr laserScanToPointCloud(const LaserScan & laserScan, const Transform & transform)
 {
 	pcl::PointCloud<pcl::PointXYZ>::Ptr output(new pcl::PointCloud<pcl::PointXYZ>);

corelib/src/util3d_mapping.cpp

@@ -35,6 +35,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #include <rtabmap/utilite/UTimer.h>
 #include <rtabmap/utilite/UStl.h>
 #include <rtabmap/utilite/UConversion.h>
+#include <rtabmap/utilite/UMath.h>
 
 #include <pcl/common/common.h>
 #include <pcl/common/centroid.h>
@@ -133,7 +134,14 @@ void occupancy2DFromLaserScan(
 	}
 
 	// copy directly obstacles precise positions
-	occupied = scanHit.clone();
+	if(scanMaxRange > cellSize)
+	{
+		occupied = util3d::rangeFiltering(LaserScan::backwardCompatibility(scanHit), 0.0f, scanMaxRange).data().clone();
+	}
+	else
+	{
+		occupied = scanHit.clone();
+	}
 }
 
 /**
@@ -628,6 +636,7 @@ cv::Mat create2DMap(const std::map<int, Transform> & poses,
 
 		map = cv::Mat::ones((yMax - yMin) / cellSize, (xMax - xMin) / cellSize, CV_8S)*-1;
 		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)
 		{
 			const Transform & pose = poses.at(iter->first);
@@ -638,15 +647,20 @@ 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);
-				cv::Point2i end((ptr[0]-xMin)/cellSize, (ptr[1]-yMin)/cellSize);
+				bool ignore = scanMaxRange>cellSize && uNormSquared(ptr[0]+cellSize, ptr[1]+cellSize) > scanMaxRangeSqr;
-				if(end!=start)
+				if(!ignore)
 				{
-					map.at<char>(end.y, end.x) = 100; // obstacle
+					cv::Point2i end((ptr[0]+startf.x-xMin)/cellSize, (ptr[1]+startf.y-yMin)/cellSize);
+					if(end!=start)
+					{
+						map.at<char>(end.y, end.x) = 100; // obstacle
+					}
 				}
 			}
 
@@ -654,7 +668,18 @@ cv::Mat create2DMap(const std::map<int, Transform> & poses,
 			for(int i=0; i<iter->second.first.cols; ++i)
 			{
 				const float * ptr = iter->second.first.ptr<float>(0, i);
-				cv::Point2i end((ptr[0]-xMin)/cellSize, (ptr[1]-yMin)/cellSize);
+
+				cv::Vec2f v(ptr[0], ptr[1]);
+				if(scanMaxRange>cellSize)
+				{
+					float n = cv::norm(v);
+					if(n > scanMaxRange+cellSize)
+					{
+						v = (v/n) * scanMaxRange;
+					}
+				}
+
+				cv::Point2i end((v[0]+startf.x-xMin)/cellSize, (v[1]+startf.y-yMin)/cellSize);
 				if(end!=start)
 				{
 					if(localScans.size() > 1 || map.at<char>(end.y, end.x) != 0)
@@ -667,7 +692,18 @@ cv::Mat create2DMap(const std::map<int, Transform> & poses,
 			for(int i=0; i<iter->second.second.cols; ++i)
 			{
 				const float * ptr = iter->second.second.ptr<float>(0, i);
-				cv::Point2i end((ptr[0]-xMin)/cellSize, (ptr[1]-yMin)/cellSize);
+
+				cv::Vec2f v(ptr[0], ptr[1]);
+				if(scanMaxRange>cellSize)
+				{
+					float n = cv::norm(v);
+					if(n > scanMaxRange+cellSize)
+					{
+						v = (v/n) * scanMaxRange;
+					}
+				}
+
+				cv::Point2i end((v[0]+startf.x-xMin)/cellSize, (v[1]+startf.y-yMin)/cellSize);
 				if(end!=start)
 				{
 					if(localScans.size() > 1 || map.at<char>(end.y, end.x) != 0)
@@ -712,10 +748,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];
+						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];
+						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];
+						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];
+						endLast.at<float>(1) = iter->second.first.ptr<float>(0,iter->second.first.cols-1)[1]+origin.at<float>(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));
@@ -849,7 +885,7 @@ void rayTrace(const cv::Point2i & start, const cv::Point2i & end, cv::Mat & grid
 }
 
 //convert to gray scaled map
-cv::Mat convertMap2Image8U(const cv::Mat & map8S)
+cv::Mat convertMap2Image8U(const cv::Mat & map8S, bool pgmFormat)
 {
 	UASSERT(map8S.channels() == 1 && map8S.type() == CV_8S);
 	cv::Mat map8U = cv::Mat(map8S.rows, map8S.cols, CV_8U);
@@ -857,11 +893,11 @@ cv::Mat convertMap2Image8U(const cv::Mat & map8S)
 	{
 		for (int j = 0; j < map8S.cols; ++j)
 		{
-			char v = map8S.at<char>(i, j);
+			char v = pgmFormat?map8S.at<char>((map8S.rows-1)-i, j):map8S.at<char>(i, j);
 			unsigned char gray;
 			if(v == 0)
 			{
-				gray = 178;
+				gray = pgmFormat?254:178;
 			}
 			else if(v == 100)
 			{
@@ -869,11 +905,11 @@ cv::Mat convertMap2Image8U(const cv::Mat & map8S)
 			}
 			else if(v == -2)
 			{
-				gray = 200;
+				gray = pgmFormat?254:200;
 			}
 			else // -1
 			{
-				gray = 89;
+				gray = pgmFormat?205:89;
 			}
 			map8U.at<unsigned char>(i, j) = gray;
 		}
@@ -881,6 +917,58 @@ cv::Mat convertMap2Image8U(const cv::Mat & map8S)
 	return map8U;
 }
 
+//convert gray scaled image to map
+cv::Mat convertImage8U2Map(const cv::Mat & map8U, bool pgmFormat)
+{
+	UASSERT_MSG(map8U.channels() == 1 && map8U.type() == CV_8U, uFormat("map8U.channels()=%d map8U.type()=%d", map8U.channels(), map8U.type()).c_str());
+	cv::Mat map8S = cv::Mat(map8U.rows, map8U.cols, CV_8S);
+	for (int i = 0; i < map8U.rows; ++i)
+	{
+		for (int j = 0; j < map8U.cols; ++j)
+		{
+			unsigned char v = pgmFormat?map8U.at<char>((map8U.rows-1)-i, j):map8U.at<char>(i, j);
+			char occupancy;
+			if(pgmFormat)
+			{
+				if(v >= 254)
+				{
+					occupancy = 0;
+				}
+				else if(v == 0)
+				{
+					occupancy = 100;
+				}
+				else // 205
+				{
+					occupancy = -1;
+				}
+			}
+			else
+			{
+				if(v == 178)
+				{
+					occupancy = 0;
+				}
+				else if(v == 0)
+				{
+					occupancy = 100;
+				}
+				else if(v == 200)
+				{
+					occupancy = -2;
+				}
+				else // 89
+				{
+					occupancy = -1;
+				}
+			}
+
+			map8S.at<char>(i, j) = occupancy;
+		}
+	}
+	return map8S;
+}
+
 cv::Mat erodeMap(const cv::Mat & map)
 {
 	UASSERT(map.type() == CV_8SC1);

corelib/src/util3d_motion_estimation.cpp

@@ -37,9 +37,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 
 #include <pcl/common/common.h>
 
-#if CV_MAJOR_VERSION < 3
 #include "opencv/solvepnp.h"
-#endif
 
 namespace rtabmap
 {
@@ -344,11 +342,10 @@ void solvePnPRansac(
 	{
 		minInliersCount = 4;
 	}
-#if CV_MAJOR_VERSION < 3
+
-	cv3::solvePnPRansac( //use OpenCV3 version of solvePnPRansac in OpenCV2
+	// Use OpenCV3 version of solvePnPRansac in OpenCV2.
-#else
+	// FIXME: we should use this version of solvePnPRansac in newer 3.3.1 too, which seems a lot less stable!?!? Why!?
-	cv::solvePnPRansac( // use directly version from OpenCV 3
+	cv3::solvePnPRansac(
-#endif
 			objectPoints,
 			imagePoints,
 			cameraMatrix,

corelib/src/util3d_surface.cpp

@@ -640,6 +640,11 @@ pcl::texture_mapping::CameraVector createTextureCameras(
 				cam.depth = cv::Mat(depthIter->second, cv::Range(0, depthIter->second.rows), cv::Range(subWidth*i, subWidth*(i+1)));
 			}
 
+			UDEBUG("%f", cam.focal_length);
+			UDEBUG("%f", cam.height);
+			UDEBUG("%f", cam.width);
+			UDEBUG("cam.pose=%s", t.prettyPrint().c_str());
+
 			cameras.push_back(cam);
 		}
 	}
@@ -1191,6 +1196,51 @@ void concatenateTextureMaterials(pcl::TextureMesh & mesh, const cv::Size & image
 	}
 }
 
+std::vector<std::vector<unsigned int> > convertPolygonsFromPCL(const std::vector<pcl::Vertices> & polygons)
+{
+	std::vector<std::vector<unsigned int> > polygonsOut(polygons.size());
+	for(unsigned int p=0; p<polygons.size(); ++p)
+	{
+		polygonsOut[p] = polygons[p].vertices;
+	}
+	return polygonsOut;
+}
+std::vector<std::vector<std::vector<unsigned int> > > convertPolygonsFromPCL(const std::vector<std::vector<pcl::Vertices> > & tex_polygons)
+{
+	std::vector<std::vector<std::vector<unsigned int> > > polygonsOut(tex_polygons.size());
+	for(unsigned int t=0; t<tex_polygons.size(); ++t)
+	{
+		polygonsOut[t].resize(tex_polygons[t].size());
+		for(unsigned int p=0; p<tex_polygons[t].size(); ++p)
+		{
+			polygonsOut[t][p] = tex_polygons[t][p].vertices;
+		}
+	}
+	return polygonsOut;
+}
+std::vector<pcl::Vertices> convertPolygonsToPCL(const std::vector<std::vector<unsigned int> > & polygons)
+{
+	std::vector<pcl::Vertices> polygonsOut(polygons.size());
+	for(unsigned int p=0; p<polygons.size(); ++p)
+	{
+		polygonsOut[p].vertices = polygons[p];
+	}
+	return polygonsOut;
+}
+std::vector<std::vector<pcl::Vertices> > convertPolygonsToPCL(const std::vector<std::vector<std::vector<unsigned int> > > & tex_polygons)
+{
+	std::vector<std::vector<pcl::Vertices> > polygonsOut(tex_polygons.size());
+	for(unsigned int t=0; t<tex_polygons.size(); ++t)
+	{
+		polygonsOut[t].resize(tex_polygons[t].size());
+		for(unsigned int p=0; p<tex_polygons[t].size(); ++p)
+		{
+			polygonsOut[t][p].vertices = tex_polygons[t][p];
+		}
+	}
+	return polygonsOut;
+}
+
 pcl::TextureMesh::Ptr assembleTextureMesh(
 		const cv::Mat & cloudMat,
 		const std::vector<std::vector<std::vector<unsigned int> > > & polygons,
@@ -1357,6 +1407,50 @@ double sqr(uchar v)
 	return double(v)*double(v);
 }
 
+cv::Mat mergeTextures(
+		pcl::TextureMesh & mesh,
+		const std::map<int, cv::Mat> & images,
+		const std::map<int, CameraModel> & calibrations,
+		const Memory * memory,
+		const DBDriver * dbDriver,
+		int textureSize,
+		int textureCount,
+		const std::vector<std::map<int, pcl::PointXY> > & vertexToPixels,
+		bool gainCompensation,
+		float gainBeta,
+		bool gainRGB,
+		bool blending,
+		int blendingDecimation,
+		int brightnessContrastRatioLow,
+		int brightnessContrastRatioHigh,
+		bool exposureFusion,
+		const ProgressState * state)
+{
+	std::map<int, std::vector<CameraModel> > calibVectors;
+	for(std::map<int, CameraModel>::const_iterator iter=calibrations.begin(); iter!=calibrations.end(); ++iter)
+	{
+		std::vector<CameraModel> m;
+		m.push_back(iter->second);
+		calibVectors.insert(std::make_pair(iter->first, m));
+	}
+	return mergeTextures(mesh,
+			images,
+			calibVectors,
+			memory,
+			dbDriver,
+			textureSize,
+			textureCount,
+			vertexToPixels,
+			gainCompensation,
+			gainBeta,
+			gainRGB,
+			blending,
+			blendingDecimation,
+			brightnessContrastRatioLow,
+			brightnessContrastRatioHigh,
+			exposureFusion,
+			state);
+}
 cv::Mat mergeTextures(
 		pcl::TextureMesh & mesh,
 		const std::map<int, cv::Mat> & images,
@@ -1533,8 +1627,8 @@ cv::Mat mergeTextures(
 						newCamIndex[t] = oi;
 						int u = oi%cols * emptyImage.cols;
 						int v = ((oi/cols) % rows ) * emptyImage.rows;
-						UASSERT(u < textureSize-emptyImage.cols);
+						UASSERT_MSG(u < textureSize-emptyImage.cols, uFormat("u=%d textureSize=%d emptyImage.cols=%d", u, textureSize, emptyImage.cols).c_str());
-						UASSERT(v < textureSize-emptyImage.rows);
+						UASSERT_MSG(v < textureSize-emptyImage.rows, uFormat("v=%d textureSize=%d emptyImage.rows=%d", v, textureSize, emptyImage.rows).c_str());
 						imageOrigin[t].x = u;
 						imageOrigin[t].y = v;
 						if(textures[t].first>=0)
@@ -1990,10 +2084,49 @@ cv::Mat mergeTextures(
 			}
 		}
 	}
-	UDEBUG("globalTextures=%d", globalTextures.cols / globalTextures.rows);
+	UDEBUG("globalTextures=%d", globalTextures.cols?globalTextures.cols / globalTextures.rows:0);
 	return globalTextures;
 }
 
+void fixTextureMeshForVisualization(pcl::TextureMesh & textureMesh)
+{
+	// VTK issue:
+	//  tex_coordinates should be linked to points, not
+	//  polygon vertices. Points linked to multiple different TCoords (different textures) should
+	//  be duplicated.
+	for (unsigned int t = 0; t < textureMesh.tex_coordinates.size(); ++t)
+	{
+		if(textureMesh.tex_polygons[t].size())
+		{
+			pcl::PointCloud<pcl::PointXYZ>::Ptr originalCloud(new pcl::PointCloud<pcl::PointXYZ>);
+			pcl::fromPCLPointCloud2(textureMesh.cloud, *originalCloud);
+
+			// make a cloud with as many points than polygon vertices
+			unsigned int nPoints = textureMesh.tex_coordinates[t].size();
+			UASSERT(nPoints == textureMesh.tex_polygons[t].size()*textureMesh.tex_polygons[t][0].vertices.size()); // assuming polygon size is constant!
+
+			pcl::PointCloud<pcl::PointXYZ>::Ptr newCloud(new pcl::PointCloud<pcl::PointXYZ>);
+			newCloud->resize(nPoints);
+
+			unsigned int oi = 0;
+			for (unsigned int i = 0; i < textureMesh.tex_polygons[t].size(); ++i)
+			{
+				pcl::Vertices & vertices = textureMesh.tex_polygons[t][i];
+
+				for(unsigned int j=0; j<vertices.vertices.size(); ++j)
+				{
+					UASSERT(oi < newCloud->size());
+					UASSERT_MSG(vertices.vertices[j] < originalCloud->size(), uFormat("%d vs %d", vertices.vertices[j], (int)originalCloud->size()).c_str());
+					newCloud->at(oi) = originalCloud->at(vertices.vertices[j]);
+					vertices.vertices[j] = oi; // new vertex index
+					++oi;
+				}
+			}
+			pcl::toPCLPointCloud2(*newCloud, textureMesh.cloud);
+		}
+	}
+}
+
 LaserScan computeNormals(
 		const LaserScan & laserScan,
 		int searchK,
@@ -2828,11 +2961,12 @@ void adjustNormalsToViewPoints(
 		const std::vector<int> & rawCameraIndices,
 		pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr & cloud)
 {
+	UASSERT(rawCloud.get() && cloud.get());
+	UDEBUG("poses=%d, rawCloud=%d, rawCameraIndices=%d, cloud=%d", (int)poses.size(), (int)rawCloud->size(), (int)rawCameraIndices.size(), (int)cloud->size());
 	if(poses.size() && rawCloud->size() && rawCloud->size() == rawCameraIndices.size() && cloud->size())
 	{
 		pcl::search::KdTree<pcl::PointXYZ>::Ptr rawTree (new pcl::search::KdTree<pcl::PointXYZ>);
 		rawTree->setInputCloud (rawCloud);
-
 		for(unsigned int i=0; i<cloud->size(); ++i)
 		{
 			pcl::PointXYZ normal(cloud->points[i].normal_x, cloud->points[i].normal_y, cloud->points[i].normal_z);
@@ -2841,9 +2975,10 @@ void adjustNormalsToViewPoints(
 				std::vector<int> indices;
 				std::vector<float> dist;
 				rawTree->nearestKSearch(pcl::PointXYZ(cloud->points[i].x, cloud->points[i].y, cloud->points[i].z), 1, indices, dist);
-				UASSERT(indices.size() == 1);
 				if(indices.size() && indices[0]>=0)
 				{
+					UASSERT_MSG(indices[0]<(int)rawCameraIndices.size(), uFormat("indices[0]=%d rawCameraIndices.size()=%d", indices[0], (int)rawCameraIndices.size()).c_str());
+					UASSERT(uContains(poses, rawCameraIndices[indices[0]]));
 					Transform p = poses.at(rawCameraIndices[indices[0]]);
 					pcl::PointXYZ viewpoint(p.x(), p.y(), p.z());
 					Eigen::Vector3f v = viewpoint.getVector3fMap() - cloud->points[i].getVector3fMap();
@@ -2861,7 +2996,7 @@ void adjustNormalsToViewPoints(
 				}
 				else
 				{
-					UWARN("Not found camera viewpoint for point %d", i);
+					UWARN("Not found camera viewpoint for point %d!?", i);
 				}
 			}
 		}

docker/bionic/Dockerfile

@@ -0,0 +1,34 @@
+# Image: introlab3it/rtabmap:bionic
+
+FROM ubuntu:18.04
+
+# Install build dependencies
+RUN apt-get update && DEBIAN_FRONTEND=noninteractive apt-get install -y \
+      libsqlite3-dev \
+      libpcl-dev \
+      git \
+      cmake \
+      libopencv-dev \
+      libproj-dev \
+      libqt5svg5-dev \
+      libfreenect-dev \
+      libopenni2-dev \
+      ffmpeg \
+      software-properties-common
+
+WORKDIR /root/
+
+# Clone source code
+RUN git clone https://github.com/introlab/rtabmap.git
+
+# Build RTAB-Map project
+RUN cd rtabmap/build && \
+    cmake .. && \
+    make -j$(nproc) && \
+    make install && \
+    cd ../.. && \
+    rm -rf rtabmap && \
+    ldconfig
+
+WORKDIR /root
+    

guilib/include/rtabmap/gui/CloudViewer.h

@@ -60,6 +60,8 @@ namespace pcl {
 
 class QMenu;
 class vtkProp;
+template <typename T> class vtkSmartPointer;
+class vtkOBBTree;
 
 namespace rtabmap {
 
@@ -190,21 +192,48 @@ public:
 	void addOrUpdateCoordinate(
 			const std::string & id,
 			const Transform & transform,
-			double scale);
+			double scale,
+			bool foreground=false);
 	bool updateCoordinatePose(
 			const std::string & id,
 			const Transform & transform);
 	void removeCoordinate(const std::string & id);
 	void removeAllCoordinates();
+	const std::set<std::string> & getAddedCoordinates() const {return _coordinates;}
 
 	void addOrUpdateLine(
 				const std::string & id,
 				const Transform & from,
 				const Transform & to,
 				const QColor & color,
-				bool arrow = false);
+				bool arrow = false,
+				bool foreground = false);
 	void removeLine(const std::string & id);
 	void removeAllLines();
+	const std::set<std::string> & getAddedLines() const {return _lines;}
+
+	void addOrUpdateSphere(
+				const std::string & id,
+				const Transform & pose,
+				float radius,
+				const QColor & color,
+				bool foreground = false);
+	void removeSphere(const std::string & id);
+	void removeAllSpheres();
+	const std::set<std::string> & getAddedSpheres() const {return _spheres;}
+
+	void addOrUpdateCube(
+				const std::string & id,
+				const Transform & pose, // center of the cube
+				float width,  // e.g., along x axis
+				float height, // e.g., along y axis
+				float depth,  // e.g., along z axis
+				const QColor & color,
+				bool wireframe = false,
+				bool foreground = false);
+	void removeCube(const std::string & id);
+	void removeAllCubes();
+	const std::set<std::string> & getAddedCubes() const {return _cubes;}
 
 	void addOrUpdateFrustum(
 			const std::string & id,
@@ -231,9 +260,11 @@ public:
 			const std::string & text,
 			const Transform & position,
 			double scale,
-			const QColor & color);
+			const QColor & color,
+			bool foreground = true);
 	void removeText(const std::string & id);
 	void removeAllTexts();
+	const std::set<std::string> & getAddedTexts() const {return _texts;}
 
 	bool isTrajectoryShown() const;
 	unsigned int getTrajectorySize() const;
@@ -260,6 +291,7 @@ public:
 	Transform getTargetPose() const;
 
 	void setBackfaceCulling(bool enabled, bool frontfaceCulling);
+	void setPolygonPicking(bool enabled);
 	void setRenderingRate(double rate);
 	void setLighting(bool on);
 	void setShading(bool on);
@@ -295,6 +327,8 @@ public:
 	float getNormalsScale() const;
 	void setNormalsStep(int step);
 	void setNormalsScale(float scale);
+	void buildPickingLocator(bool enable);
+	const std::map<std::string, vtkSmartPointer<vtkOBBTree> > & getLocators() const {return _locators;}
 
 public slots:
 	void setDefaultBackgroundColor(const QColor & color);
@@ -348,11 +382,14 @@ private:
     QAction * _aSetFlatShading;
     QAction * _aSetEdgeVisibility;
     QAction * _aBackfaceCulling;
+    QAction * _aPolygonPicking;
     QMenu * _menu;
     std::set<std::string> _graphes;
     std::set<std::string> _coordinates;
     std::set<std::string> _texts;
     std::set<std::string> _lines;
+    std::set<std::string> _spheres;
+    std::set<std::string> _cubes;
     QMap<std::string, Transform> _frustums;
     pcl::PointCloud<pcl::PointXYZ>::Ptr _trajectory;
     unsigned int _maxTrajectorySize;
@@ -362,6 +399,8 @@ private:
     float _gridCellSize;
     int _normalsStep;
     float _normalsScale;
+    bool _buildLocator;
+    std::map<std::string, vtkSmartPointer<vtkOBBTree> > _locators;
     cv::Vec3d _lastCameraOrientation;
     cv::Vec3d _lastCameraPose;
     QMap<std::string, Transform> _addedClouds; // include cloud, scan, meshes

guilib/include/rtabmap/gui/DatabaseViewer.h

@@ -93,6 +93,11 @@ private slots:
 	void selectEmptyColor();
 	void editDepthImage();
 	void generateGraph();
+	void exportSaved2DMap();
+	void import2DMap();
+	void viewOptimizedMesh();
+	void exportOptimizedMesh();
+	void updateOptimizedMesh();
 	void exportDatabase();
 	void extractImages();
 	void exportPosesRaw();

guilib/include/rtabmap/gui/ExportCloudsDialog.h

@@ -64,6 +64,10 @@ public:
 	void loadSettings(QSettings & settings, const QString & group = "");
 
 	void setDBDriver(const DBDriver * dbDriver) {_dbDriver = dbDriver;}
+	void forceAssembling(bool enabled);
+	void setProgressDialogToMax();
+	void setSaveButton();
+	void setOkButton();
 
 	void exportClouds(
 			const std::map<int, Transform> & poses,
@@ -85,6 +89,31 @@ public:
 			const QString & workingDirectory,
 			const ParametersMap & parameters);
 
+	bool getExportedClouds(
+			const std::map<int, Transform> & poses,
+			const std::multimap<int, Link> & links,
+			const std::map<int, int> & mapIds,
+			const QMap<int, Signature> & cachedSignatures,
+			const std::map<int, std::pair<pcl::PointCloud<pcl::PointXYZRGB>::Ptr, pcl::IndicesPtr> > & cachedClouds,
+			const std::map<int, LaserScan> & cachedScans,
+			const QString & workingDirectory,
+			const ParametersMap & parameters,
+			std::map<int, pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr> & clouds,
+			std::map<int, pcl::PolygonMesh::Ptr> & meshes,
+			std::map<int, pcl::TextureMesh::Ptr> & textureMeshes,
+			std::vector<std::map<int, pcl::PointXY> > & textureVertexToPixels);
+
+	int getTextureSize() const;
+	int getMaxTextures() const;
+	bool isGainCompensation() const;
+	double getGainBeta() const;
+	bool isGainRGB() const;
+	bool isBlending() const;
+	int getBlendingDecimation() const;
+	int getTextureBrightnessConstrastRatioLow() const;
+	int getTextureBrightnessConstrastRatioHigh() const;
+	bool isExposeFusion() const;
+
 	static bool removeDirRecursively(const QString & dirName);
 
 signals:
@@ -94,6 +123,8 @@ public slots:
 	void restoreDefaults();
 
 private slots:
+	void loadSettings();
+	void saveSettings();
 	void updateReconstructionFlavor();
 	void selectDistortionModel();
 	void updateMLSGrpVisibility();
@@ -107,26 +138,10 @@ private:
 			const std::map<int, LaserScan> & cachedScans,
 			const ParametersMap & parameters,
 			bool & has2dScans) const;
-	bool getExportedClouds(
-				const std::map<int, Transform> & poses,
-				const std::multimap<int, Link> & links,
-				const std::map<int, int> & mapIds,
-				const QMap<int, Signature> & cachedSignatures,
-				const std::map<int, std::pair<pcl::PointCloud<pcl::PointXYZRGB>::Ptr, pcl::IndicesPtr> > & cachedClouds,
-				const std::map<int, LaserScan> & cachedScans,
-				const QString & workingDirectory,
-				const ParametersMap & parameters,
-				std::map<int, pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr> & clouds,
-				std::map<int, pcl::PolygonMesh::Ptr> & meshes,
-				std::map<int, pcl::TextureMesh::Ptr> & textureMeshes,
-				std::vector<std::map<int, pcl::PointXY> > & textureVertexToPixels);
 	void saveClouds(const QString & workingDirectory, const std::map<int, Transform> & poses, const std::map<int, pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr> & clouds, bool binaryMode = true);
 	void saveMeshes(const QString & workingDirectory, const std::map<int, Transform> & poses, const std::map<int, pcl::PolygonMesh::Ptr> & meshes, bool binaryMode = true);
 	void saveTextureMeshes(const QString & workingDirectory, const std::map<int, Transform> & poses, std::map<int, pcl::TextureMesh::Ptr> & textureMeshes, const QMap<int, Signature> & cachedSignatures, const std::vector<std::map<int, pcl::PointXY> > & textureVertexToPixels);
 
-	void setSaveButton();
-	void setOkButton();
-
 private:
 	Ui_ExportCloudsDialog * _ui;
 	ProgressDialog * _progressDialog;

guilib/src/AboutDialog.cpp

@@ -73,6 +73,12 @@ AboutDialog::AboutDialog(QWidget * parent) :
 	_ui->label_cputsdf->setText("No");
 	_ui->label_cputsdf_license->setEnabled(false);
 #endif
+#ifdef RTABMAP_OPENCHISEL
+	_ui->label_openchisel->setText("Yes");
+#else
+	_ui->label_openchisel->setText("No");
+#endif
+
 	_ui->label_freenect->setText(CameraFreenect::available()?"Yes":"No");
 	_ui->label_freenect_license->setEnabled(CameraFreenect::available());
 	_ui->label_openni2->setText(CameraOpenNI2::available()?"Yes":"No");

guilib/src/CMakeLists.txt

@@ -157,6 +157,17 @@ IF(CPUTSDF_FOUND)
 	)
 ENDIF(CPUTSDF_FOUND)
 
+IF(open_chisel_FOUND)
+	SET(INCLUDE_DIRS
+		${INCLUDE_DIRS}
+		${open_chisel_INCLUDE_DIRS}
+	)
+	SET(LIBRARIES
+		${LIBRARIES}
+		${open_chisel_LIBRARIES}
+	)
+ENDIF(open_chisel_FOUND)
+
 IF(VTK_USE_QVTK)
 	SET(INCLUDE_DIRS ${INCLUDE_DIRS} ${QVTK_INCLUDE_DIR})
 	SET(LIBRARIES ${LIBRARIES} ${QVTK_LIBRARY})

guilib/src/CloudViewer.cpp

@@ -66,6 +66,9 @@ 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 <opencv/vtkImageMatSource.h>
 
 #ifdef RTABMAP_OCTOMAP
@@ -77,6 +80,17 @@ namespace rtabmap {
 class MyInteractorStyle: public pcl::visualization::PCLVisualizerInteractorStyle
 {
 public:
+	MyInteractorStyle(CloudViewer * viewer) :
+		pcl::visualization::PCLVisualizerInteractorStyle(),
+		NumberOfClicks(0),
+		ResetPixelDistance(0),
+		pointsHolder_(new pcl::PointCloud<pcl::PointXYZRGB>),
+		viewer_(viewer)
+	{
+		UASSERT(viewer_!=0);
+		PreviousPosition[0] = PreviousPosition[1] = 0;
+		PreviousMeasure[0] = PreviousMeasure[1] =  PreviousMeasure[2] = 0.0f;
+	}
 	virtual void Rotate()
 	{
 		if (this->CurrentRenderer == NULL)
@@ -98,6 +112,7 @@ public:
 		double ryf = dy * delta_elevation * this->MotionFactor;
 
 		vtkCamera *camera = this->CurrentRenderer->GetActiveCamera();
+		UASSERT(camera);
 		camera->Azimuth(rxf);
 		camera->Elevation(ryf);
 		camera->OrthogonalizeViewUp();
@@ -116,57 +131,218 @@ public:
 	}
 
 protected:
+	virtual void OnMouseMove()
+	{
+		if(this->CurrentRenderer &&
+			this->CurrentRenderer->GetLayer() == 1 &&
+			this->GetInteractor()->GetControlKey() &&
+			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_ctrl");
+				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_ctrl");
+				viewer_->removeText("interactor_ray_text_ctrl");
+
+				// 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_ctrl", 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_ctrl",
+								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_ctrl", pointsHolder_);
+				viewer_->setCloudPointSize("interactor_points_ctrl", 15);
+				viewer_->setCloudOpacity("interactor_points_ctrl", 0.5);
+			}
+		}
+		// Forward events
+		PCLVisualizerInteractorStyle::OnMouseMove();
+	}
+
 	virtual void 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;
+			this->NumberOfClicks++;
-		}
+			int pickPosition[2];
+			this->GetInteractor()->GetEventPosition(pickPosition);
+			int xdist = pickPosition[0] - this->PreviousPosition[0];
+			int ydist = pickPosition[1] - this->PreviousPosition[1];
 
-		if(this->NumberOfClicks == 2)
+			this->PreviousPosition[0] = pickPosition[0];
-		{
+			this->PreviousPosition[1] = pickPosition[1];
-			this->NumberOfClicks = 0;
 
-			this->Interactor->GetPicker()->Pick(pickPosition[0], pickPosition[1],
+			int moveDistance = (int)sqrt((double)(xdist*xdist + ydist*ydist));
-					0,  // always zero.
-					this->Interactor->GetRenderWindow()->GetRenderers()->GetFirstRenderer());
-			double picked[3];
-			this->Interactor->GetPicker()->GetPickPosition(picked);
-			UINFO("Double clicked! Picked value: %f %f %f", picked[0], picked[1], picked[2]);
 
-			vtkCamera *camera = this->CurrentRenderer->GetActiveCamera();
+			// Reset numClicks - If mouse moved further than resetPixelDistance
-			double position[3];
+			if(moveDistance > this->ResetPixelDistance)
-			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();
+				this->NumberOfClicks = 1;
 			}
 
-			if (this->Interactor->GetLightFollowCamera())
+			if(this->NumberOfClicks >= 2)
 			{
-				this->CurrentRenderer->UpdateLightsGeometryToFollowCamera();
+				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()->GetShiftKey()==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
+				{
+					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_ctrl");
+					viewer_->removeLine("interactor_ray_ctrl");
+					viewer_->removeText("interactor_ray_text_ctrl");
+					PreviousMeasure[0] = 0.0f;
+					PreviousMeasure[1] = 0.0f;
+					PreviousMeasure[2] = 0.0f;
+				}
+			}
+			else if(this->GetInteractor()->GetShiftKey())
+			{
+				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);
 			}
 		}
 
@@ -178,6 +354,9 @@ private:
 	unsigned int NumberOfClicks;
 	int PreviousPosition[2];
 	int ResetPixelDistance;
+	float PreviousMeasure[3];
+	pcl::PointCloud<pcl::PointXYZRGB>::Ptr pointsHolder_;
+	CloudViewer * viewer_;
 };
 
 
@@ -214,6 +393,7 @@ CloudViewer::CloudViewer(QWidget *parent) :
 		_gridCellSize(1),
 		_normalsStep(1),
 		_normalsScale(0.2),
+		_buildLocator(false),
 		_lastCameraOrientation(0,0,0),
 		_lastCameraPose(0,0,0),
 		_defaultBgColor(Qt::black),
@@ -230,11 +410,28 @@ CloudViewer::CloudViewer(QWidget *parent) :
 		argc, 
 		0, 
 		"PCLVisualizer", 
-		vtkSmartPointer<MyInteractorStyle>(new MyInteractorStyle()),
+		vtkSmartPointer<MyInteractorStyle>(new MyInteractorStyle(this)),
 		false);
 
 	_visualizer->setShowFPS(false);
 	
+	int viewport;
+	_visualizer->createViewPort (0,0,1.0, 1.0, viewport); // all 3d objects here
+	_visualizer->createViewPort (0,0,1.0, 1.0, viewport); // text overlay
+	_visualizer->getRendererCollection()->InitTraversal ();
+	vtkRenderer* renderer = NULL;
+	int i =0;
+	while ((renderer = _visualizer->getRendererCollection()->GetNextItem ()) != NULL)
+	{
+		renderer->SetLayer(i);
+		if(i==1)
+		{
+			_visualizer->getInteractorStyle()->SetDefaultRenderer(renderer);
+		}
+		++i;
+	}
+	_visualizer->getRenderWindow()->SetNumberOfLayers(3);
+
 	this->SetRenderWindow(_visualizer->getRenderWindow());
 
 	// Replaced by the second line, to avoid a crash in Mac OS X on close, as well as
@@ -252,7 +449,7 @@ CloudViewer::CloudViewer(QWidget *parent) :
 	_visualizer->setCameraPosition(
 				-1, 0, 0,
 				0, 0, 0,
-				0, 0, 1);
+				0, 0, 1, 1);
 #ifndef _WIN32
 	// Crash on startup on Windows (vtk issue)
 	this->addOrUpdateCoordinate("reference", Transform::getIdentity(), 0.2);
@@ -339,6 +536,9 @@ void CloudViewer::createMenu()
 	_aBackfaceCulling = new QAction("Backface culling", this);
 	_aBackfaceCulling->setCheckable(true);
 	_aBackfaceCulling->setChecked(true);
+	_aPolygonPicking = new QAction("Polygon picking", this);
+	_aPolygonPicking->setCheckable(true);
+	_aPolygonPicking->setChecked(false);
 
 	QMenu * cameraMenu = new QMenu("Camera", this);
 	cameraMenu->addAction(_aLockCamera);
@@ -385,6 +585,7 @@ void CloudViewer::createMenu()
 	_menu->addAction(_aSetFlatShading);
 	_menu->addAction(_aSetEdgeVisibility);
 	_menu->addAction(_aBackfaceCulling);
+	_menu->addAction(_aPolygonPicking);
 }
 
 void CloudViewer::saveSettings(QSettings & settings, const QString & group) const
@@ -557,7 +758,7 @@ bool CloudViewer::addCloud(
 	// add random color channel
 	 pcl::visualization::PointCloudColorHandler<pcl::PCLPointCloud2>::Ptr colorHandler;
 	 colorHandler.reset (new pcl::visualization::PointCloudColorHandlerRandom<pcl::PCLPointCloud2> (binaryCloud));
-	 if(_visualizer->addPointCloud (binaryCloud, colorHandler, origin, orientation, id))
+	 if(_visualizer->addPointCloud (binaryCloud, colorHandler, origin, orientation, id, 1))
 	 {
 		QColor c = Qt::gray;
 		if(color.isValid())
@@ -565,27 +766,27 @@ bool CloudViewer::addCloud(
 			c = color;
 		}
 		colorHandler.reset (new pcl::visualization::PointCloudColorHandlerCustom<pcl::PCLPointCloud2> (binaryCloud, c.red(), c.green(), c.blue()));
-		_visualizer->addPointCloud (binaryCloud, colorHandler, origin, orientation, id);
+		_visualizer->addPointCloud (binaryCloud, colorHandler, origin, orientation, id, 1);
 
 		// x,y,z
 		colorHandler.reset (new pcl::visualization::PointCloudColorHandlerGenericField<pcl::PCLPointCloud2> (binaryCloud, "x"));
-		_visualizer->addPointCloud (binaryCloud, colorHandler, origin, orientation, id);
+		_visualizer->addPointCloud (binaryCloud, colorHandler, origin, orientation, id, 1);
 		colorHandler.reset (new pcl::visualization::PointCloudColorHandlerGenericField<pcl::PCLPointCloud2> (binaryCloud, "y"));
-		_visualizer->addPointCloud (binaryCloud, colorHandler, origin, orientation, id);
+		_visualizer->addPointCloud (binaryCloud, colorHandler, origin, orientation, id, 1);
 		colorHandler.reset (new pcl::visualization::PointCloudColorHandlerGenericField<pcl::PCLPointCloud2> (binaryCloud, "z"));
-		_visualizer->addPointCloud (binaryCloud, colorHandler, origin, orientation, id);
+		_visualizer->addPointCloud (binaryCloud, colorHandler, origin, orientation, id, 1);
 
 		if(rgb)
 		{
 			//rgb
 			colorHandler.reset(new pcl::visualization::PointCloudColorHandlerRGBField<pcl::PCLPointCloud2>(binaryCloud));
-			_visualizer->addPointCloud (binaryCloud, colorHandler, origin, orientation, id);
+			_visualizer->addPointCloud (binaryCloud, colorHandler, origin, orientation, id, 1);
 		}
 		else if(hasIntensity)
 		{
 			//rgb
 			colorHandler.reset(new pcl::visualization::PointCloudColorHandlerGenericField<pcl::PCLPointCloud2>(binaryCloud, "intensity"));
-			_visualizer->addPointCloud (binaryCloud, colorHandler, origin, orientation, id);
+			_visualizer->addPointCloud (binaryCloud, colorHandler, origin, orientation, id, 1);
 		}
 		else if(previousColorIndex == 5)
 		{
@@ -596,11 +797,11 @@ bool CloudViewer::addCloud(
 		{
 			//normals
 			colorHandler.reset (new pcl::visualization::PointCloudColorHandlerGenericField<pcl::PCLPointCloud2> (binaryCloud, "normal_x"));
-			_visualizer->addPointCloud (binaryCloud, colorHandler, origin, orientation, id);
+			_visualizer->addPointCloud (binaryCloud, colorHandler, origin, orientation, id, 1);
 			colorHandler.reset (new pcl::visualization::PointCloudColorHandlerGenericField<pcl::PCLPointCloud2> (binaryCloud, "normal_y"));
-			_visualizer->addPointCloud (binaryCloud, colorHandler, origin, orientation, id);
+			_visualizer->addPointCloud (binaryCloud, colorHandler, origin, orientation, id, 1);
 			colorHandler.reset (new pcl::visualization::PointCloudColorHandlerGenericField<pcl::PCLPointCloud2> (binaryCloud, "normal_z"));
-			_visualizer->addPointCloud (binaryCloud, colorHandler, origin, orientation, id);
+			_visualizer->addPointCloud (binaryCloud, colorHandler, origin, orientation, id, 1);
 		}
 		else if(previousColorIndex > 5)
 		{
@@ -712,7 +913,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))
+	if(_visualizer->addPolygonMesh<pcl::PointXYZ>(cloud, polygons, id, 1))
 	{
 		_visualizer->getCloudActorMap()->find(id)->second.actor->GetProperty()->SetLighting(_aSetLighting->isChecked());
 		_visualizer->getCloudActorMap()->find(id)->second.actor->GetProperty()->SetInterpolation(_aSetFlatShading->isChecked()?VTK_FLAT:VTK_PHONG);
@@ -720,6 +921,13 @@ bool CloudViewer::addCloudMesh(
 		_visualizer->getCloudActorMap()->find(id)->second.actor->GetProperty()->SetBackfaceCulling(_aBackfaceCulling->isChecked());
 		_visualizer->getCloudActorMap()->find(id)->second.actor->GetProperty()->SetFrontfaceCulling(_frontfaceCulling);
 		_visualizer->updatePointCloudPose(id, pose.toEigen3f());
+		if(_buildLocator)
+		{
+			vtkSmartPointer<vtkOBBTree> tree = vtkSmartPointer<vtkOBBTree>::New();
+			tree->SetDataSet(_visualizer->getCloudActorMap()->find(id)->second.actor->GetMapper()->GetInput());
+			tree->BuildLocator();
+			_locators.insert(std::make_pair(id, tree));
+		}
 		_addedClouds.insert(id, pose);
 		return true;
 	}
@@ -738,7 +946,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))
+	if(_visualizer->addPolygonMesh<pcl::PointXYZRGB>(cloud, polygons, id, 1))
 	{
 		_visualizer->getCloudActorMap()->find(id)->second.actor->GetProperty()->SetLighting(_aSetLighting->isChecked());
 		_visualizer->getCloudActorMap()->find(id)->second.actor->GetProperty()->SetInterpolation(_aSetFlatShading->isChecked()?VTK_FLAT:VTK_PHONG);
@@ -746,6 +954,13 @@ bool CloudViewer::addCloudMesh(
 		_visualizer->getCloudActorMap()->find(id)->second.actor->GetProperty()->SetBackfaceCulling(_aBackfaceCulling->isChecked());
 		_visualizer->getCloudActorMap()->find(id)->second.actor->GetProperty()->SetFrontfaceCulling(_frontfaceCulling);
 		_visualizer->updatePointCloudPose(id, pose.toEigen3f());
+		if(_buildLocator)
+		{
+			vtkSmartPointer<vtkOBBTree> tree = vtkSmartPointer<vtkOBBTree>::New();
+			tree->SetDataSet(_visualizer->getCloudActorMap()->find(id)->second.actor->GetMapper()->GetInput());
+			tree->BuildLocator();
+			_locators.insert(std::make_pair(id, tree));
+		}
 		_addedClouds.insert(id, pose);
 		return true;
 	}
@@ -764,7 +979,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))
+	if(_visualizer->addPolygonMesh<pcl::PointXYZRGBNormal>(cloud, polygons, id, 1))
 	{
 		_visualizer->getCloudActorMap()->find(id)->second.actor->GetProperty()->SetLighting(_aSetLighting->isChecked());
 		_visualizer->getCloudActorMap()->find(id)->second.actor->GetProperty()->SetInterpolation(_aSetFlatShading->isChecked()?VTK_FLAT:VTK_PHONG);
@@ -772,6 +987,13 @@ bool CloudViewer::addCloudMesh(
 		_visualizer->getCloudActorMap()->find(id)->second.actor->GetProperty()->SetBackfaceCulling(_aBackfaceCulling->isChecked());
 		_visualizer->getCloudActorMap()->find(id)->second.actor->GetProperty()->SetFrontfaceCulling(_frontfaceCulling);
 		_visualizer->updatePointCloudPose(id, pose.toEigen3f());
+		if(_buildLocator)
+		{
+			vtkSmartPointer<vtkOBBTree> tree = vtkSmartPointer<vtkOBBTree>::New();
+			tree->SetDataSet(_visualizer->getCloudActorMap()->find(id)->second.actor->GetMapper()->GetInput());
+			tree->BuildLocator();
+			_locators.insert(std::make_pair(id, tree));
+		}
 		_addedClouds.insert(id, pose);
 		return true;
 	}
@@ -789,13 +1011,20 @@ bool CloudViewer::addCloudMesh(
 	}
 
 	UDEBUG("Adding %s with %d polygons", id.c_str(), (int)mesh->polygons.size());
-	if(_visualizer->addPolygonMesh(*mesh, id))
+	if(_visualizer->addPolygonMesh(*mesh, id, 1))
 	{
 		_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());
 		_visualizer->getCloudActorMap()->find(id)->second.actor->GetProperty()->SetFrontfaceCulling(_frontfaceCulling);
 		_visualizer->updatePointCloudPose(id, pose.toEigen3f());
+		if(_buildLocator)
+		{
+			vtkSmartPointer<vtkOBBTree> tree = vtkSmartPointer<vtkOBBTree>::New();
+			tree->SetDataSet(_visualizer->getCloudActorMap()->find(id)->second.actor->GetMapper()->GetInput());
+			tree->BuildLocator();
+			_locators.insert(std::make_pair(id, tree));
+		}
 		_addedClouds.insert(id, pose);
 		return true;
 	}
@@ -815,7 +1044,7 @@ bool CloudViewer::addCloudTextureMesh(
 	}
 
 	UDEBUG("Adding %s", id.c_str());
-	if(this->addTextureMesh(*textureMesh, texture, id))
+	if(this->addTextureMesh(*textureMesh, texture, id, 1))
 	{
 		_visualizer->getCloudActorMap()->find(id)->second.actor->GetProperty()->SetLighting(_aSetLighting->isChecked());
 		_visualizer->getCloudActorMap()->find(id)->second.actor->GetProperty()->SetInterpolation(_aSetFlatShading->isChecked()?VTK_FLAT:VTK_PHONG);
@@ -828,6 +1057,13 @@ bool CloudViewer::addCloudTextureMesh(
 			_visualizer->getCloudActorMap()->find(id)->second.actor->GetTexture()->SetBlendingMode(vtkTexture::VTK_TEXTURE_BLENDING_MODE_REPLACE);
 		}
 		_visualizer->updatePointCloudPose(id, pose.toEigen3f());
+		if(_buildLocator)
+		{
+			vtkSmartPointer<vtkOBBTree> tree = vtkSmartPointer<vtkOBBTree>::New();
+			tree->SetDataSet(_visualizer->getCloudActorMap()->find(id)->second.actor->GetMapper()->GetInput());
+			tree->BuildLocator();
+			_locators.insert(std::make_pair(id, tree));
+		}
 		_addedClouds.insert(id, pose);
 		return true;
 	}
@@ -853,20 +1089,13 @@ bool CloudViewer::addOctomap(const OctoMap * octomap, unsigned int treeDepth, bo
 		treeDepth = octomap->octree()->getTreeDepth();
 	}
 
-	//get the renderer of the visualizer object
+	removeOctomap();
-	vtkRenderer *renderer = _visualizer->getRenderWindow()->GetRenderers()->GetFirstRenderer();
 
 	if(!volumeRepresentation)
 	{
 		pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud = octomap->createCloud(treeDepth, obstacles.get(), 0, 0, false);
 		if(obstacles->size())
 		{
-			if(_octomapActor)
-			{
-				renderer->RemoveActor(_octomapActor);
-				_octomapActor = 0;
-			}
-
 			//vtkSmartPointer<vtkUnsignedCharArray> colors = vtkSmartPointer<vtkUnsignedCharArray>::New();
 			//colors->SetName("colors");
 			//colors->SetNumberOfComponents(3);
@@ -924,7 +1153,13 @@ bool CloudViewer::addOctomap(const OctoMap * octomap, unsigned int treeDepth, bo
 			octomapActor->GetProperty()->SetEdgeVisibility(_aSetEdgeVisibility->isChecked());
 			octomapActor->GetProperty()->SetLighting(_aSetLighting->isChecked());
 
+			_visualizer->getRendererCollection()->InitTraversal ();
+			vtkRenderer* renderer = NULL;
+			renderer = _visualizer->getRendererCollection()->GetNextItem ();
+			renderer = _visualizer->getRendererCollection()->GetNextItem ();
+			UASSERT(renderer);
 			renderer->AddActor(octomapActor);
+
 			_octomapActor = octomapActor.GetPointer();
 			return true;
 		}
@@ -933,11 +1168,6 @@ bool CloudViewer::addOctomap(const OctoMap * octomap, unsigned int treeDepth, bo
 	{
 		if(octomap->octree()->size())
 		{
-			if(_octomapActor)
-			{
-				renderer->RemoveActor(_octomapActor);
-				_octomapActor = 0;
-			}
 			// Create an image data
 			vtkSmartPointer<vtkImageData> imageData =
 					vtkSmartPointer<vtkImageData>::New();
@@ -1023,6 +1253,12 @@ bool CloudViewer::addOctomap(const OctoMap * octomap, unsigned int treeDepth, bo
 			volume->SetProperty(volumeProperty);
 			volume->SetScale(cellSize);
 			volume->SetPosition(minX, minY, minZ);
+
+			_visualizer->getRendererCollection()->InitTraversal ();
+			vtkRenderer* renderer = NULL;
+			renderer = _visualizer->getRendererCollection()->GetNextItem ();
+			renderer = _visualizer->getRendererCollection()->GetNextItem ();
+			UASSERT(renderer);
 			renderer->AddViewProp(volume);
 
 			// 3D texture mode. For coverage.
@@ -1048,7 +1284,11 @@ void CloudViewer::removeOctomap()
 #ifdef RTABMAP_OCTOMAP
 	if(_octomapActor)
 	{
-		vtkRenderer *renderer = _visualizer->getRenderWindow()->GetRenderers()->GetFirstRenderer();
+		_visualizer->getRendererCollection()->InitTraversal ();
+		vtkRenderer* renderer = NULL;
+		renderer = _visualizer->getRendererCollection()->GetNextItem ();
+		renderer = _visualizer->getRendererCollection()->GetNextItem ();
+		UASSERT(renderer);
 		renderer->RemoveActor(_octomapActor);
 		_octomapActor = 0;
 	}
@@ -1194,7 +1434,6 @@ bool CloudViewer::addTextureMesh (
   // set mapper
   actor->SetMapper (mapper);
 
-
   //_visualizer->addActorToRenderer (actor, viewport);
   // Add it to all renderers
 	_visualizer->getRendererCollection()->InitTraversal ();
@@ -1280,7 +1519,7 @@ bool CloudViewer::addOccupancyGridMap(
 		coordinates.push_back(Eigen::Vector2f(0,0));
 		mesh->tex_coordinates.push_back(coordinates);
 
-		this->addTextureMesh(*mesh, map8U, "map");
+		this->addTextureMesh(*mesh, map8U, "map", 1);
 		setCloudOpacity("map", opacity);
 	}
 	return true;
@@ -1297,7 +1536,8 @@ void CloudViewer::removeOccupancyGridMap()
 void CloudViewer::addOrUpdateCoordinate(
 			const std::string & id,
 			const Transform & transform,
-			double scale)
+			double scale,
+			bool foreground)
 {
 	if(id.empty())
 	{
@@ -1311,10 +1551,10 @@ void CloudViewer::addOrUpdateCoordinate(
 	{
 		_coordinates.insert(id);
 #if PCL_VERSION_COMPARE(>=, 1, 7, 2)
-		_visualizer->addCoordinateSystem(scale, transform.toEigen3f(), id);
+		_visualizer->addCoordinateSystem(scale, transform.toEigen3f(), id, foreground?2:1);
 #else
 		// Well, on older versions, just update the main coordinate
-		_visualizer->addCoordinateSystem(scale, transform.toEigen3f(), 0);
+		_visualizer->addCoordinateSystem(scale, transform.toEigen3f(), foreground?2:1);
 #endif
 	}
 }
@@ -1370,7 +1610,8 @@ void CloudViewer::addOrUpdateLine(
 			const Transform & from,
 			const Transform & to,
 			const QColor & color,
-			bool arrow)
+			bool arrow,
+			bool foreground)
 {
 	if(id.empty())
 	{
@@ -1395,12 +1636,13 @@ void CloudViewer::addOrUpdateLine(
 
 		if(arrow)
 		{
-			_visualizer->addArrow(pt2, pt1, c.redF(), c.greenF(), c.blueF(), false, id);
+			_visualizer->addArrow(pt2, pt1, c.redF(), c.greenF(), c.blueF(), false, id, foreground?2:1);
 		}
 		else
 		{
-			_visualizer->addLine(pt2, pt1, c.redF(), c.greenF(), c.blueF(), id);
+			_visualizer->addLine(pt2, pt1, c.redF(), c.greenF(), c.blueF(), id, foreground?2:1);
 		}
+		_visualizer->setShapeRenderingProperties(pcl::visualization::PCL_VISUALIZER_OPACITY, c.alphaF(), id);
 	}
 }
 
@@ -1429,6 +1671,124 @@ void CloudViewer::removeAllLines()
 	UASSERT(_lines.empty());
 }
 
+void CloudViewer::addOrUpdateSphere(
+			const std::string & id,
+			const Transform & pose,
+			float radius,
+			const QColor & color,
+			bool foreground)
+{
+	if(id.empty())
+	{
+		UERROR("id should not be empty!");
+		return;
+	}
+
+	removeSphere(id);
+
+	if(!pose.isNull())
+	{
+		_spheres.insert(id);
+
+		QColor c = Qt::gray;
+		if(color.isValid())
+		{
+			c = color;
+		}
+
+		pcl::PointXYZ center(pose.x(), pose.y(), pose.z());
+		_visualizer->addSphere(center, radius, c.redF(), c.greenF(), c.blueF(), id, foreground?2:1);
+		_visualizer->setShapeRenderingProperties(pcl::visualization::PCL_VISUALIZER_OPACITY, c.alphaF(), id);
+	}
+}
+
+void CloudViewer::removeSphere(const std::string & id)
+{
+	if(id.empty())
+	{
+		UERROR("id should not be empty!");
+		return;
+	}
+
+	if(_spheres.find(id) != _spheres.end())
+	{
+		_visualizer->removeShape(id);
+		_spheres.erase(id);
+	}
+}
+
+void CloudViewer::removeAllSpheres()
+{
+	std::set<std::string> spheres = _spheres;
+	for(std::set<std::string>::iterator iter = spheres.begin(); iter!=spheres.end(); ++iter)
+	{
+		this->removeSphere(*iter);
+	}
+	UASSERT(_spheres.empty());
+}
+
+void CloudViewer::addOrUpdateCube(
+			const std::string & id,
+			const Transform & pose,
+			float width,
+			float height,
+			float depth,
+			const QColor & color,
+			bool wireframe,
+			bool foreground)
+{
+	if(id.empty())
+	{
+		UERROR("id should not be empty!");
+		return;
+	}
+
+	removeCube(id);
+
+	if(!pose.isNull())
+	{
+		_cubes.insert(id);
+
+		QColor c = Qt::gray;
+		if(color.isValid())
+		{
+			c = color;
+		}
+		_visualizer->addCube(Eigen::Vector3f(pose.x(), pose.y(), pose.z()), pose.getQuaternionf(), width, height, depth, id, foreground?2:1);
+		if(wireframe)
+		{
+			_visualizer->setShapeRenderingProperties(pcl::visualization::PCL_VISUALIZER_REPRESENTATION, pcl::visualization::PCL_VISUALIZER_REPRESENTATION_WIREFRAME, id);
+		}
+		_visualizer->setShapeRenderingProperties(pcl::visualization::PCL_VISUALIZER_COLOR, c.redF(), c.greenF(), c.blueF(), id);
+		_visualizer->setShapeRenderingProperties(pcl::visualization::PCL_VISUALIZER_OPACITY, c.alphaF(), id);
+	}
+}
+
+void CloudViewer::removeCube(const std::string & id)
+{
+	if(id.empty())
+	{
+		UERROR("id should not be empty!");
+		return;
+	}
+
+	if(_cubes.find(id) != _cubes.end())
+	{
+		_visualizer->removeShape(id);
+		_cubes.erase(id);
+	}
+}
+
+void CloudViewer::removeAllCubes()
+{
+	std::set<std::string> cubes = _cubes;
+	for(std::set<std::string>::iterator iter = cubes.begin(); iter!=cubes.end(); ++iter)
+	{
+		this->removeCube(*iter);
+	}
+	UASSERT(_cubes.empty());
+}
+
 static const float frustum_vertices[] = {
     0.0f,  0.0f, 0.0f,
 	1.0f, 1.0f, 1.0f,
@@ -1499,8 +1859,9 @@ void CloudViewer::addOrUpdateFrustum(
 			}
 			pcl::toPCLPointCloud2(frustumPoints, mesh.cloud);
 			mesh.polygons.push_back(vertices);
-			_visualizer->addPolylineFromPolygonMesh(mesh, id);
+			_visualizer->addPolylineFromPolygonMesh(mesh, id, 1);
 			_visualizer->setShapeRenderingProperties(pcl::visualization::PCL_VISUALIZER_COLOR, c.redF(), c.greenF(), c.blueF(), id);
+			_visualizer->setShapeRenderingProperties(pcl::visualization::PCL_VISUALIZER_OPACITY, c.alphaF(), id);
 		}
 #if PCL_VERSION_COMPARE(>=, 1, 7, 2)
 		if(!this->updateFrustumPose(id, transform))
@@ -1612,8 +1973,9 @@ void CloudViewer::addOrUpdateGraph(
 		}
 		pcl::toPCLPointCloud2(*graph, mesh.cloud);
 		mesh.polygons.push_back(vertices);
-		_visualizer->addPolylineFromPolygonMesh(mesh, id);
+		_visualizer->addPolylineFromPolygonMesh(mesh, id, 1);
 		_visualizer->setShapeRenderingProperties(pcl::visualization::PCL_VISUALIZER_COLOR, color.redF(), color.greenF(), color.blueF(), id);
+		_visualizer->setShapeRenderingProperties(pcl::visualization::PCL_VISUALIZER_OPACITY, color.alphaF(), id);
 
 		this->addCloud(id+"_nodes", graph, Transform::getIdentity(), color);
 		this->setCloudPointSize(id+"_nodes", 5);
@@ -1651,7 +2013,8 @@ void CloudViewer::addOrUpdateText(
 			const std::string & text,
 			const Transform & position,
 			double scale,
-			const QColor & color)
+			const QColor & color,
+			bool foreground)
 {
 	if(id.empty())
 	{
@@ -1659,7 +2022,7 @@ void CloudViewer::addOrUpdateText(
 		return;
 	}
 
-	removeCoordinate(id);
+	removeText(id);
 
 	if(!position.isNull())
 	{
@@ -1671,7 +2034,8 @@ void CloudViewer::addOrUpdateText(
 				color.redF(),
 				color.greenF(),
 				color.blueF(),
-				id);
+				id,
+				foreground?2:1);
 	}
 }
 
@@ -1798,14 +2162,14 @@ void CloudViewer::resetCamera()
 			_visualizer->setCameraPosition(
 					pt.x, pt.y, pt.z,
 					_lastPose.x(), _lastPose.y(), _lastPose.z(),
-					0, 0, 1);
+					0, 0, 1, 1);
 		}
 		else
 		{
 			_visualizer->setCameraPosition(
 					pt.x, pt.y, pt.z,
 					_lastPose.x(), _lastPose.y(), _lastPose.z(),
-					_lastPose.r31(), _lastPose.r32(), _lastPose.r33());
+					_lastPose.r31(), _lastPose.r32(), _lastPose.r33(), 1);
 		}
 	}
 	else
@@ -1813,7 +2177,7 @@ void CloudViewer::resetCamera()
 		_visualizer->setCameraPosition(
 				-1, 0, 0,
 				0, 0, 0,
-				0, 0, 1);
+				0, 0, 1, 1);
 	}
 	this->update();
 }
@@ -1821,6 +2185,7 @@ void CloudViewer::resetCamera()
 void CloudViewer::removeAllClouds()
 {
 	_addedClouds.clear();
+	_locators.clear();
 	_visualizer->removeAllPointClouds();
 }
 
@@ -1831,6 +2196,7 @@ bool CloudViewer::removeCloud(const std::string & id)
 	_visualizer->removePointCloud(id+"-normals");
 	_addedClouds.remove(id); // remove after visualizer
 	_addedClouds.remove(id+"-normals");
+	_locators.erase(id);
 	return success;
 }
 
@@ -1867,6 +2233,28 @@ void CloudViewer::setBackfaceCulling(bool enabled, bool frontfaceCulling)
 	this->update();
 }
 
+void CloudViewer::setPolygonPicking(bool enabled)
+{
+	_aPolygonPicking->setChecked(enabled);
+
+	if(!_aPolygonPicking->isChecked())
+	{
+		vtkSmartPointer<vtkPointPicker> pp = vtkSmartPointer<vtkPointPicker>::New ();
+		pp->SetTolerance (pp->GetTolerance());
+		this->GetInteractor()->SetPicker (pp);
+		setMouseTracking(false);
+	}
+	else
+	{
+		vtkSmartPointer<vtkCellPicker> pp = vtkSmartPointer<vtkCellPicker>::New ();
+		pp->SetTolerance (pp->GetTolerance());
+		this->GetInteractor()->SetPicker (pp);
+		setMouseTracking(true);
+	}
+}
+
+
+
 void CloudViewer::setRenderingRate(double rate)
 {
 	_renderingRate = rate;
@@ -1937,7 +2325,7 @@ void CloudViewer::setCameraPosition(
 		float upX, float upY, float upZ)
 {
 	_lastCameraOrientation= _lastCameraPose= cv::Vec3f(0,0,0);
-	_visualizer->setCameraPosition(x,y,z, focalX,focalY,focalX, upX,upY,upZ);
+	_visualizer->setCameraPosition(x,y,z, focalX,focalY,focalX, upX,upY,upZ, 1);
 }
 
 void CloudViewer::updateCameraTargetPosition(const Transform & pose)
@@ -1975,7 +2363,7 @@ void CloudViewer::updateCameraTargetPosition(const Transform & pose)
 			}
 			pcl::toPCLPointCloud2(*_trajectory, mesh.cloud);
 			mesh.polygons.push_back(vertices);
-			_visualizer->addPolylineFromPolygonMesh(mesh, "trajectory");
+			_visualizer->addPolylineFromPolygonMesh(mesh, "trajectory", 1);
 		}
 
 		if(pose != _lastPose || _lastPose.isNull())
@@ -2051,12 +2439,10 @@ void CloudViewer::updateCameraTargetPosition(const Transform & pose)
 				this->addOrUpdateCoordinate("reference", pose, 0.2);
 			}
 
-			vtkRenderer* renderer = _visualizer->getRendererCollection()->GetFirstRenderer();
+			_visualizer->setCameraPosition(
-			vtkSmartPointer<vtkCamera> cam = renderer->GetActiveCamera ();
+				cameras.front().pos[0], cameras.front().pos[1], cameras.front().pos[2],
-			cam->SetPosition (cameras.front().pos[0], cameras.front().pos[1], cameras.front().pos[2]);
+				cameras.front().focal[0], cameras.front().focal[1], cameras.front().focal[2],
-			cam->SetFocalPoint (cameras.front().focal[0], cameras.front().focal[1], cameras.front().focal[2]);
+				cameras.front().view[0], cameras.front().view[1], cameras.front().view[2], 1);
-			cam->SetViewUp (cameras.front().view[0], cameras.front().view[1], cameras.front().view[2]);
-			renderer->ResetCameraClippingRange();
 		}
 	}
 
@@ -2312,14 +2698,14 @@ void CloudViewer::addGrid()
 		{
 			//over x
 			name = uFormat("line%d", ++id);
-			_visualizer->addLine(pcl::PointXYZ(i, min, 0.0f), pcl::PointXYZ(i, max, 0.0f), r, g, b, name);
+			_visualizer->addLine(pcl::PointXYZ(i, min, 0.0f), pcl::PointXYZ(i, max, 0.0f), r, g, b, name, 1);
 			_gridLines.push_back(name);
 			//over y or z
 			name = uFormat("line%d", ++id);
 			_visualizer->addLine(
 					pcl::PointXYZ(min, i, 0),
 					pcl::PointXYZ(max, i, 0),
-					r, g, b, name);
+					r, g, b, name, 1);
 			_gridLines.push_back(name);
 		}
 	}
@@ -2382,6 +2768,11 @@ void CloudViewer::setNormalsScale(float scale)
 	}
 }
 
+void CloudViewer::buildPickingLocator(bool enable)
+{
+	_buildLocator = enable;
+}
+
 Eigen::Vector3f rotatePointAroundAxe(
 		const Eigen::Vector3f & point,
 		const Eigen::Vector3f & axis,
@@ -2519,7 +2910,7 @@ void CloudViewer::keyPressEvent(QKeyEvent * event)
 		_visualizer->setCameraPosition(
 			cameras.front().pos[0], cameras.front().pos[1], cameras.front().pos[2],
 			cameras.front().focal[0], cameras.front().focal[1], cameras.front().focal[2],
-			cameras.front().view[0], cameras.front().view[1], cameras.front().view[2]);
+			cameras.front().view[0], cameras.front().view[1], cameras.front().view[2], 1);
 
 		update();
 
@@ -2576,7 +2967,7 @@ void CloudViewer::mouseMoveEvent(QMouseEvent * event)
 		_visualizer->setCameraPosition(
 			cameras.front().pos[0], cameras.front().pos[1], cameras.front().pos[2],
 			cameras.front().focal[0], cameras.front().focal[1], cameras.front().focal[2],
-			cameras.front().view[0], cameras.front().view[1], cameras.front().view[2]);
+			cameras.front().view[0], cameras.front().view[1], cameras.front().view[2], 1);
 
 	}
 	this->update();
@@ -2742,6 +3133,10 @@ void CloudViewer::handleAction(QAction * a)
 	{
 		this->setBackfaceCulling(_aBackfaceCulling->isChecked(), _frontfaceCulling);
 	}
+	else if(a == _aPolygonPicking)
+	{
+		this->setPolygonPicking(_aPolygonPicking->isChecked());
+	}
 }
 
 } /* namespace rtabmap */

guilib/src/DatabaseViewer.cpp

@@ -82,6 +82,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #include "rtabmap/gui/RecoveryState.h"
 #include <pcl/io/pcd_io.h>
 #include <pcl/io/ply_io.h>
+#include <pcl/io/obj_io.h>
 #include <pcl/filters/voxel_grid.h>
 #include <pcl/common/transforms.h>
 #include <pcl/common/common.h>
@@ -246,6 +247,11 @@ DatabaseViewer::DatabaseViewer(const QString & ini, QWidget * parent) :
 	connect(ui_->actionPoses_KML, SIGNAL(triggered()), this , SLOT(exportPosesKML()));
 	connect(ui_->actionGPS_TXT, SIGNAL(triggered()), this , SLOT(exportGPS_TXT()));
 	connect(ui_->actionGPS_KML, SIGNAL(triggered()), this , SLOT(exportGPS_KML()));
+	connect(ui_->actionExport_saved_2D_map, SIGNAL(triggered()), this , SLOT(exportSaved2DMap()));
+	connect(ui_->actionImport_2D_map, SIGNAL(triggered()), this , SLOT(import2DMap()));
+	connect(ui_->actionView_optimized_mesh, SIGNAL(triggered()), this , SLOT(viewOptimizedMesh()));
+	connect(ui_->actionExport_optimized_mesh, SIGNAL(triggered()), this , SLOT(exportOptimizedMesh()));
+	connect(ui_->actionUpdate_optimized_mesh, SIGNAL(triggered()), this , SLOT(updateOptimizedMesh()));
 	connect(ui_->actionView_3D_map, SIGNAL(triggered()), this, SLOT(view3DMap()));
 	connect(ui_->actionGenerate_3D_map_pcd, SIGNAL(triggered()), this, SLOT(generate3DMap()));
 	connect(ui_->actionDetect_more_loop_closures, SIGNAL(triggered()), this, SLOT(detectMoreLoopClosures()));
@@ -266,9 +272,17 @@ DatabaseViewer::DatabaseViewer(const QString & ini, QWidget * parent) :
 	ui_->pushButton_reset->setEnabled(false);
 	ui_->pushButton_reject->setEnabled(false);
 
+	ui_->menuEdit->setEnabled(false);
+	ui_->actionGenerate_3D_map_pcd->setEnabled(false);
+	ui_->actionExport->setEnabled(false);
+	ui_->actionExtract_images->setEnabled(false);
 	ui_->menuExport_poses->setEnabled(false);
 	ui_->menuExport_GPS->setEnabled(false);
 	ui_->actionPoses_KML->setEnabled(false);
+	ui_->actionExport_saved_2D_map->setEnabled(false);
+	ui_->actionView_optimized_mesh->setEnabled(false);
+	ui_->actionExport_optimized_mesh->setEnabled(false);
+	ui_->actionUpdate_optimized_mesh->setEnabled(false);
 
 	ui_->horizontalSlider_A->setTracking(false);
 	ui_->horizontalSlider_B->setTracking(false);
@@ -922,9 +936,18 @@ bool DatabaseViewer::closeDatabase()
 		generatedLocalMapsInfo_.clear();
 		ui_->graphViewer->clearAll();
 		occupancyGridViewer_->clear();
+		ui_->menuEdit->setEnabled(false);
+		ui_->actionGenerate_3D_map_pcd->setEnabled(false);
+		ui_->actionExport->setEnabled(false);
+		ui_->actionExtract_images->setEnabled(false);
 		ui_->menuExport_poses->setEnabled(false);
 		ui_->menuExport_GPS->setEnabled(false);
 		ui_->actionPoses_KML->setEnabled(false);
+		ui_->actionExport_saved_2D_map->setEnabled(false);
+		ui_->actionImport_2D_map->setEnabled(false);
+		ui_->actionView_optimized_mesh->setEnabled(false);
+		ui_->actionExport_optimized_mesh->setEnabled(false);
+		ui_->actionUpdate_optimized_mesh->setEnabled(false);
 		ui_->checkBox_showOptimized->setEnabled(false);
 		ui_->toolBox_statistics->clear();
 		databaseFileName_.clear();
@@ -1411,7 +1434,7 @@ void DatabaseViewer::extractImages()
 			else if(!data.imageRaw().empty() && !data.depthRaw().empty())
 			{
 				cv::imwrite(QString("%1/rgb/%2.%3").arg(path).arg(id).arg(ext).toStdString(), data.imageRaw());
-				cv::imwrite(QString("%1/depth/%2.png").arg(path).arg(id).toStdString(), data.depthRaw());
+				cv::imwrite(QString("%1/depth/%2.png").arg(path).arg(id).toStdString(), data.depthRaw().type()==CV_32FC1?util2d::cvtDepthFromFloat(data.depthRaw()):data.depthRaw());
 				UINFO(QString("Saved rgb/%1.%2 and depth/%1.png").arg(id).arg(ext).toStdString().c_str());
 				++imagesExported;
 			}
@@ -1456,8 +1479,18 @@ void DatabaseViewer::updateIds()
 	ui_->label_ignoreINtermediateNdoes->setVisible(false);
 	ui_->label_alignPosesWithGroundTruth->setVisible(false);
 	ui_->label_alignScansCloudsWithGroundTruth->setVisible(false);
+	ui_->menuEdit->setEnabled(true);
+	ui_->actionGenerate_3D_map_pcd->setEnabled(true);
+	ui_->actionExport->setEnabled(true);
+	ui_->actionExtract_images->setEnabled(true);
+	ui_->menuExport_poses->setEnabled(false);
 	ui_->menuExport_GPS->setEnabled(false);
 	ui_->actionPoses_KML->setEnabled(false);
+	ui_->actionExport_saved_2D_map->setEnabled(false);
+	ui_->actionImport_2D_map->setEnabled(false);
+	ui_->actionView_optimized_mesh->setEnabled(false);
+	ui_->actionExport_optimized_mesh->setEnabled(false);
+	ui_->actionUpdate_optimized_mesh->setEnabled(uStrNumCmp(dbDriver_->getDatabaseVersion(), "0.13.0") >= 0);
 	links_.clear();
 	linksAdded_.clear();
 	linksRefined_.clear();
@@ -1526,7 +1559,7 @@ void DatabaseViewer::updateIds()
 		previousPose=p;
 
 		//links
-		bool addPose = false;
+		bool addPose = links.find(ids_[i]) == links.end();
 		for(std::multimap<int, Link>::iterator jter=links.find(ids_[i]); jter!=links.end() && jter->first == ids_[i]; ++jter)
 		{
 			if(jter->second.type() == Link::kNeighborMerged)
@@ -1618,6 +1651,17 @@ void DatabaseViewer::updateIds()
 		ui_->actionPoses_KML->setEnabled(groundTruthPoses_.empty());
 	}
 
+	float xMin, yMin, cellSize;
+	bool hasMap = !dbDriver_->load2DMap(xMin, yMin, cellSize).empty();
+	ui_->actionExport_saved_2D_map->setEnabled(hasMap);
+	ui_->actionImport_2D_map->setEnabled(hasMap);
+
+	if(!dbDriver_->loadOptimizedMesh().empty())
+	{
+		ui_->actionView_optimized_mesh->setEnabled(true);
+		ui_->actionExport_optimized_mesh->setEnabled(true);
+	}
+
 	UINFO("Loaded %d ids, %d poses and %d links", (int)ids_.size(), (int)odomPoses_.size(), (int)links_.size());
 
 	if(ids_.size() && ui_->toolBox_statistics->isVisible())
@@ -2318,6 +2362,375 @@ void DatabaseViewer::exportGPS(int format)
 	}
 }
 
+void DatabaseViewer::exportSaved2DMap()
+{
+	if(!dbDriver_)
+	{
+		QMessageBox::warning(this, tr("Cannot export 2D map"), tr("A database must must loaded first...\nUse File->Open database."));
+		return;
+	}
+
+	float xMin, yMin, cellSize;
+	cv::Mat map = dbDriver_->load2DMap(xMin, yMin, cellSize);
+	if(map.empty())
+	{
+		QMessageBox::warning(this, tr("Cannot export 2D map"), tr("The database doesn't contain a saved 2D map."));
+	}
+	else
+	{
+		cv::Mat map8U = rtabmap::util3d::convertMap2Image8U(map, true);
+		QString name = QFileInfo(databaseFileName_.c_str()).baseName();
+		QString path = QFileDialog::getSaveFileName(
+				this,
+				tr("Save File"),
+				pathDatabase_+"/" + name + ".pgm",
+				tr("Map (*.pgm)"));
+
+		if(!path.isEmpty())
+		{
+			if(QFileInfo(path).suffix() == "")
+			{
+				path += ".pgm";
+			}
+			cv::imwrite(path.toStdString(), map8U);
+			QMessageBox::information(this, tr("Export 2D map"), tr("Exported %1!").arg(path));
+		}
+	}
+}
+
+void DatabaseViewer::import2DMap()
+{
+	if(!dbDriver_)
+	{
+		QMessageBox::warning(this, tr("Cannot import 2D map"), tr("A database must must loaded first...\nUse File->Open database."));
+		return;
+	}
+
+	float xMin, yMin, cellSize;
+	cv::Mat mapOrg = dbDriver_->load2DMap(xMin, yMin, cellSize);
+	if(mapOrg.empty())
+	{
+		QMessageBox::warning(this, tr("Cannot import 2D map"), tr("The database doesn't contain a saved 2D map."));
+	}
+	else
+	{
+		QString path = QFileDialog::getOpenFileName(
+						this,
+						tr("Open File"),
+						pathDatabase_,
+						tr("Map (*.pgm)"));
+		if(!path.isEmpty())
+		{
+			cv::Mat map8U = cv::imread(path.toStdString(), cv::IMREAD_UNCHANGED);
+			cv::Mat map = rtabmap::util3d::convertImage8U2Map(map8U, true);
+
+			if(mapOrg.cols == map.cols && mapOrg.rows == map8U.rows)
+			{
+				dbDriver_->save2DMap(map, xMin, yMin, cellSize);
+				QMessageBox::information(this, tr("Import 2D map"), tr("Imported %1!").arg(path));
+			}
+			else
+			{
+				QMessageBox::warning(this, tr("Import 2D map"), tr("Cannot import %1 as its size doesn't match the current saved map. Import 2D Map action should only be used to modify the map saved in the database.").arg(path));
+			}
+		}
+	}
+}
+
+void DatabaseViewer::viewOptimizedMesh()
+{
+	if(!dbDriver_)
+	{
+		QMessageBox::warning(this, tr("Cannot view optimized mesh"), tr("A database must must loaded first...\nUse File->Open database."));
+		return;
+	}
+
+	std::vector<std::vector<std::vector<unsigned int> > > polygons;
+#if PCL_VERSION_COMPARE(>=, 1, 8, 0)
+	std::vector<std::vector<Eigen::Vector2f, Eigen::aligned_allocator<Eigen::Vector2f> > > texCoords;
+#else
+	std::vector<std::vector<Eigen::Vector2f> > texCoords;
+#endif
+	cv::Mat textures;
+	cv::Mat cloudMat = dbDriver_->loadOptimizedMesh(&polygons, &texCoords, &textures);
+	if(cloudMat.empty())
+	{
+		QMessageBox::warning(this, tr("Cannot view optimized mesh"), tr("The database doesn't contain a saved optimized mesh."));
+	}
+	else
+	{
+		CloudViewer * viewer = new CloudViewer(this);
+		viewer->setWindowFlags(Qt::Window);
+		viewer->setAttribute(Qt::WA_DeleteOnClose);
+		viewer->buildPickingLocator(true);
+		if(!textures.empty())
+		{
+			pcl::TextureMeshPtr mesh = util3d::assembleTextureMesh(cloudMat, polygons, texCoords, textures, true);
+			util3d::fixTextureMeshForVisualization(*mesh);
+			viewer->setWindowTitle("Optimized Textured Mesh");
+			viewer->setPolygonPicking(true);
+			viewer->addCloudTextureMesh("mesh", mesh, textures);
+		}
+		else if(polygons.size() == 1)
+		{
+			pcl::PolygonMeshPtr mesh = util3d::assemblePolygonMesh(cloudMat, polygons.at(0));
+			viewer->setWindowTitle("Optimized Mesh");
+			viewer->setPolygonPicking(true);
+			viewer->addCloudMesh("mesh", mesh);
+		}
+		else
+		{
+			LaserScan scan = LaserScan::backwardCompatibility(cloudMat);
+			pcl::PCLPointCloud2::Ptr cloud = util3d::laserScanToPointCloud2(scan);
+			viewer->setWindowTitle("Optimized Point Cloud");
+			viewer->addCloud("mesh", cloud, Transform::getIdentity(), scan.hasRGB(), scan.hasNormals(), scan.hasIntensity());
+		}
+		viewer->show();
+	}
+}
+
+void DatabaseViewer::exportOptimizedMesh()
+{
+	if(!dbDriver_)
+	{
+		QMessageBox::warning(this, tr("Cannot export optimized mesh"), tr("A database must must loaded first...\nUse File->Open database."));
+		return;
+	}
+
+	std::vector<std::vector<std::vector<unsigned int> > > polygons;
+#if PCL_VERSION_COMPARE(>=, 1, 8, 0)
+	std::vector<std::vector<Eigen::Vector2f, Eigen::aligned_allocator<Eigen::Vector2f> > > texCoords;
+#else
+	std::vector<std::vector<Eigen::Vector2f> > texCoords;
+#endif
+	cv::Mat textures;
+	cv::Mat cloudMat = dbDriver_->loadOptimizedMesh(&polygons, &texCoords, &textures);
+	if(cloudMat.empty())
+	{
+		QMessageBox::warning(this, tr("Cannot export optimized mesh"), tr("The database doesn't contain a saved optimized mesh."));
+	}
+	else
+	{
+		QString name = QFileInfo(databaseFileName_.c_str()).baseName();
+
+		if(!textures.empty())
+		{
+			pcl::TextureMeshPtr mesh = util3d::assembleTextureMesh(cloudMat, polygons, texCoords, textures);
+			QString path = QFileDialog::getSaveFileName(
+					this,
+					tr("Save File"),
+					pathDatabase_+"/" + name + ".obj",
+					tr("Mesh (*.obj)"));
+
+			if(!path.isEmpty())
+			{
+				if(QFileInfo(path).suffix() == "")
+				{
+					path += ".obj";
+				}
+				QString baseName = QFileInfo(path).baseName();
+				if(mesh->tex_materials.size() == 1)
+				{
+					mesh->tex_materials.at(0).tex_file = baseName.toStdString() + ".png";
+					cv::imwrite((QFileInfo(path).absoluteDir().absolutePath()+QDir::separator()+baseName).toStdString() + ".png", textures);
+				}
+				else
+				{
+					for(unsigned int i=0; i<mesh->tex_materials.size(); ++i)
+					{
+						mesh->tex_materials.at(i).tex_file = (baseName+QDir::separator()+QString::number(i)+".png").toStdString();
+						UASSERT((i+1)*textures.rows <= (unsigned int)textures.cols);
+						cv::imwrite((QFileInfo(path).absoluteDir().absolutePath()+QDir::separator()+baseName+QDir::separator()+QString::number(i)+".png").toStdString(), textures(cv::Range::all(), cv::Range(i*textures.rows, (i+1)*textures.rows)));
+					}
+				}
+				pcl::io::saveOBJFile(path.toStdString(), *mesh);
+
+				QMessageBox::information(this, tr("Export Textured Mesh"), tr("Exported %1!").arg(path));
+			}
+		}
+		else if(polygons.size() == 1)
+		{
+			pcl::PolygonMeshPtr mesh = util3d::assemblePolygonMesh(cloudMat, polygons.at(0));
+			QString path = QFileDialog::getSaveFileName(
+					this,
+					tr("Save File"),
+					pathDatabase_+"/" + name + ".ply",
+					tr("Mesh (*.ply)"));
+
+			if(!path.isEmpty())
+			{
+				if(QFileInfo(path).suffix() == "")
+				{
+					path += ".ply";
+				}
+				pcl::io::savePLYFileBinary(path.toStdString(), *mesh);
+				QMessageBox::information(this, tr("Export Mesh"), tr("Exported %1!").arg(path));
+			}
+		}
+		else
+		{
+			QString path = QFileDialog::getSaveFileName(
+					this,
+					tr("Save File"),
+					pathDatabase_+"/" + name + ".ply",
+					tr("Point cloud data (*.ply *.pcd)"));
+
+			if(!path.isEmpty())
+			{
+				if(QFileInfo(path).suffix() == "")
+				{
+					path += ".ply";
+				}
+				bool success = false;
+				pcl::PCLPointCloud2::Ptr cloud = util3d::laserScanToPointCloud2(LaserScan::backwardCompatibility(cloudMat));
+				if(QFileInfo(path).suffix() == "pcd")
+				{
+					success = pcl::io::savePCDFile(path.toStdString(), *cloud) == 0;
+				}
+				else
+				{
+					success = pcl::io::savePLYFile(path.toStdString(), *cloud) == 0;
+				}
+				if(success)
+				{
+					QMessageBox::information(this, tr("Export Point Cloud"), tr("Exported %1!").arg(path));
+				}
+				else
+				{
+					QMessageBox::critical(this, tr("Export Point Cloud"), tr("Failed exporting %1!").arg(path));
+				}
+			}
+		}
+	}
+}
+
+void DatabaseViewer::updateOptimizedMesh()
+{
+	if(!ids_.size() || !dbDriver_)
+	{
+		QMessageBox::warning(this, tr("Cannot generate a graph"), tr("The database is empty..."));
+		return;
+	}
+
+	if(graphes_.empty())
+	{
+		this->updateGraphView();
+		if(graphes_.empty() || ui_->horizontalSlider_iterations->maximum() != (int)graphes_.size()-1)
+		{
+			QMessageBox::warning(this, tr("Cannot generate a graph"), tr("No graph in database?!"));
+			return;
+		}
+	}
+
+	std::map<int, Transform> optimizedPoses;
+	if(ui_->checkBox_alignScansCloudsWithGroundTruth->isChecked() && !groundTruthPoses_.empty())
+	{
+		optimizedPoses = groundTruthPoses_;
+	}
+	else
+	{
+		optimizedPoses = uValueAt(graphes_, ui_->horizontalSlider_iterations->value());
+	}
+	if(ui_->groupBox_posefiltering->isChecked())
+	{
+		optimizedPoses = graph::radiusPosesFiltering(optimizedPoses,
+				ui_->doubleSpinBox_posefilteringRadius->value(),
+				ui_->doubleSpinBox_posefilteringAngle->value()*CV_PI/180.0);
+	}
+	if(optimizedPoses.size() > 0)
+	{
+		exportDialog_->setDBDriver(dbDriver_);
+		exportDialog_->forceAssembling(true);
+		exportDialog_->setOkButton();
+
+		std::map<int, pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr> clouds;
+		std::map<int, pcl::PolygonMesh::Ptr> meshes;
+		std::map<int, pcl::TextureMesh::Ptr> textureMeshes;
+		std::vector<std::map<int, pcl::PointXY> > textureVertexToPixels;
+
+		if(exportDialog_->getExportedClouds(
+				optimizedPoses,
+				updateLinksWithModifications(links_),
+				mapIds_,
+				QMap<int, Signature>(),
+				std::map<int, std::pair<pcl::PointCloud<pcl::PointXYZRGB>::Ptr, pcl::IndicesPtr> >(),
+				std::map<int, LaserScan>(),
+				pathDatabase_,
+				ui_->parameters_toolbox->getParameters(),
+				clouds,
+				meshes,
+				textureMeshes,
+				textureVertexToPixels))
+		{
+			if(textureMeshes.size())
+			{
+				dbDriver_->saveOptimizedPoses(optimizedPoses, Transform());
+
+				cv::Mat globalTextures;
+				pcl::TextureMeshPtr textureMesh = textureMeshes.at(0);
+				if(textureMesh->tex_materials.size()>1)
+				{
+					globalTextures = util3d::mergeTextures(
+							*textureMesh,
+							std::map<int, cv::Mat>(),
+							std::map<int, std::vector<CameraModel> >(),
+							0,
+							dbDriver_,
+							exportDialog_->getTextureSize(),
+							exportDialog_->getMaxTextures(),
+							textureVertexToPixels,
+							exportDialog_->isGainCompensation(),
+							exportDialog_->getGainBeta(),
+							exportDialog_->isGainRGB(),
+							exportDialog_->isBlending(),
+							exportDialog_->getBlendingDecimation(),
+							exportDialog_->getTextureBrightnessConstrastRatioLow(),
+							exportDialog_->getTextureBrightnessConstrastRatioHigh(),
+							exportDialog_->isExposeFusion());
+				}
+				dbDriver_->saveOptimizedMesh(
+						util3d::laserScanFromPointCloud(textureMesh->cloud, false).data(),
+						util3d::convertPolygonsFromPCL(textureMesh->tex_polygons),
+						textureMesh->tex_coordinates,
+						globalTextures);
+				QMessageBox::information(this, tr("Update Optimized Textured Mesh"), tr("Updated!"));
+				ui_->actionView_optimized_mesh->setEnabled(true);
+				ui_->actionExport_optimized_mesh->setEnabled(true);
+				this->viewOptimizedMesh();
+			}
+			else if(meshes.size())
+			{
+				dbDriver_->saveOptimizedPoses(optimizedPoses, Transform());
+				std::vector<std::vector<std::vector<unsigned int> > > polygons(1);
+				polygons.at(0) = util3d::convertPolygonsFromPCL(meshes.at(0)->polygons);
+				dbDriver_->saveOptimizedMesh(util3d::laserScanFromPointCloud(meshes.at(0)->cloud, false).data(), polygons);
+				QMessageBox::information(this, tr("Update Optimized Mesh"), tr("Updated!"));
+				ui_->actionView_optimized_mesh->setEnabled(true);
+				ui_->actionExport_optimized_mesh->setEnabled(true);
+				this->viewOptimizedMesh();
+			}
+			else if(clouds.size())
+			{
+				dbDriver_->saveOptimizedPoses(optimizedPoses, Transform());
+				dbDriver_->saveOptimizedMesh(util3d::laserScanFromPointCloud(*clouds.at(0)));
+				QMessageBox::information(this, tr("Update Optimized PointCloud"), tr("Updated!"));
+				ui_->actionView_optimized_mesh->setEnabled(true);
+				ui_->actionExport_optimized_mesh->setEnabled(true);
+				this->viewOptimizedMesh();
+			}
+			else
+			{
+				QMessageBox::critical(this, tr("Update Optimized Mesh"), tr("Nothing to save!"));
+			}
+		}
+		exportDialog_->setProgressDialogToMax();
+	}
+	else
+	{
+		QMessageBox::critical(this, tr("Error"), tr("No neighbors found for node %1.").arg(ui_->spinBox_optimizationsFrom->value()));
+	}
+}
+
 void DatabaseViewer::generateGraph()
 {
 	if(!dbDriver_)

guilib/src/ExportCloudsDialog.cpp

@@ -54,6 +54,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #include <pcl/io/obj_io.h>
 #include <pcl/pcl_config.h>
 #include <pcl/surface/poisson.h>
+#include <pcl/common/common.h>
 
 #include <QPushButton>
 #include <QDir>
@@ -68,6 +69,13 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #include <cpu_tsdf/marching_cubes_tsdf_octree.h>
 #endif
 
+#ifdef RTABMAP_OPENCHISEL
+#include "chisel_conversions.h"
+#include <open_chisel/ProjectionIntegrator.h>
+#include <open_chisel/truncation/QuadraticTruncator.h>
+#include <open_chisel/weighting/ConstantWeighter.h>
+#endif
+
 namespace rtabmap {
 
 ExportCloudsDialog::ExportCloudsDialog(QWidget *parent) :
@@ -80,6 +88,10 @@ ExportCloudsDialog::ExportCloudsDialog(QWidget *parent) :
 	_ui->setupUi(this);
 
 	connect(_ui->buttonBox->button(QDialogButtonBox::RestoreDefaults), SIGNAL(clicked()), this, SLOT(restoreDefaults()));
+	QPushButton * loadSettingsButton = _ui->buttonBox->addButton("Load Settings", QDialogButtonBox::ActionRole);
+	QPushButton * saveSettingsButton = _ui->buttonBox->addButton("Save Settings", QDialogButtonBox::ActionRole);
+	connect(loadSettingsButton, SIGNAL(clicked()), this, SLOT(loadSettings()));
+	connect(saveSettingsButton, SIGNAL(clicked()), this, SLOT(saveSettings()));
 
 	restoreDefaults();
 	_ui->comboBox_upsamplingMethod->setItemData(1, 0, Qt::UserRole - 1); // disable DISTINCT_CLOUD
@@ -201,6 +213,21 @@ ExportCloudsDialog::ExportCloudsDialog(QWidget *parent) :
 	connect(_ui->doubleSpinBox_cputsdf_flattenRadius, SIGNAL(valueChanged(double)), this, SIGNAL(configChanged()));
 	connect(_ui->spinBox_cputsdf_randomSplit, SIGNAL(valueChanged(int)), this, SIGNAL(configChanged()));
 
+	connect(_ui->checkBox_openchisel_mergeVertices, SIGNAL(stateChanged(int)), this, SIGNAL(configChanged()));
+	connect(_ui->spinBox_openchisel_chunk_size_x, SIGNAL(valueChanged(int)), this, SIGNAL(configChanged()));
+	connect(_ui->spinBox_openchisel_chunk_size_y, SIGNAL(valueChanged(int)), this, SIGNAL(configChanged()));
+	connect(_ui->spinBox_openchisel_chunk_size_z, SIGNAL(valueChanged(int)), this, SIGNAL(configChanged()));
+	connect(_ui->doubleSpinBox_openchisel_truncation_constant, SIGNAL(valueChanged(double)), this, SIGNAL(configChanged()));
+	connect(_ui->doubleSpinBox_openchisel_truncation_linear, SIGNAL(valueChanged(double)), this, SIGNAL(configChanged()));
+	connect(_ui->doubleSpinBox_openchisel_truncation_quadratic, SIGNAL(valueChanged(double)), this, SIGNAL(configChanged()));
+	connect(_ui->doubleSpinBox_openchisel_truncation_scale, SIGNAL(valueChanged(double)), this, SIGNAL(configChanged()));
+	connect(_ui->spinBox_openchisel_integration_weight, SIGNAL(valueChanged(int)), this, SIGNAL(configChanged()));
+	connect(_ui->checkBox_openchisel_use_voxel_carving, SIGNAL(stateChanged(int)), this, SIGNAL(configChanged()));
+	connect(_ui->doubleSpinBox_openchisel_carving_dist_m, SIGNAL(valueChanged(double)), this, SIGNAL(configChanged()));
+	connect(_ui->doubleSpinBox_openchisel_near_plane_dist, SIGNAL(valueChanged(double)), this, SIGNAL(configChanged()));
+	connect(_ui->doubleSpinBox_openchisel_far_plane_dist, SIGNAL(valueChanged(double)), this, SIGNAL(configChanged()));
+
+
 	_progressDialog = new ProgressDialog(this);
 	_progressDialog->setVisible(false);
 	_progressDialog->setAutoClose(true, 2);
@@ -246,6 +273,24 @@ void ExportCloudsDialog::cancel()
 	_progressDialog->appendText(tr("Canceled!"));
 }
 
+void ExportCloudsDialog::forceAssembling(bool enabled)
+{
+	if(enabled)
+	{
+		_ui->checkBox_assemble->setChecked(true);
+		_ui->checkBox_assemble->setEnabled(false);
+	}
+	else
+	{
+		_ui->checkBox_assemble->setEnabled(true);
+	}
+}
+
+void ExportCloudsDialog::setProgressDialogToMax()
+{
+	_progressDialog->setValue(_progressDialog->maximumSteps());
+}
+
 void ExportCloudsDialog::saveSettings(QSettings & settings, const QString & group) const
 {
 	if(!group.isEmpty())
@@ -356,6 +401,20 @@ void ExportCloudsDialog::saveSettings(QSettings & settings, const QString & grou
 	settings.setValue("cputsdf_flattenRadius", _ui->doubleSpinBox_cputsdf_flattenRadius->value());
 	settings.setValue("cputsdf_randomSplit", _ui->spinBox_cputsdf_randomSplit->value());
 
+	settings.setValue("openchisel_merge_vertices", _ui->checkBox_openchisel_mergeVertices->isChecked());
+	settings.setValue("openchisel_chunk_size_x", _ui->spinBox_openchisel_chunk_size_x->value());
+	settings.setValue("openchisel_chunk_size_y", _ui->spinBox_openchisel_chunk_size_y->value());
+	settings.setValue("openchisel_chunk_size_z", _ui->spinBox_openchisel_chunk_size_z->value());
+	settings.setValue("openchisel_truncation_constant", _ui->doubleSpinBox_openchisel_truncation_constant->value());
+	settings.setValue("openchisel_truncation_linear", _ui->doubleSpinBox_openchisel_truncation_linear->value());
+	settings.setValue("openchisel_truncation_quadratic", _ui->doubleSpinBox_openchisel_truncation_quadratic->value());
+	settings.setValue("openchisel_truncation_scale", _ui->doubleSpinBox_openchisel_truncation_scale->value());
+	settings.setValue("openchisel_integration_weight", _ui->spinBox_openchisel_integration_weight->value());
+	settings.setValue("openchisel_use_voxel_carving", _ui->checkBox_openchisel_use_voxel_carving->isChecked());
+	settings.setValue("openchisel_carving_dist_m", _ui->doubleSpinBox_openchisel_carving_dist_m->value());
+	settings.setValue("openchisel_near_plane_dist", _ui->doubleSpinBox_openchisel_near_plane_dist->value());
+	settings.setValue("openchisel_far_plane_dist", _ui->doubleSpinBox_openchisel_far_plane_dist->value());
+
 	if(!group.isEmpty())
 	{
 		settings.endGroup();
@@ -392,7 +451,10 @@ void ExportCloudsDialog::loadSettings(QSettings & settings, const QString & grou
 	_ui->doubleSpinBox_filteringRadius->setValue(settings.value("filtering_radius", _ui->doubleSpinBox_filteringRadius->value()).toDouble());
 	_ui->spinBox_filteringMinNeighbors->setValue(settings.value("filtering_min_neighbors", _ui->spinBox_filteringMinNeighbors->value()).toInt());
 
-	_ui->checkBox_assemble->setChecked(settings.value("assemble", _ui->checkBox_assemble->isChecked()).toBool());
+	if(_ui->checkBox_assemble->isEnabled())
+	{
+		_ui->checkBox_assemble->setChecked(settings.value("assemble", _ui->checkBox_assemble->isChecked()).toBool());
+	}
 	_ui->doubleSpinBox_voxelSize_assembled->setValue(settings.value("assemble_voxel", _ui->doubleSpinBox_voxelSize_assembled->value()).toDouble());
 	_ui->comboBox_frame->setCurrentIndex(settings.value("frame", _ui->comboBox_frame->currentIndex()).toInt());
 
@@ -475,6 +537,20 @@ void ExportCloudsDialog::loadSettings(QSettings & settings, const QString & grou
 	_ui->doubleSpinBox_cputsdf_flattenRadius->setValue(settings.value("cputsdf_flattenRadius", _ui->doubleSpinBox_cputsdf_flattenRadius->value()).toDouble());
 	_ui->spinBox_cputsdf_randomSplit->setValue(settings.value("cputsdf_randomSplit", _ui->spinBox_cputsdf_randomSplit->value()).toInt());
 
+	_ui->checkBox_openchisel_mergeVertices->setChecked(settings.value("openchisel_merge_vertices", _ui->checkBox_openchisel_mergeVertices->isChecked()).toBool());
+	_ui->spinBox_openchisel_chunk_size_x->setValue(settings.value("openchisel_chunk_size_x", _ui->spinBox_openchisel_chunk_size_x->value()).toInt());
+	_ui->spinBox_openchisel_chunk_size_y->setValue(settings.value("openchisel_chunk_size_y", _ui->spinBox_openchisel_chunk_size_y->value()).toInt());
+	_ui->spinBox_openchisel_chunk_size_z->setValue(settings.value("openchisel_chunk_size_z", _ui->spinBox_openchisel_chunk_size_z->value()).toInt());
+	_ui->doubleSpinBox_openchisel_truncation_constant->setValue(settings.value("openchisel_truncation_constant", _ui->doubleSpinBox_openchisel_truncation_constant->value()).toDouble());
+	_ui->doubleSpinBox_openchisel_truncation_linear->setValue(settings.value("openchisel_truncation_linear", _ui->doubleSpinBox_openchisel_truncation_linear->value()).toDouble());
+	_ui->doubleSpinBox_openchisel_truncation_quadratic->setValue(settings.value("openchisel_truncation_quadratic", _ui->doubleSpinBox_openchisel_truncation_quadratic->value()).toDouble());
+	_ui->doubleSpinBox_openchisel_truncation_scale->setValue(settings.value("openchisel_truncation_scale", _ui->doubleSpinBox_openchisel_truncation_scale->value()).toDouble());
+	_ui->spinBox_openchisel_integration_weight->setValue(settings.value("openchisel_integration_weight", _ui->spinBox_openchisel_integration_weight->value()).toInt());
+	_ui->checkBox_openchisel_use_voxel_carving->setChecked(settings.value("openchisel_use_voxel_carving", _ui->checkBox_openchisel_use_voxel_carving->isChecked()).toBool());
+	_ui->doubleSpinBox_openchisel_carving_dist_m->setValue(settings.value("openchisel_carving_dist_m", _ui->doubleSpinBox_openchisel_carving_dist_m->value()).toDouble());
+	_ui->doubleSpinBox_openchisel_near_plane_dist->setValue(settings.value("openchisel_near_plane_dist", _ui->doubleSpinBox_openchisel_near_plane_dist->value()).toDouble());
+	_ui->doubleSpinBox_openchisel_far_plane_dist->setValue(settings.value("openchisel_far_plane_dist", _ui->doubleSpinBox_openchisel_far_plane_dist->value()).toDouble());
+
 	updateReconstructionFlavor();
 	updateMLSGrpVisibility();
 
@@ -573,7 +649,7 @@ void ExportCloudsDialog::restoreDefaults()
 
 	_ui->checkBox_poisson_outputPolygons->setChecked(false);
 	_ui->checkBox_poisson_manifold->setChecked(true);
-	_ui->spinBox_poisson_depth->setValue(9);
+	_ui->spinBox_poisson_depth->setValue(0);
 	_ui->spinBox_poisson_iso->setValue(8);
 	_ui->spinBox_poisson_solver->setValue(8);
 	_ui->spinBox_poisson_minDepth->setValue(5);
@@ -589,12 +665,55 @@ void ExportCloudsDialog::restoreDefaults()
 	_ui->doubleSpinBox_cputsdf_flattenRadius->setValue(0.005);
 	_ui->spinBox_cputsdf_randomSplit->setValue(1);
 
+	_ui->checkBox_openchisel_mergeVertices->setChecked(true);
+	_ui->spinBox_openchisel_chunk_size_x->setValue(16);
+	_ui->spinBox_openchisel_chunk_size_y->setValue(16);
+	_ui->spinBox_openchisel_chunk_size_z->setValue(16);
+	_ui->doubleSpinBox_openchisel_truncation_constant->setValue(0.001504);
+	_ui->doubleSpinBox_openchisel_truncation_linear->setValue(0.00152);
+	_ui->doubleSpinBox_openchisel_truncation_quadratic->setValue(0.0019);
+	_ui->doubleSpinBox_openchisel_truncation_scale->setValue(10.0);
+	_ui->spinBox_openchisel_integration_weight->setValue(1);
+	_ui->checkBox_openchisel_use_voxel_carving->setChecked(false);
+	_ui->doubleSpinBox_openchisel_carving_dist_m->setValue(0.05);
+	_ui->doubleSpinBox_openchisel_near_plane_dist->setValue(0.05);
+	_ui->doubleSpinBox_openchisel_far_plane_dist->setValue(1.1);
+
+
 	updateReconstructionFlavor();
 	updateMLSGrpVisibility();
 
 	this->update();
 }
 
+void ExportCloudsDialog::loadSettings()
+{
+	QString path = QFileDialog::getOpenFileName(this, tr("Load Settings"), _workingDirectory, tr("Config (*.ini)"));
+	if(path.size())
+	{
+		QSettings settings(path, QSettings::IniFormat);
+		settings.beginGroup("Gui");
+		settings.beginGroup(this->objectName());
+		this->loadSettings(settings);
+		settings.endGroup(); // "name"
+		settings.endGroup(); // Gui
+	}
+}
+
+void ExportCloudsDialog::saveSettings()
+{
+	QString path = QFileDialog::getSaveFileName(this, tr("Save Settings"), _workingDirectory, tr("Config (*.ini)"));
+	if(path.size())
+	{
+		QSettings settings(path, QSettings::IniFormat);
+		settings.beginGroup("Gui");
+		settings.beginGroup(this->objectName());
+		this->saveSettings(settings);
+		settings.endGroup(); // "name"
+		settings.endGroup(); // Gui
+	}
+}
+
 void ExportCloudsDialog::updateReconstructionFlavor()
 {
 	if(!_ui->checkBox_fromDepth->isChecked())
@@ -621,6 +740,7 @@ void ExportCloudsDialog::updateReconstructionFlavor()
 
 	_ui->checkBox_smoothing->setVisible(_ui->comboBox_pipeline->currentIndex() == 1);
 	_ui->checkBox_smoothing->setEnabled(_ui->comboBox_pipeline->currentIndex() == 1);
+	_ui->label_smoothing->setVisible(_ui->comboBox_pipeline->currentIndex() == 1);
 
 	_ui->comboBox_frame->setEnabled(!_ui->checkBox_assemble->isChecked() && _ui->checkBox_binary->isEnabled());
 	_ui->comboBox_frame->setVisible(_ui->comboBox_frame->isEnabled());
@@ -643,20 +763,34 @@ void ExportCloudsDialog::updateReconstructionFlavor()
 	// dense texturing options
 	if(_ui->checkBox_meshing->isChecked())
 	{
+		//GP3
 		_ui->comboBox_meshingApproach->setItemData(0, _ui->comboBox_pipeline->currentIndex() == 1?1 | 32:0,Qt::UserRole - 1);
+
+		//Poisson
 		_ui->comboBox_meshingApproach->setItemData(1, _ui->comboBox_pipeline->currentIndex() == 1 && _ui->checkBox_assemble->isChecked()?1 | 32:0,Qt::UserRole - 1);
+
+		//CPU-TSDF
 #ifdef RTABMAP_CPUTSDF
 		_ui->comboBox_meshingApproach->setItemData(2, _ui->comboBox_pipeline->currentIndex() == 0 && _ui->checkBox_assemble->isChecked()?1 | 32:0,Qt::UserRole - 1);
 #else
-		_ui->comboBox_meshingApproach->setItemData(2, Qt::UserRole - 1);
+		_ui->comboBox_meshingApproach->setItemData(2, 0, Qt::UserRole - 1);
 #endif
+
+		// Organized
 		_ui->comboBox_meshingApproach->setItemData(3, _ui->comboBox_pipeline->currentIndex() == 0?1 | 32:0,Qt::UserRole - 1);
 
+		//Open Chisel
+#ifdef RTABMAP_OPENCHISEL
+		_ui->comboBox_meshingApproach->setItemData(4, _ui->checkBox_assemble->isChecked()?1 | 32:0,Qt::UserRole - 1);
+#else
+		_ui->comboBox_meshingApproach->setItemData(4, 0, Qt::UserRole - 1);
+#endif
+
 		if(_ui->comboBox_pipeline->currentIndex() == 0 && _ui->comboBox_meshingApproach->currentIndex()<2)
 		{
 			_ui->comboBox_meshingApproach->setCurrentIndex(3);
 		}
-		if(_ui->comboBox_pipeline->currentIndex() == 1 && _ui->comboBox_meshingApproach->currentIndex()>1)
+		if(_ui->comboBox_pipeline->currentIndex() == 1 && (_ui->comboBox_meshingApproach->currentIndex()==2 || _ui->comboBox_meshingApproach->currentIndex()==3))
 		{
 			_ui->comboBox_meshingApproach->setCurrentIndex(1);
 		}
@@ -678,6 +812,7 @@ void ExportCloudsDialog::updateReconstructionFlavor()
 		_ui->groupBox_poisson->setVisible(_ui->comboBox_pipeline->currentIndex() == 1 && _ui->comboBox_meshingApproach->currentIndex()==1);
 		_ui->groupBox_cputsdf->setVisible(_ui->comboBox_pipeline->currentIndex() == 0 && _ui->comboBox_meshingApproach->currentIndex()==2);
 		_ui->groupBox_organized->setVisible(_ui->comboBox_pipeline->currentIndex() == 0 && _ui->comboBox_meshingApproach->currentIndex()==3);
+		_ui->groupBox_openchisel->setVisible(_ui->comboBox_meshingApproach->currentIndex()==4);
 
 #ifndef DISABLE_VTK
 		_ui->doubleSpinBox_meshDecimationFactor->setEnabled(_ui->comboBox_meshingApproach->currentIndex()!=3);
@@ -842,6 +977,7 @@ void ExportCloudsDialog::viewClouds(
 		}
 		viewer->setLighting(true);
 		viewer->setDefaultBackgroundColor(QColor(40, 40, 40, 255));
+		viewer->buildPickingLocator(true);
 
 		QVBoxLayout *layout = new QVBoxLayout();
 		layout->addWidget(viewer);
@@ -858,6 +994,7 @@ void ExportCloudsDialog::viewClouds(
 
 		if(textureMeshes.size())
 		{
+			viewer->setPolygonPicking(true);
 			std::map<int, cv::Mat> images;
 			std::map<int, std::vector<CameraModel> > calibrations;
 			for(QMap<int, Signature>::const_iterator iter=cachedSignatures.constBegin(); iter!=cachedSignatures.constEnd(); ++iter)
@@ -1014,6 +1151,7 @@ void ExportCloudsDialog::viewClouds(
 		}
 		else if(meshes.size())
 		{
+			viewer->setPolygonPicking(true);
 			for(std::map<int, pcl::PolygonMesh::Ptr>::iterator iter = meshes.begin(); iter!=meshes.end(); ++iter)
 			{
 				_progressDialog->appendText(tr("Viewing the mesh %1 (%2 polygons)...").arg(iter->first).arg(iter->second->polygons.size()));
@@ -1070,6 +1208,60 @@ void ExportCloudsDialog::viewClouds(
 	_progressDialog->setValue(_progressDialog->maximumSteps());
 }
 
+int ExportCloudsDialog::getTextureSize() const
+{
+	int textureSize = 1024;
+	if(_ui->comboBox_meshingTextureSize->currentIndex() > 0)
+	{
+		textureSize = 128 << _ui->comboBox_meshingTextureSize->currentIndex(); // start at 256
+	}
+	return textureSize;
+}
+int ExportCloudsDialog::getMaxTextures() const
+{
+	return _ui->spinBox_mesh_maxTextures->value();
+}
+bool ExportCloudsDialog::isGainCompensation() const
+{
+	return _ui->checkBox_gainCompensation->isChecked();
+}
+double ExportCloudsDialog::getGainBeta() const
+{
+	return _ui->doubleSpinBox_gainBeta->value();
+}
+bool ExportCloudsDialog::isGainRGB() const
+{
+	return _ui->checkBox_gainRGB->isChecked();
+}
+bool ExportCloudsDialog::isBlending() const
+{
+	return _ui->checkBox_blending->isChecked();
+}
+int ExportCloudsDialog::getBlendingDecimation() const
+{
+	int blendingDecimation = 0;
+	if(_ui->checkBox_blending->isChecked())
+	{
+		if(_ui->comboBox_blendingDecimation->currentIndex() > 0)
+		{
+			blendingDecimation = 1 << (_ui->comboBox_blendingDecimation->currentIndex()-1);
+		}
+	}
+	return blendingDecimation;
+}
+int ExportCloudsDialog::getTextureBrightnessConstrastRatioLow() const
+{
+	return _ui->spinBox_textureBrightnessContrastRatioLow->value();
+}
+int ExportCloudsDialog::getTextureBrightnessConstrastRatioHigh() const
+{
+	return _ui->spinBox_textureBrightnessContrastRatioHigh->value();
+}
+bool ExportCloudsDialog::isExposeFusion() const
+{
+	return _ui->checkBox_exposureFusion->isEnabled() && _ui->checkBox_exposureFusion->isChecked();
+}
+
 bool ExportCloudsDialog::removeDirRecursively(const QString & dirName)
 {
     bool result = true;
@@ -1157,14 +1349,38 @@ bool ExportCloudsDialog::getExportedClouds(
 		}
 		_progressDialog->setMaximumSteps(int(poses.size())*mul+1);
 
+		bool loadClouds = true;
+#ifdef RTABMAP_OPENCHISEL
+		if(_ui->comboBox_meshingApproach->currentIndex()==4 && _ui->checkBox_assemble->isChecked())
+		{
+			loadClouds = !_ui->checkBox_fromDepth->isChecked();
+		}
+#endif
+
 		bool has2dScans = false;
-		std::map<int, std::pair<pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr, pcl::IndicesPtr> > clouds = this->getClouds(
+		std::map<int, std::pair<pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr, pcl::IndicesPtr> > clouds;
-				poses,
+		if(loadClouds)
-				cachedSignatures,
+		{
-				cachedClouds,
+			clouds = this->getClouds(
-				cachedScans,
+					poses,
-				parameters,
+					cachedSignatures,
-				has2dScans);
+					cachedClouds,
+					cachedScans,
+					parameters,
+					has2dScans);
+		}
+		else
+		{
+			// just create empty clouds
+			for(std::map<int, Transform>::const_iterator iter=poses.begin(); iter!=poses.end(); ++iter)
+			{
+				clouds.insert(std::make_pair(iter->first,
+						std::make_pair(
+								pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr(new pcl::PointCloud<pcl::PointXYZRGBNormal>),
+								pcl::IndicesPtr(new std::vector<int>))));
+
+			}
+		}
 
 		std::set<int> validCameras = uKeysSet(clouds);
 
@@ -1327,7 +1543,7 @@ bool ExportCloudsDialog::getExportedClouds(
 		pcl::PointCloud<pcl::PointXYZ>::Ptr rawAssembledCloud(new pcl::PointCloud<pcl::PointXYZ>);
 		std::vector<int> rawCameraIndices;
 		if(_ui->checkBox_assemble->isChecked() &&
-		   !(_ui->comboBox_pipeline->currentIndex()==0 && _ui->checkBox_meshing->isChecked()))
+		   !((_ui->comboBox_pipeline->currentIndex()==0 || _ui->comboBox_meshingApproach->currentIndex()==4) && _ui->checkBox_meshing->isChecked()))
 		{
 			_progressDialog->appendText(tr("Assembling %1 clouds...").arg(clouds.size()));
 			QApplication::processEvents();
@@ -1470,6 +1686,11 @@ bool ExportCloudsDialog::getExportedClouds(
 						(float)_ui->doubleSpinBox_dilationVoxelSize->value(),
 						_ui->spinBox_dilationSteps->value());
 
+				// make sure there are no nans
+				UDEBUG("NaNs filtering... size before = %d", cloudWithNormals->size());
+				cloudWithNormals = util3d::removeNaNNormalsFromPointCloud(cloudWithNormals);
+				UDEBUG("NaNs filtering... size after = %d", cloudWithNormals->size());
+
 				if(_ui->checkBox_assemble->isChecked())
 				{
 					// Re-voxelize to make sure to have uniform density
@@ -1516,6 +1737,10 @@ bool ExportCloudsDialog::getExportedClouds(
 #ifdef RTABMAP_CPUTSDF
 		cpu_tsdf::TSDFVolumeOctree::Ptr tsdf;
 #endif
+#ifdef RTABMAP_OPENCHISEL
+		chisel::ChiselPtr chiselMap;
+		chisel::ProjectionIntegrator projectionIntegrator;
+#endif
 
 		//used for organized texturing below
 		std::map<int, std::vector<int> > organizedIndices;
@@ -1525,11 +1750,166 @@ bool ExportCloudsDialog::getExportedClouds(
 		UDEBUG("Meshing=%d", _ui->checkBox_meshing->isChecked()?1:0);
 		if(_ui->checkBox_meshing->isChecked() && !has2dScans)
 		{
+
+#ifdef RTABMAP_OPENCHISEL
+			if(_ui->comboBox_meshingApproach->currentIndex()==4 && _ui->checkBox_assemble->isChecked())
+			{
+				_progressDialog->appendText(tr("Creating TSDF volume with OpenChisel... "));
+
+				QApplication::processEvents();
+				uSleep(100);
+				QApplication::processEvents();
+
+				pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr mergedClouds(new pcl::PointCloud<pcl::PointXYZRGBNormal>);
+				std::vector<pcl::Vertices> mergedPolygons;
+
+				int cloudsAdded = 1;
+				for(std::map<int, pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr >::iterator iter=cloudsWithNormals.begin();
+					iter!= cloudsWithNormals.end();
+					++iter,++cloudsAdded)
+				{
+					std::vector<CameraModel> models;
+					StereoCameraModel stereoModel;
+					bool cacheHasCompressedImage = false;
+					LaserScan scanInfo;
+					if(cachedSignatures.contains(iter->first))
+					{
+						const SensorData & data = cachedSignatures.find(iter->first)->sensorData();
+						models = data.cameraModels();
+						cacheHasCompressedImage = !data.imageCompressed().empty();
+						scanInfo = !data.laserScanRaw().isEmpty()?data.laserScanRaw():data.laserScanCompressed();
+					}
+					else if(_dbDriver)
+					{
+						_dbDriver->getCalibration(iter->first, models, stereoModel);
+						_dbDriver->getLaserScanInfo(iter->first, scanInfo);
+					}
+
+					if(chiselMap.get() == 0)
+					{
+						UDEBUG("");
+						int chunkSizeX = _ui->spinBox_openchisel_chunk_size_x->value();
+						int chunkSizeY = _ui->spinBox_openchisel_chunk_size_y->value();
+						int chunkSizeZ = _ui->spinBox_openchisel_chunk_size_z->value();
+						float voxelResolution = _ui->doubleSpinBox_voxelSize_assembled->value();
+						if(voxelResolution <=0.0f)
+						{
+							_progressDialog->appendText(tr("OpenChisel: Voxel size should not be null!"), Qt::darkYellow);
+							_progressDialog->setAutoClose(false);
+							break;
+						}
+						bool useColor = _ui->checkBox_fromDepth->isChecked();
+						chiselMap.reset(new chisel::Chisel(Eigen::Vector3i(chunkSizeX, chunkSizeY, chunkSizeZ), voxelResolution, useColor));
+						double truncationDistConst = _ui->doubleSpinBox_openchisel_truncation_constant->value();
+						double truncationDistLinear = _ui->doubleSpinBox_openchisel_truncation_linear->value();
+						double truncationDistQuad = _ui->doubleSpinBox_openchisel_truncation_quadratic->value();
+						double truncationDistScale = _ui->doubleSpinBox_openchisel_truncation_scale->value();
+						int weight = _ui->spinBox_openchisel_integration_weight->value();
+						bool useCarving = _ui->checkBox_openchisel_use_voxel_carving->isChecked();
+						double carvingDist = _ui->doubleSpinBox_openchisel_carving_dist_m->value();
+						chisel::Vec4 truncation(truncationDistQuad, truncationDistLinear, truncationDistConst, truncationDistScale);
+						UDEBUG("If crashing just after this message, make sure PCL and OpenChisel are built both with -march=native or both without -march=native");
+						projectionIntegrator.SetCentroids(chiselMap->GetChunkManager().GetCentroids());
+						projectionIntegrator.SetTruncator(chisel::TruncatorPtr(new chisel::QuadraticTruncator(truncation(0), truncation(1), truncation(2), truncation(3))));
+						projectionIntegrator.SetWeighter(chisel::WeighterPtr(new chisel::ConstantWeighter(weight)));
+						projectionIntegrator.SetCarvingDist(carvingDist);
+						projectionIntegrator.SetCarvingEnabled(useCarving);
+					}
+
+					UDEBUG("");
+					double nearPlaneDist = _ui->doubleSpinBox_openchisel_near_plane_dist->value();
+					double farPlaneDist = _ui->doubleSpinBox_openchisel_far_plane_dist->value();
+					if(_ui->checkBox_fromDepth->isChecked())
+					{
+						if(models.size() == 1 && !models[0].localTransform().isNull())
+						{
+							// get just the depth
+							cv::Mat rgb;
+							cv::Mat depth;
+							if(cacheHasCompressedImage)
+							{
+								cachedSignatures.find(iter->first)->sensorData().uncompressDataConst(&rgb, &depth);
+							}
+							else if(_dbDriver)
+							{
+								SensorData data;
+								_dbDriver->getNodeData(iter->first, data, true, false, false, false);
+								data.uncompressDataConst(&rgb, &depth);
+							}
+							if(!rgb.empty() && !depth.empty())
+							{
+								CameraModel rgbModel = models[0];
+								CameraModel depthModel = rgbModel;
+								if(rgb.cols > depth.cols)
+								{
+									UASSERT(rgb.cols % depth.cols == 0);
+									depthModel = depthModel.scaled(double(depth.cols)/double(rgb.cols));
+								}
+
+								if(depth.type() == CV_16UC1)
+								{
+									depth = util2d::cvtDepthToFloat(depth);
+								}
+
+								std::shared_ptr<chisel::ColorImage<unsigned char> > colorChisel = colorImageToChisel(rgb);
+								std::shared_ptr<chisel::DepthImage<float> > depthChisel = depthImageToChisel(depth);
+
+								chisel::PinholeCamera cameraColor = cameraModelToChiselCamera(rgbModel);
+								chisel::PinholeCamera cameraDepth = cameraModelToChiselCamera(depthModel);
+								cameraColor.SetNearPlane(nearPlaneDist);
+								cameraColor.SetFarPlane(farPlaneDist);
+								cameraDepth.SetNearPlane(nearPlaneDist);
+								cameraDepth.SetFarPlane(farPlaneDist);
+
+								chisel::Transform  pose_rel_to_first_frame = (poses.at(iter->first)*models[0].localTransform()).toEigen3f();
+								chiselMap->IntegrateDepthScanColor<float, unsigned char>(projectionIntegrator, depthChisel, pose_rel_to_first_frame, cameraDepth, colorChisel, pose_rel_to_first_frame, cameraColor);
+								UDEBUG("");
+							}
+							else
+							{
+								_progressDialog->appendText(tr("OpenChisel: Depth and RGB images not found for %1!").arg(iter->first), Qt::darkYellow);
+							}
+						}
+						else
+						{
+							_progressDialog->appendText(tr("OpenChisel: Invalid camera model for cloud %1! Only single RGB-D camera supported.").arg(iter->first), Qt::darkYellow);
+							_progressDialog->setAutoClose(false);
+							break;
+						}
+					}
+					else if(!scanInfo.localTransform().isNull())
+					{
+						chisel::PointCloudPtr chiselCloud = pointCloudRGBToChisel(*iter->second, scanInfo.localTransform().inverse());
+						chisel::Transform  pose_rel_to_first_frame = (poses.at(iter->first)*scanInfo.localTransform()).toEigen3f();
+						chiselMap->IntegratePointCloud(projectionIntegrator, *chiselCloud, pose_rel_to_first_frame, farPlaneDist);
+						UDEBUG("");
+					}
+					else
+					{
+						_progressDialog->appendText(tr("OpenChisel: not valid scan info for cloud %1!").arg(iter->first), Qt::darkYellow);
+						_progressDialog->setAutoClose(false);
+						break;
+					}
+					chiselMap->UpdateMeshes();
+					UDEBUG("");
+					_progressDialog->appendText(tr("OpenChisel: Integrated cloud %1 (%2/%3) to TSDF volume").arg(iter->first).arg(cloudsAdded).arg(cloudsWithNormals.size()));
+
+					_progressDialog->incrementStep();
+					QApplication::processEvents();
+					if(_canceled)
+					{
+						return false;
+					}
+				}
+			}
+			else
+#endif
+
 			if(_ui->comboBox_pipeline->currentIndex() == 0)
 			{
 				if(_ui->comboBox_meshingApproach->currentIndex()==2)
 				{
-					_progressDialog->appendText(tr("Creating TSDF volume... "));
+					_progressDialog->appendText(tr("Creating TSDF volume with CPUTSDF... "));
 				}
 				else
 				{
@@ -1542,10 +1922,10 @@ bool ExportCloudsDialog::getExportedClouds(
 				pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr mergedClouds(new pcl::PointCloud<pcl::PointXYZRGBNormal>);
 				std::vector<pcl::Vertices> mergedPolygons;
 
-				int i=0;
+				int cloudsAdded = 1;
 				for(std::map<int, pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr >::iterator iter=cloudsWithNormals.begin();
 					iter!= cloudsWithNormals.end();
-					++iter)
+					++iter,++cloudsAdded)
 				{
 					if(iter->second->isOrganized())
 					{
@@ -1613,11 +1993,11 @@ bool ExportCloudsDialog::getExportedClouds(
 										Eigen::Affine3d  pose_rel_to_first_frame = ((poses.begin()->second.inverse() * poses.at(iter->first))*models[0].localTransform()).toEigen3d();
 										if(!tsdf->integrateCloud(*util3d::transformPointCloud(iter->second, models[0].localTransform().inverse()), pcl::PointCloud<pcl::Normal>(), pose_rel_to_first_frame))
 										{
-											_progressDialog->appendText(tr("CPU-TSDF: Failed integrating cloud %1 to TSDF volume").arg(iter->first));
+											_progressDialog->appendText(tr("CPU-TSDF: Failed integrating cloud %1 (%2/%3) to TSDF volume").arg(iter->first).arg(cloudsAdded).arg(cloudsWithNormals.size()));
 										}
 										else
 										{
-											_progressDialog->appendText(tr("CPU-TSDF: Integrated cloud %1 to TSDF volume").arg(iter->first));
+											_progressDialog->appendText(tr("CPU-TSDF: Integrated cloud %1 (%2/%3) to TSDF volume").arg(iter->first).arg(cloudsAdded).arg(cloudsWithNormals.size()));
 										}
 									}
 								}
@@ -1715,7 +2095,7 @@ bool ExportCloudsDialog::getExportedClouds(
 									QApplication::processEvents();
 								}
 
-								_progressDialog->appendText(tr("Mesh %1 created with %2 polygons (%3/%4).").arg(iter->first).arg(polygons.size()).arg(++i).arg(cloudsWithNormals.size()));
+								_progressDialog->appendText(tr("Mesh %1 created with %2 polygons (%3/%4).").arg(iter->first).arg(polygons.size()).arg(cloudsAdded).arg(cloudsWithNormals.size()));
 
 								pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr denseCloud(new pcl::PointCloud<pcl::PointXYZRGBNormal>);
 								std::vector<pcl::Vertices> densePolygons;
@@ -1757,7 +2137,7 @@ bool ExportCloudsDialog::getExportedClouds(
 						}
 						else
 						{
-							_progressDialog->appendText(tr("Mesh %1 not created (no valid points) (%2/%3).").arg(iter->first).arg(++i).arg(cloudsWithNormals.size()));
+							_progressDialog->appendText(tr("Mesh %1 not created (no valid points) (%2/%3).").arg(iter->first).arg(cloudsAdded).arg(cloudsWithNormals.size()));
 						}
 					}
 					else
@@ -1774,7 +2154,7 @@ bool ExportCloudsDialog::getExportedClouds(
 						}
 						if(weight>=0) // don't show error for intermediate nodes
 						{
-							_progressDialog->appendText(tr("Mesh %1 not created (cloud is not organized). You may want to check cloud regeneration option (%2/%3).").arg(iter->first).arg(++i).arg(cloudsWithNormals.size()));
+							_progressDialog->appendText(tr("Mesh %1 not created (cloud is not organized). You may want to check cloud regeneration option (%2/%3).").arg(iter->first).arg(cloudsAdded).arg(cloudsWithNormals.size()));
 						}
 					}
 
@@ -1838,10 +2218,10 @@ bool ExportCloudsDialog::getExportedClouds(
 				uSleep(100);
 				QApplication::processEvents();
 
-				int i=0;
+				int cloudsAdded=1;
 				for(std::map<int, pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr>::iterator iter=cloudsWithNormals.begin();
 					iter!= cloudsWithNormals.end();
-					++iter)
+					++iter,++cloudsAdded)
 				{
 					pcl::PolygonMesh::Ptr mesh(new pcl::PolygonMesh);
 					if(_ui->comboBox_meshingApproach->currentIndex() == 0)
@@ -1857,7 +2237,31 @@ bool ExportCloudsDialog::getExportedClouds(
 						poisson.setOutputPolygons(_ui->checkBox_poisson_outputPolygons->isEnabled()?_ui->checkBox_poisson_outputPolygons->isChecked():false);
 						poisson.setManifold(_ui->checkBox_poisson_manifold->isChecked());
 						poisson.setSamplesPerNode(_ui->doubleSpinBox_poisson_samples->value());
-						poisson.setDepth(_ui->spinBox_poisson_depth->value());
+						int depth = _ui->spinBox_poisson_depth->value();
+						if(depth == 0)
+						{
+							Eigen::Vector4f min,max;
+							pcl::getMinMax3D(*iter->second, min, max);
+							float mapLength = uMax3(max[0]-min[0], max[1]-min[1], max[2]-min[2]);
+							depth = 12;
+							for(int i=6; i<12; ++i)
+							{
+								if(mapLength/float(1<<i) < 0.03f)
+								{
+									depth = i;
+									break;
+								}
+							}
+							_progressDialog->appendText(tr("Poisson depth resolution chosen is %1, map size (m) = %2x%3x%4")
+									.arg(depth)
+									.arg(int(max[0]-min[0]))
+									.arg(int(max[1]-min[1]))
+									.arg(int(max[2]-min[2])));
+							QApplication::processEvents();
+							uSleep(100);
+							QApplication::processEvents();
+						}
+						poisson.setDepth(depth);
 						poisson.setIsoDivide(_ui->spinBox_poisson_iso->value());
 						poisson.setSolverDivide(_ui->spinBox_poisson_solver->value());
 						poisson.setMinDepth(_ui->spinBox_poisson_minDepth->value());
@@ -1867,7 +2271,7 @@ bool ExportCloudsDialog::getExportedClouds(
 						poisson.reconstruct(*mesh);
 					}
 
-					_progressDialog->appendText(tr("Mesh %1 created with %2 polygons (%3/%4).").arg(iter->first).arg(mesh->polygons.size()).arg(++i).arg(clouds.size()));
+					_progressDialog->appendText(tr("Mesh %1 created with %2 polygons (%3/%4).").arg(iter->first).arg(mesh->polygons.size()).arg(cloudsAdded).arg(cloudsWithNormals.size()));
 					QApplication::processEvents();
 
 					if(mesh->polygons.size()>0)
@@ -1922,7 +2326,7 @@ bool ExportCloudsDialog::getExportedClouds(
 			mc.setColorByRGB (true);
 			pcl::PolygonMesh::Ptr mesh (new pcl::PolygonMesh);
 			mc.reconstruct (*mesh);
-			_progressDialog->appendText(tr("CPU-TSDF: Creating mesh from TSDF volume...done!"));
+			_progressDialog->appendText(tr("CPU-TSDF: Creating mesh from TSDF volume...done! %1 polygons").arg(mesh->polygons.size()));
 			meshes.clear();
 
 			if(mesh->polygons.size()>0)
@@ -2002,6 +2406,78 @@ bool ExportCloudsDialog::getExportedClouds(
 			}
 		}
 #endif
+#ifdef RTABMAP_OPENCHISEL
+		if(chiselMap.get())
+		{
+			_progressDialog->appendText(tr("OpenChisel: Creating mesh from TSDF volume..."));
+			QApplication::processEvents();
+			uSleep(100);
+			QApplication::processEvents();
+
+			const chisel::MeshMap& meshMap = chiselMap->GetChunkManager().GetAllMeshes();
+			pcl::PolygonMesh::Ptr mesh = chiselToPolygonMesh(meshMap);
+
+			// To debug...
+			//std::string filePly = _workingDirectory.toStdString()+"/"+"chisel.ply";
+			//chiselMap->SaveAllMeshesToPLY(filePly);
+			//UWARN("Saved %s", filePly.c_str());
+
+			_progressDialog->appendText(tr("OpenChisel: Creating mesh from TSDF volume...done! %1 polygons").arg(mesh->polygons.size()));
+
+			meshes.clear();
+			if(mesh->polygons.size()>0)
+			{
+				pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr mergedClouds(new pcl::PointCloud<pcl::PointXYZRGBNormal>);
+				pcl::fromPCLPointCloud2(mesh->cloud, *mergedClouds);
+				if(_ui->checkBox_openchisel_mergeVertices->isChecked())
+				{
+					_progressDialog->appendText(tr("Filtering assembled mesh for close vertices (points=%1, polygons=%2)...").arg(mergedClouds->size()).arg(mesh->polygons.size()));
+					QApplication::processEvents();
+
+					mesh->polygons = util3d::filterCloseVerticesFromMesh(
+							mergedClouds,
+							mesh->polygons,
+							_ui->doubleSpinBox_voxelSize_assembled->value()/2.0,
+							M_PI/4,
+							true);
+
+					// filter invalid polygons
+					unsigned int count = mesh->polygons.size();
+					mesh->polygons = util3d::filterInvalidPolygons(mesh->polygons);
+					_progressDialog->appendText(tr("Filtered %1 invalid polygons.").arg(count-mesh->polygons.size()));
+					QApplication::processEvents();
+
+					// filter not used vertices
+					pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr filteredCloud(new pcl::PointCloud<pcl::PointXYZRGBNormal>);
+					std::vector<pcl::Vertices> filteredPolygons;
+					count = mergedClouds->size();
+					util3d::filterNotUsedVerticesFromMesh(*mergedClouds, mesh->polygons, *filteredCloud, filteredPolygons);
+					mergedClouds = filteredCloud;
+					pcl::toPCLPointCloud2(*mergedClouds, mesh->cloud);
+					mesh->polygons = filteredPolygons;
+					_progressDialog->appendText(tr("Filtered %1 duplicate vertices.").arg(count-mergedClouds->size()));
+					QApplication::processEvents();
+				}
+				TexturingState texturingState(_progressDialog, false);
+				util3d::denseMeshPostProcessing<pcl::PointXYZRGBNormal>(
+						mesh,
+						_ui->doubleSpinBox_meshDecimationFactor->isEnabled()?(float)_ui->doubleSpinBox_meshDecimationFactor->value():0.0f,
+						_ui->spinBox_meshMaxPolygons->isEnabled()?_ui->spinBox_meshMaxPolygons->value():0,
+						mergedClouds,
+						(float)_ui->doubleSpinBox_transferColorRadius->value(),
+						!(_ui->checkBox_textureMapping->isEnabled() && _ui->checkBox_textureMapping->isChecked()),
+						_ui->checkBox_cleanMesh->isChecked(),
+						_ui->spinBox_mesh_minClusterSize->value(),
+						&texturingState);
+				meshes.insert(std::make_pair(0, mesh));
+			}
+			else
+			{
+				_progressDialog->appendText(tr("No polygons created TSDF volume!"), Qt::darkYellow);
+				_progressDialog->setAutoClose(false);
+			}
+		}
+#endif
 
 		UDEBUG("");
 		if(_canceled)

guilib/src/MainWindow.cpp

@@ -44,6 +44,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #include "rtabmap/core/OccupancyGrid.h"
 #include "rtabmap/core/GainCompensator.h"
 #include "rtabmap/core/Recovery.h"
+#include "rtabmap/core/util2d.h"
 
 #include "rtabmap/gui/ImageView.h"
 #include "rtabmap/gui/KeypointItem.h"
@@ -1931,8 +1932,6 @@ void MainWindow::processStats(const rtabmap::Statistics & stat)
 
 			_odometryReceived = false;
 
-			_odometryCorrection = stat.mapCorrection();
-
 			UDEBUG("time= %d ms", time.restart());
 
 			for(std::map<std::string, float>::iterator iter=updateCloudSats.begin(); iter!=updateCloudSats.end(); ++iter)
@@ -1940,6 +1939,7 @@ void MainWindow::processStats(const rtabmap::Statistics & stat)
 				_ui->statsToolBox->updateStat(iter->first.c_str(), _preferencesDialog->isTimeUsedInFigures()?stat.stamp()-_firstStamp:stat.refImageId(), int(iter->second), _preferencesDialog->isCacheSavedInFigures());
 			}
 		}
+		_odometryCorrection = stat.mapCorrection();
 
 		if( _ui->graphicsView_graphView->isVisible())
 		{
@@ -6832,7 +6832,7 @@ void MainWindow::exportImages()
 			else if(!data.imageRaw().empty() && !data.depthRaw().empty())
 			{
 				cv::imwrite(QString("%1/rgb/%2.%3").arg(path).arg(id).arg(ext).toStdString(), data.imageRaw());
-				cv::imwrite(QString("%1/depth/%2.png").arg(path).arg(id).toStdString(), data.depthRaw());
+				cv::imwrite(QString("%1/depth/%2.png").arg(path).arg(id).toStdString(), data.depthRaw().type()==CV_32FC1?util2d::cvtDepthFromFloat(data.depthRaw()):data.depthRaw());
 				info = tr("Saved rgb/%1.%2 and depth/%1.png.").arg(id).arg(ext);
 			}
 			else if(!data.imageRaw().empty())

guilib/src/PreferencesDialog.cpp

@@ -836,6 +836,7 @@ PreferencesDialog::PreferencesDialog(QWidget * parent) :
 	_ui->rgdb_angularUpdate->setObjectName(Parameters::kRGBDAngularUpdate().c_str());
 	_ui->rgdb_linearSpeedUpdate->setObjectName(Parameters::kRGBDLinearSpeedUpdate().c_str());
 	_ui->rgdb_angularSpeedUpdate->setObjectName(Parameters::kRGBDAngularSpeedUpdate().c_str());
+	_ui->rgbd_savedLocalizationIgnored->setObjectName(Parameters::kRGBDSavedLocalizationIgnored().c_str());
 	_ui->rgdb_rehearsalWeightIgnoredWhileMoving->setObjectName(Parameters::kMemRehearsalWeightIgnoredWhileMoving().c_str());
 	_ui->rgdb_newMapOdomChange->setObjectName(Parameters::kRGBDNewMapOdomChangeDistance().c_str());
 	_ui->odomScanHistory->setObjectName(Parameters::kRGBDNeighborLinkRefining().c_str());
@@ -964,7 +965,6 @@ PreferencesDialog::PreferencesDialog(QWidget * parent) :
 	_ui->doubleSpinBox_grid_noiseRadius->setObjectName(Parameters::kGridNoiseFilteringRadius().c_str());
 	_ui->groupBox_grid_normalsSegmentation->setObjectName(Parameters::kGridNormalsSegmentation().c_str());
 	_ui->checkBox_grid_unknownSpaceFilled->setObjectName(Parameters::kGridScan2dUnknownSpaceFilled().c_str());
-	_ui->doubleSpinBox_grid_unknownSpaceFilledMaxRange->setObjectName(Parameters::kGridScan2dMaxFilledRange().c_str());
 	_ui->spinBox_grid_scanDecimation->setObjectName(Parameters::kGridScanDecimation().c_str());
 
 	_ui->checkBox_grid_fullUpdate->setObjectName(Parameters::kGridGlobalFullUpdate().c_str());

guilib/src/ProgressDialog.cpp

@@ -101,6 +101,7 @@ void ProgressDialog::setCancelButtonVisible(bool visible)
 
 void ProgressDialog::appendText(const QString & text, const QColor & color)
 {
+	UDEBUG(text.toStdString().c_str());
 	_text->setText(text);
 	QString html = tr("<html><font color=\"#999999\">%1 </font><font color=\"%2\">%3</font></html>").arg(QTime::currentTime().toString("HH:mm:ss")).arg(color.name()).arg(text);
 	_detailedText->append(html);

guilib/src/chisel_conversions.h

@@ -0,0 +1,159 @@
+/*
+ * chisel_conversions.h
+ *
+ *  Created on: 2018-03-25
+ *      Author: mathieu
+ */
+
+#ifndef CHISEL_CONVERSIONS_H_
+#define CHISEL_CONVERSIONS_H_
+
+#include <rtabmap/core/CameraModel.h>
+#include <open_chisel/Chisel.h>
+#include <pcl/PolygonMesh.h>
+#include <pcl/common/transforms.h>
+
+namespace rtabmap
+{
+
+std::shared_ptr<chisel::ColorImage<unsigned char> > colorImageToChisel(const cv::Mat & image)
+{
+	UASSERT(image.type() == CV_8UC3 || image.type() == CV_8UC4);
+	std::shared_ptr<chisel::ColorImage<unsigned char> > imageChisel(new chisel::ColorImage<unsigned char>(image.cols, image.rows, image.channels()));
+	memcpy(imageChisel->GetMutableData(), image.data, image.total()*sizeof(unsigned char)*image.channels());
+	return imageChisel;
+}
+
+std::shared_ptr<chisel::DepthImage<float> > depthImageToChisel(const cv::Mat & image)
+{
+	UASSERT(image.type() == CV_32FC1);
+	std::shared_ptr<chisel::DepthImage<float> > imageChisel(new chisel::DepthImage<float>(image.cols, image.rows));
+	memcpy(imageChisel->GetMutableData(), (float*)image.data, image.total()*sizeof(float));
+	return imageChisel;
+}
+
+chisel::PinholeCamera cameraModelToChiselCamera(const CameraModel& camera)
+{
+   chisel::PinholeCamera cameraToReturn;
+   chisel::Intrinsics intrinsics;
+   intrinsics.SetFx(camera.fx());
+   intrinsics.SetFy(camera.fy());
+   intrinsics.SetCx(camera.cx());
+   intrinsics.SetCy(camera.cy());
+   cameraToReturn.SetIntrinsics(intrinsics);
+   cameraToReturn.SetWidth(camera.imageWidth());
+   cameraToReturn.SetHeight(camera.imageHeight());
+   return cameraToReturn;
+}
+
+template<typename PointRGBT>
+chisel::PointCloudPtr pointCloudRGBToChisel(const typename pcl::PointCloud<PointRGBT>& cloud, const Transform & transform = Transform::getIdentity())
+{
+	chisel::PointCloudPtr chiselCloud(new chisel::PointCloud());
+	chiselCloud->GetMutablePoints().resize(cloud.size());
+	chiselCloud->GetMutableColors().resize(cloud.size());
+	float byteToFloat = 1.0f / 255.0f;
+	int oi=0;
+	Eigen::Affine3f transformf = transform.toEigen3f();
+	for(unsigned int i=0; i<cloud.size(); ++i)
+	{
+		const PointRGBT & pt = cloud.at(i);
+		if(pcl::isFinite(pt))
+		{
+			PointRGBT ptt = pcl::transformPoint(pt, transformf);
+
+			chisel::Vec3& xyz =  chiselCloud->GetMutablePoints().at(oi);
+			xyz(0) = ptt.x;
+			xyz(1) = ptt.y;
+			xyz(2) = ptt.z;
+
+			chisel::Vec3& rgb = chiselCloud->GetMutableColors().at(oi);
+			rgb(0) = ptt.r * byteToFloat;
+			rgb(1) = ptt.g * byteToFloat;
+			rgb(2) = ptt.b * byteToFloat;
+
+			++oi;
+		}
+	}
+	chiselCloud->GetMutablePoints().resize(oi);
+	chiselCloud->GetMutableColors().resize(oi);
+	return chiselCloud;
+}
+
+template<typename PointT>
+chisel::PointCloudPtr pointCloudToChisel(const typename pcl::PointCloud<PointT>& cloud, const Transform & transform = Transform::getIdentity())
+{
+	chisel::PointCloudPtr chiselCloud(new chisel::PointCloud());
+	chiselCloud->GetMutablePoints().resize(cloud.size());
+	int oi=0;
+	Eigen::Affine3f transformf = transform.toEigen3f();
+	for(unsigned int i=0; i<cloud.size(); ++i)
+	{
+		const PointT & pt = cloud.at(i);
+		if(pcl::isFinite(pt))
+		{
+			PointT ptt = pcl::transformPoint(pt, transformf);
+
+			chisel::Vec3& xyz =  chiselCloud->GetMutablePoints().at(oi);
+			xyz(0) = ptt.x;
+			xyz(1) = ptt.y;
+			xyz(2) = ptt.z;
+
+			++oi;
+		}
+	}
+	chiselCloud->GetMutablePoints().resize(oi);
+	return chiselCloud;
+}
+
+pcl::PolygonMesh::Ptr chiselToPolygonMesh(const chisel::MeshMap& meshMap, unsigned char r=100, unsigned char g=100, unsigned char b=100)
+{
+	pcl::PolygonMesh::Ptr mesh (new pcl::PolygonMesh);
+
+	if(meshMap.size())
+	{
+		bool hasColor = meshMap.begin()->second->colors.size();
+		pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZRGBNormal>);
+		size_t v = 0;
+		for (const std::pair<chisel::ChunkID, chisel::MeshPtr>& it : meshMap)
+		{
+			UASSERT((!hasColor || (it.second->vertices.size() == it.second->colors.size())) &&
+					it.second->vertices.size() == it.second->normals.size());
+			cloud->resize(cloud->size() + it.second->vertices.size());
+
+			mesh->polygons.resize(mesh->polygons.size()+it.second->vertices.size()/3);
+
+			for (unsigned int i=0;i<it.second->vertices.size(); ++i)
+			{
+				pcl::PointXYZRGBNormal & pt = cloud->at(v);
+				pt.x = it.second->vertices[i][0];
+				pt.y = it.second->vertices[i][1];
+				pt.z = it.second->vertices[i][2];
+				if(hasColor)
+				{
+					pt.r = it.second->colors[i][0] * 255.0f;
+					pt.g = it.second->colors[i][1] * 255.0f;
+					pt.b = it.second->colors[i][2] * 255.0f;
+				}
+				else
+				{
+					pt.r = r;
+					pt.g = g;
+					pt.b = b;
+				}
+				pt.normal_x = it.second->normals[i][0];
+				pt.normal_y = it.second->normals[i][1];
+				pt.normal_z = it.second->normals[i][2];
+				pcl::Vertices & polygon = mesh->polygons.at(v/3);
+				polygon.vertices.push_back(v++);
+			}
+		}
+		pcl::toPCLPointCloud2(*cloud, mesh->cloud);
+	}
+	return mesh;
+}
+
+}
+
+
+#endif /* CHISEL_CONVERSIONS_H_ */

guilib/src/ui/DatabaseViewer.ui

@@ -21,7 +21,16 @@
     <property name="spacing">
      <number>0</number>
     </property>
-    <property name="margin">
+    <property name="leftMargin">
+     <number>0</number>
+    </property>
+    <property name="topMargin">
+     <number>0</number>
+    </property>
+    <property name="rightMargin">
+     <number>0</number>
+    </property>
+    <property name="bottomMargin">
      <number>0</number>
     </property>
     <item>
@@ -52,8 +61,8 @@
           <rect>
            <x>0</x>
            <y>0</y>
-           <width>415</width>
+           <width>398</width>
-           <height>256</height>
+           <height>242</height>
           </rect>
          </property>
          <layout class="QGridLayout" name="gridLayout" columnstretch="0,1">
@@ -244,8 +253,8 @@
           <rect>
            <x>0</x>
            <y>0</y>
-           <width>415</width>
+           <width>397</width>
-           <height>256</height>
+           <height>242</height>
           </rect>
          </property>
          <layout class="QGridLayout" name="gridLayout_2" columnstretch="0,1">
@@ -425,7 +434,16 @@
       </item>
       <item row="1" column="0">
        <layout class="QHBoxLayout" name="horizontalLayout">
-        <property name="margin">
+        <property name="leftMargin">
+         <number>12</number>
+        </property>
+        <property name="topMargin">
+         <number>12</number>
+        </property>
+        <property name="rightMargin">
+         <number>12</number>
+        </property>
+        <property name="bottomMargin">
          <number>12</number>
         </property>
         <item>
@@ -481,7 +499,16 @@
       </item>
       <item row="1" column="1">
        <layout class="QHBoxLayout" name="horizontalLayout_2">
-        <property name="margin">
+        <property name="leftMargin">
+         <number>12</number>
+        </property>
+        <property name="topMargin">
+         <number>12</number>
+        </property>
+        <property name="rightMargin">
+         <number>12</number>
+        </property>
+        <property name="bottomMargin">
          <number>12</number>
         </property>
         <item>
@@ -552,7 +579,7 @@
      <x>0</x>
      <y>0</y>
      <width>1547</width>
-     <height>22</height>
+     <height>25</height>
     </rect>
    </property>
    <widget class="QMenu" name="menuFile">
@@ -583,6 +610,7 @@
     <addaction name="actionDatabase_recovery"/>
     <addaction name="separator"/>
     <addaction name="actionSave_config"/>
+    <addaction name="actionRestore_default_GUI_settings"/>
     <addaction name="separator"/>
     <addaction name="actionGenerate_3D_map_pcd"/>
     <addaction name="actionExport"/>
@@ -590,6 +618,13 @@
     <addaction name="menuExport_poses"/>
     <addaction name="menuExport_GPS"/>
     <addaction name="separator"/>
+    <addaction name="actionExport_saved_2D_map"/>
+    <addaction name="actionImport_2D_map"/>
+    <addaction name="separator"/>
+    <addaction name="actionView_optimized_mesh"/>
+    <addaction name="actionUpdate_optimized_mesh"/>
+    <addaction name="actionExport_optimized_mesh"/>
+    <addaction name="separator"/>
     <addaction name="actionQuit"/>
    </widget>
    <widget class="QMenu" name="menuEdit">
@@ -609,7 +644,6 @@
     <addaction name="actionEdit_depth_image"/>
     <addaction name="separator"/>
     <addaction name="actionReset_all_changes"/>
-    <addaction name="actionRestore_default_GUI_settings"/>
     <addaction name="separator"/>
     <addaction name="actionView_3D_map"/>
    </widget>
@@ -1120,7 +1154,16 @@
    </attribute>
    <widget class="QWidget" name="dockWidgetContents_3">
     <layout class="QVBoxLayout" name="verticalLayout_10">
-     <property name="margin">
+     <property name="leftMargin">
+      <number>0</number>
+     </property>
+     <property name="topMargin">
+      <number>0</number>
+     </property>
+     <property name="rightMargin">
+      <number>0</number>
+     </property>
+     <property name="bottomMargin">
       <number>0</number>
      </property>
      <item>
@@ -1174,8 +1217,8 @@
          <rect>
           <x>0</x>
           <y>0</y>
-          <width>310</width>
+          <width>318</width>
-          <height>222</height>
+          <height>219</height>
          </rect>
         </property>
         <attribute name="label">
@@ -1336,8 +1379,8 @@
          <rect>
           <x>0</x>
           <y>0</y>
-          <width>280</width>
+          <width>282</width>
-          <height>915</height>
+          <height>845</height>
          </rect>
         </property>
         <attribute name="label">
@@ -1844,8 +1887,8 @@
          <rect>
           <x>0</x>
           <y>0</y>
-          <width>201</width>
+          <width>205</width>
-          <height>126</height>
+          <height>117</height>
          </rect>
         </property>
         <attribute name="label">
@@ -1944,8 +1987,8 @@
          <rect>
           <x>0</x>
           <y>0</y>
-          <width>186</width>
+          <width>185</width>
-          <height>496</height>
+          <height>487</height>
          </rect>
         </property>
         <attribute name="label">
@@ -2047,7 +2090,16 @@
      <property name="spacing">
       <number>0</number>
      </property>
-     <property name="margin">
+     <property name="leftMargin">
+      <number>0</number>
+     </property>
+     <property name="topMargin">
+      <number>0</number>
+     </property>
+     <property name="rightMargin">
+      <number>0</number>
+     </property>
+     <property name="bottomMargin">
       <number>0</number>
      </property>
      <item>
@@ -2160,7 +2212,16 @@
      <property name="spacing">
       <number>0</number>
      </property>
-     <property name="margin">
+     <property name="leftMargin">
+      <number>0</number>
+     </property>
+     <property name="topMargin">
+      <number>0</number>
+     </property>
+     <property name="rightMargin">
+      <number>0</number>
+     </property>
+     <property name="bottomMargin">
       <number>0</number>
      </property>
      <item>
@@ -2291,7 +2352,16 @@
    </attribute>
    <widget class="QWidget" name="dockWidgetContents_7">
     <layout class="QVBoxLayout" name="verticalLayout_13">
-     <property name="margin">
+     <property name="leftMargin">
+      <number>0</number>
+     </property>
+     <property name="topMargin">
+      <number>0</number>
+     </property>
+     <property name="rightMargin">
+      <number>0</number>
+     </property>
+     <property name="bottomMargin">
       <number>0</number>
      </property>
      <item>
@@ -2309,7 +2379,16 @@
    </attribute>
    <widget class="QWidget" name="dockWidgetContents_5">
     <layout class="QVBoxLayout" name="verticalLayout_7">
-     <property name="margin">
+     <property name="leftMargin">
+      <number>0</number>
+     </property>
+     <property name="topMargin">
+      <number>0</number>
+     </property>
+     <property name="rightMargin">
+      <number>0</number>
+     </property>
+     <property name="bottomMargin">
       <number>0</number>
      </property>
      <item>
@@ -2354,7 +2433,16 @@
      <property name="spacing">
       <number>0</number>
      </property>
-     <property name="margin">
+     <property name="leftMargin">
+      <number>0</number>
+     </property>
+     <property name="topMargin">
+      <number>0</number>
+     </property>
+     <property name="rightMargin">
+      <number>0</number>
+     </property>
+     <property name="bottomMargin">
       <number>0</number>
      </property>
      <item>
@@ -2546,6 +2634,31 @@
     <string>Close database</string>
    </property>
   </action>
+  <action name="actionExport_saved_2D_map">
+   <property name="text">
+    <string>Export saved 2D map...</string>
+   </property>
+  </action>
+  <action name="actionImport_2D_map">
+   <property name="text">
+    <string>Import 2D map...</string>
+   </property>
+  </action>
+  <action name="actionView_optimized_mesh">
+   <property name="text">
+    <string>View optimized mesh</string>
+   </property>
+  </action>
+  <action name="actionExport_optimized_mesh">
+   <property name="text">
+    <string>Export optimized mesh...</string>
+   </property>
+  </action>
+  <action name="actionUpdate_optimized_mesh">
+   <property name="text">
+    <string>Update optimized mesh...</string>
+   </property>
+  </action>
  </widget>
  <customwidgets>
   <customwidget>

guilib/src/ui/aboutDialog.ui

@@ -2,6 +2,14 @@
 <ui version="4.0">
  <class>aboutDialog</class>
  <widget class="QDialog" name="aboutDialog">
+  <property name="geometry">
+   <rect>
+    <x>0</x>
+    <y>0</y>
+    <width>760</width>
+    <height>738</height>
+   </rect>
+  </property>
   <property name="windowTitle">
    <string>About RTAB-Map</string>
   </property>
@@ -154,9 +162,9 @@ p, li { white-space: pre-wrap; }
            <property name="geometry">
             <rect>
              <x>0</x>
-             <y>0</y>
+             <y>-181</y>
-             <width>565</width>
+             <width>594</width>
-             <height>544</height>
+             <height>567</height>
             </rect>
            </property>
            <layout class="QVBoxLayout" name="verticalLayout_2">
@@ -177,6 +185,19 @@ p, li { white-space: pre-wrap; }
             </property>
             <item>
              <layout class="QGridLayout" name="gridLayout_2" columnstretch="0,0,1">
+              <item row="22" column="1">
+               <widget class="QLabel" name="label_dvo">
+                <property name="text">
+                 <string/>
+                </property>
+                <property name="alignment">
+                 <set>Qt::AlignLeading|Qt::AlignLeft|Qt::AlignVCenter</set>
+                </property>
+                <property name="wordWrap">
+                 <bool>true</bool>
+                </property>
+               </widget>
+              </item>
               <item row="17" column="1">
                <widget class="QLabel" name="label_octomap">
                 <property name="text">
@@ -302,7 +323,7 @@ p, li { white-space: pre-wrap; }
                 </property>
                </widget>
               </item>
-              <item row="19" column="0">
+              <item row="20" column="0">
                <widget class="QLabel" name="label_25">
                 <property name="text">
                  <string>With FOVIS :</string>
@@ -322,6 +343,16 @@ p, li { white-space: pre-wrap; }
                 </property>
                </widget>
               </item>
+              <item row="17" column="2">
+               <widget class="QLabel" name="label_octomap_license">
+                <property name="text">
+                 <string>BSD</string>
+                </property>
+                <property name="wordWrap">
+                 <bool>true</bool>
+                </property>
+               </widget>
+              </item>
               <item row="17" column="0">
                <widget class="QLabel" name="label_20">
                 <property name="text">
@@ -332,20 +363,17 @@ p, li { white-space: pre-wrap; }
                 </property>
                </widget>
               </item>
-              <item row="21" column="1">
+              <item row="21" column="2">
-               <widget class="QLabel" name="label_dvo">
+               <widget class="QLabel" name="label_viso2_license">
                 <property name="text">
-                 <string/>
+                 <string>GPLv3</string>
-                </property>
-                <property name="alignment">
-                 <set>Qt::AlignLeading|Qt::AlignLeft|Qt::AlignVCenter</set>
                 </property>
                 <property name="wordWrap">
                  <bool>true</bool>
                 </property>
                </widget>
               </item>
-              <item row="22" column="0">
+              <item row="23" column="0">
                <widget class="QLabel" name="label_28">
                 <property name="text">
                  <string>With ORB SLAM 2 :</string>
@@ -355,7 +383,7 @@ p, li { white-space: pre-wrap; }
                 </property>
                </widget>
               </item>
-              <item row="22" column="1">
+              <item row="23" column="1">
                <widget class="QLabel" name="label_orbslam2">
                 <property name="text">
                  <string/>
@@ -401,7 +429,7 @@ p, li { white-space: pre-wrap; }
                 </property>
                </widget>
               </item>
-              <item row="19" column="1">
+              <item row="20" column="1">
                <widget class="QLabel" name="label_fovis">
                 <property name="text">
                  <string/>
@@ -414,7 +442,17 @@ p, li { white-space: pre-wrap; }
                 </property>
                </widget>
               </item>
-              <item row="20" column="0">
+              <item row="12" column="0">
+               <widget class="QLabel" name="label_77">
+                <property name="text">
+                 <string>With TORO :</string>
+                </property>
+                <property name="wordWrap">
+                 <bool>true</bool>
+                </property>
+               </widget>
+              </item>
+              <item row="21" column="0">
                <widget class="QLabel" name="label_26">
                 <property name="text">
                  <string>With Viso2 :</string>
@@ -503,6 +541,16 @@ p, li { white-space: pre-wrap; }
                 </property>
                </widget>
               </item>
+              <item row="9" column="0">
+               <widget class="QLabel" name="label_16">
+                <property name="text">
+                 <string>With stereo dc1394 :</string>
+                </property>
+                <property name="wordWrap">
+                 <bool>true</bool>
+                </property>
+               </widget>
+              </item>
               <item row="1" column="1">
                <widget class="QLabel" name="label_opencv_version">
                 <property name="text">
@@ -529,10 +577,10 @@ p, li { white-space: pre-wrap; }
                 </property>
                </widget>
               </item>
-              <item row="9" column="0">
+              <item row="8" column="0">
-               <widget class="QLabel" name="label_16">
+               <widget class="QLabel" name="label_22">
                 <property name="text">
-                 <string>With stereo dc1394 :</string>
+                 <string>With RealSense :</string>
                 </property>
                 <property name="wordWrap">
                  <bool>true</bool>
@@ -565,10 +613,10 @@ p, li { white-space: pre-wrap; }
                 </property>
                </widget>
               </item>
-              <item row="8" column="0">
+              <item row="1" column="2">
-               <widget class="QLabel" name="label_22">
+               <widget class="QLabel" name="label_opencv_license">
                 <property name="text">
-                 <string>With RealSense :</string>
+                 <string>BSD</string>
                 </property>
                 <property name="wordWrap">
                  <bool>true</bool>
@@ -588,6 +636,16 @@ p, li { white-space: pre-wrap; }
                 </property>
                </widget>
               </item>
+              <item row="7" column="2">
+               <widget class="QLabel" name="label_freenect2_license">
+                <property name="text">
+                 <string>Apache v2 and/or GPLv2</string>
+                </property>
+                <property name="wordWrap">
+                 <bool>true</bool>
+                </property>
+               </widget>
+              </item>
               <item row="5" column="1">
                <widget class="QLabel" name="label_freenect">
                 <property name="text">
@@ -601,20 +659,10 @@ p, li { white-space: pre-wrap; }
                 </property>
                </widget>
               </item>
-              <item row="1" column="2">
+              <item row="6" column="2">
-               <widget class="QLabel" name="label_opencv_license">
+               <widget class="QLabel" name="label_openni2_license">
                 <property name="text">
-                 <string>BSD</string>
+                 <string>Apache v2</string>
-                </property>
-                <property name="wordWrap">
-                 <bool>true</bool>
-                </property>
-               </widget>
-              </item>
-              <item row="7" column="2">
-               <widget class="QLabel" name="label_freenect2_license">
-                <property name="text">
-                 <string>Apache v2 and/or GPLv2</string>
                 </property>
                 <property name="wordWrap">
                  <bool>true</bool>
@@ -631,10 +679,10 @@ p, li { white-space: pre-wrap; }
                 </property>
                </widget>
               </item>
-              <item row="6" column="2">
+              <item row="5" column="2">
-               <widget class="QLabel" name="label_openni2_license">
+               <widget class="QLabel" name="label_freenect_license">
                 <property name="text">
-                 <string>Apache v2</string>
+                 <string>Apache v2 and/or GPLv2</string>
                 </property>
                 <property name="wordWrap">
                  <bool>true</bool>
@@ -654,10 +702,10 @@ p, li { white-space: pre-wrap; }
                 </property>
                </widget>
               </item>
-              <item row="5" column="2">
+              <item row="8" column="2">
-               <widget class="QLabel" name="label_freenect_license">
+               <widget class="QLabel" name="label_realsense_license">
                 <property name="text">
-                 <string>Apache v2 and/or GPLv2</string>
+                 <string>Apache-2</string>
                 </property>
                 <property name="wordWrap">
                  <bool>true</bool>
@@ -674,20 +722,10 @@ p, li { white-space: pre-wrap; }
                 </property>
                </widget>
               </item>
-              <item row="8" column="2">
+              <item row="20" column="2">
-               <widget class="QLabel" name="label_realsense_license">
+               <widget class="QLabel" name="label_fovis_license">
                 <property name="text">
-                 <string>Apache-2</string>
+                 <string>GPLv2</string>
-                </property>
-                <property name="wordWrap">
-                 <bool>true</bool>
-                </property>
-               </widget>
-              </item>
-              <item row="17" column="2">
-               <widget class="QLabel" name="label_octomap_license">
-                <property name="text">
-                 <string>BSD</string>
                 </property>
                 <property name="wordWrap">
                  <bool>true</bool>
@@ -704,27 +742,7 @@ p, li { white-space: pre-wrap; }
                 </property>
                </widget>
               </item>
-              <item row="19" column="2">
+              <item row="22" column="2">
-               <widget class="QLabel" name="label_fovis_license">
-                <property name="text">
-                 <string>GPLv2</string>
-                </property>
-                <property name="wordWrap">
-                 <bool>true</bool>
-                </property>
-               </widget>
-              </item>
-              <item row="20" column="2">
-               <widget class="QLabel" name="label_viso2_license">
-                <property name="text">
-                 <string>GPLv3</string>
-                </property>
-                <property name="wordWrap">
-                 <bool>true</bool>
-                </property>
-               </widget>
-              </item>
-              <item row="21" column="2">
                <widget class="QLabel" name="label_dvo_license">
                 <property name="text">
                  <string>GPLv3</string>
@@ -734,6 +752,16 @@ p, li { white-space: pre-wrap; }
                 </property>
                </widget>
               </item>
+              <item row="12" column="2">
+               <widget class="QLabel" name="label_toro_license">
+                <property name="text">
+                 <string>Creative Commons [Attribution-NonCommercial-ShareAlike]</string>
+                </property>
+                <property name="wordWrap">
+                 <bool>true</bool>
+                </property>
+               </widget>
+              </item>
               <item row="12" column="1">
                <widget class="QLabel" name="label_toro">
                 <property name="text">
@@ -757,26 +785,6 @@ p, li { white-space: pre-wrap; }
                 </property>
                </widget>
               </item>
-              <item row="12" column="2">
-               <widget class="QLabel" name="label_toro_license">
-                <property name="text">
-                 <string>Creative Commons [Attribution-NonCommercial-ShareAlike]</string>
-                </property>
-                <property name="wordWrap">
-                 <bool>true</bool>
-                </property>
-               </widget>
-              </item>
-              <item row="12" column="0">
-               <widget class="QLabel" name="label_77">
-                <property name="text">
-                 <string>With TORO :</string>
-                </property>
-                <property name="wordWrap">
-                 <bool>true</bool>
-                </property>
-               </widget>
-              </item>
               <item row="6" column="0">
                <widget class="QLabel" name="label_13">
                 <property name="text">
@@ -787,20 +795,7 @@ p, li { white-space: pre-wrap; }
                 </property>
                </widget>
               </item>
-              <item row="20" column="1">
+              <item row="22" column="0">
-               <widget class="QLabel" name="label_viso2">
-                <property name="text">
-                 <string/>
-                </property>
-                <property name="alignment">
-                 <set>Qt::AlignLeading|Qt::AlignLeft|Qt::AlignVCenter</set>
-                </property>
-                <property name="wordWrap">
-                 <bool>true</bool>
-                </property>
-               </widget>
-              </item>
-              <item row="21" column="0">
                <widget class="QLabel" name="label_27">
                 <property name="text">
                  <string>With DVO :</string>
@@ -820,8 +815,8 @@ p, li { white-space: pre-wrap; }
                 </property>
                </widget>
               </item>
-              <item row="4" column="1">
+              <item row="21" column="1">
-               <widget class="QLabel" name="label_qt_version">
+               <widget class="QLabel" name="label_viso2">
                 <property name="text">
                  <string/>
                 </property>
@@ -853,6 +848,19 @@ p, li { white-space: pre-wrap; }
                 </property>
                </widget>
               </item>
+              <item row="4" column="1">
+               <widget class="QLabel" name="label_qt_version">
+                <property name="text">
+                 <string/>
+                </property>
+                <property name="alignment">
+                 <set>Qt::AlignLeading|Qt::AlignLeft|Qt::AlignVCenter</set>
+                </property>
+                <property name="wordWrap">
+                 <bool>true</bool>
+                </property>
+               </widget>
+              </item>
               <item row="15" column="2">
                <widget class="QLabel" name="label_cvsba_license">
                 <property name="text">
@@ -863,20 +871,20 @@ p, li { white-space: pre-wrap; }
                 </property>
                </widget>
               </item>
-              <item row="22" column="2">
+              <item row="16" column="2">
-               <widget class="QLabel" name="label_orbslam2_license">
+               <widget class="QLabel" name="label_libpointmatcher_license">
                 <property name="text">
-                 <string>GPLv3</string>
+                 <string>BSD</string>
                 </property>
                 <property name="wordWrap">
                  <bool>true</bool>
                 </property>
                </widget>
               </item>
-              <item row="16" column="2">
+              <item row="23" column="2">
-               <widget class="QLabel" name="label_libpointmatcher_license">
+               <widget class="QLabel" name="label_orbslam2_license">
                 <property name="text">
-                 <string>BSD</string>
+                 <string>GPLv3</string>
                 </property>
                 <property name="wordWrap">
                  <bool>true</bool>
@@ -906,6 +914,29 @@ p, li { white-space: pre-wrap; }
                 </property>
                </widget>
               </item>
+              <item row="19" column="0">
+               <widget class="QLabel" name="label_30">
+                <property name="text">
+                 <string>With OpenChisel :</string>
+                </property>
+                <property name="wordWrap">
+                 <bool>true</bool>
+                </property>
+               </widget>
+              </item>
+              <item row="19" column="1">
+               <widget class="QLabel" name="label_openchisel">
+                <property name="text">
+                 <string/>
+                </property>
+                <property name="alignment">
+                 <set>Qt::AlignLeading|Qt::AlignLeft|Qt::AlignVCenter</set>
+                </property>
+                <property name="wordWrap">
+                 <bool>true</bool>
+                </property>
+               </widget>
+              </item>
              </layout>
             </item>
            </layout>

guilib/src/ui/exportCloudsDialog.ui

@@ -23,9 +23,9 @@
       <property name="geometry">
        <rect>
         <x>0</x>
-        <y>-2703</y>
+        <y>0</y>
-        <width>773</width>
+        <width>778</width>
-        <height>4137</height>
+        <height>4697</height>
        </rect>
       </property>
       <layout class="QVBoxLayout" name="verticalLayout_13">
@@ -178,7 +178,7 @@
           </widget>
          </item>
          <item row="11" column="1">
-          <widget class="QLabel" name="label_binaryFile_10">
+          <widget class="QLabel" name="label_smoothing">
            <property name="text">
             <string>Cloud smoothing using Moving Least Squares algorithm (MLS).</string>
            </property>
@@ -1325,7 +1325,7 @@ Guidelines: 4 times the voxel size, 0.025 for voxel=0.</string>
             <item row="0" column="1">
              <widget class="QLabel" name="label_denseReconstruction">
               <property name="text">
-               <string>Surface reconstruction approach. Poisson is available when clouds are assembled and dense reconstruction flavor is selected. CPU-TSDF is available when clouds are assembled, organized reconstruction flavor is selected and RTAB-Map is built with CPU-TSDF support.</string>
+               <string>Surface reconstruction approach. Poisson is available when clouds are assembled and dense reconstruction flavor is selected. CPU-TSDF is available when clouds are assembled, organized reconstruction flavor is selected and RTAB-Map is built with CPU-TSDF support. OpenChisel is available when clouds are assembled and RTAB-Map is built with OpenChisel support.</string>
               </property>
               <property name="wordWrap">
                <bool>true</bool>
@@ -1403,6 +1403,11 @@ Guidelines: 4 times the voxel size, 0.025 for voxel=0.</string>
                 <string>Organized</string>
                </property>
               </item>
+              <item>
+               <property name="text">
+                <string>Open Chisel (TSDF)</string>
+               </property>
+              </item>
              </widget>
             </item>
             <item row="4" column="0">
@@ -2111,7 +2116,7 @@ Guidelines: 4 times the voxel size, 0.025 for voxel=0.</string>
              <item row="0" column="1">
               <widget class="QLabel" name="label_172">
                <property name="text">
-                <string>Depth. Increasing the depth increase the output mesh size but it would be more precise (also longer to compute).</string>
+                <string>Depth. Lowering this parameter decreases reconstruction time, but geometry precision is lower. Minimum polygon size: map length / 2^depth), you can start tuning from depth of 8 or 9. 0 means that depth is chosen so that polygon size is just under 3 cm.</string>
                </property>
                <property name="wordWrap">
                 <bool>true</bool>
@@ -2191,13 +2196,6 @@ Guidelines: 4 times the voxel size, 0.025 for voxel=0.</string>
                </property>
               </widget>
              </item>
-             <item row="0" column="0">
-              <widget class="QSpinBox" name="spinBox_poisson_depth">
-               <property name="minimum">
-                <number>1</number>
-               </property>
-              </widget>
-             </item>
              <item row="8" column="1">
               <widget class="QLabel" name="label_178">
                <property name="text">
@@ -2208,6 +2206,13 @@ Guidelines: 4 times the voxel size, 0.025 for voxel=0.</string>
                </property>
               </widget>
              </item>
+             <item row="2" column="0">
+              <widget class="QCheckBox" name="checkBox_poisson_manifold">
+               <property name="text">
+                <string/>
+               </property>
+              </widget>
+             </item>
              <item row="9" column="1">
               <widget class="QLabel" name="label_179">
                <property name="text">
@@ -2228,8 +2233,8 @@ Guidelines: 4 times the voxel size, 0.025 for voxel=0.</string>
                </property>
               </widget>
              </item>
-             <item row="2" column="0">
+             <item row="1" column="0">
-              <widget class="QCheckBox" name="checkBox_poisson_manifold">
+              <widget class="QCheckBox" name="checkBox_poisson_outputPolygons">
                <property name="text">
                 <string/>
                </property>
@@ -2252,13 +2257,6 @@ Guidelines: 4 times the voxel size, 0.025 for voxel=0.</string>
                </property>
               </widget>
              </item>
-             <item row="1" column="0">
-              <widget class="QCheckBox" name="checkBox_poisson_outputPolygons">
-               <property name="text">
-                <string/>
-               </property>
-              </widget>
-             </item>
              <item row="3" column="0">
               <widget class="QDoubleSpinBox" name="doubleSpinBox_poisson_pointWeight">
                <property name="decimals">
@@ -2276,6 +2274,13 @@ Guidelines: 4 times the voxel size, 0.025 for voxel=0.</string>
                </property>
               </widget>
              </item>
+             <item row="0" column="0">
+              <widget class="QSpinBox" name="spinBox_poisson_depth">
+               <property name="minimum">
+                <number>0</number>
+               </property>
+              </widget>
+             </item>
             </layout>
            </widget>
           </item>
@@ -2601,6 +2606,400 @@ Guidelines: 4 times the voxel size, 0.025 for voxel=0.</string>
             </layout>
            </widget>
           </item>
+          <item>
+           <widget class="QGroupBox" name="groupBox_openchisel">
+            <property name="title">
+             <string>OpenChisel Reconstruction</string>
+            </property>
+            <layout class="QVBoxLayout" name="verticalLayout_18">
+             <item>
+              <widget class="QLabel" name="label_26">
+               <property name="text">
+                <string>&lt;html&gt;&lt;head/&gt;&lt;body&gt;&lt;p&gt;&lt;a href=&quot;https://github.com/personalrobotics/OpenChisel&quot;&gt;&lt;span style=&quot; text-decoration: underline; color:#0000ff;&quot;&gt;https://github.com/personalrobotics/OpenChisel&lt;/span&gt;&lt;/a&gt;&lt;/p&gt;&lt;p&gt;Parameters below match those in chisel_ros package. Voxel resolution is taken from the general voxel parameter above, it should not be null. &lt;br/&gt;&lt;span style=&quot; font-weight:600;&quot;&gt;Issue: &lt;/span&gt;If voxel size or chunk size are changed after exporting/viewing one time, the app should be restarted to avoid black meshes.&lt;/p&gt;&lt;/body&gt;&lt;/html&gt;</string>
+               </property>
+               <property name="wordWrap">
+                <bool>true</bool>
+               </property>
+               <property name="openExternalLinks">
+                <bool>true</bool>
+               </property>
+              </widget>
+             </item>
+             <item>
+              <layout class="QGridLayout" name="gridLayout_19" columnstretch="0,1">
+               <item row="4" column="1">
+                <widget class="QLabel" name="label_36">
+                 <property name="text">
+                  <string>truncation_constant</string>
+                 </property>
+                 <property name="wordWrap">
+                  <bool>true</bool>
+                 </property>
+                </widget>
+               </item>
+               <item row="3" column="1">
+                <widget class="QLabel" name="label_35">
+                 <property name="text">
+                  <string>chunk_size_z</string>
+                 </property>
+                 <property name="wordWrap">
+                  <bool>true</bool>
+                 </property>
+                </widget>
+               </item>
+               <item row="5" column="1">
+                <widget class="QLabel" name="label_40">
+                 <property name="text">
+                  <string>truncation_linear</string>
+                 </property>
+                 <property name="wordWrap">
+                  <bool>true</bool>
+                 </property>
+                </widget>
+               </item>
+               <item row="8" column="1">
+                <widget class="QLabel" name="label_43">
+                 <property name="text">
+                  <string>integration_weight</string>
+                 </property>
+                 <property name="wordWrap">
+                  <bool>true</bool>
+                 </property>
+                </widget>
+               </item>
+               <item row="11" column="1">
+                <widget class="QLabel" name="label_46">
+                 <property name="text">
+                  <string>near_plane_dist</string>
+                 </property>
+                 <property name="wordWrap">
+                  <bool>true</bool>
+                 </property>
+                </widget>
+               </item>
+               <item row="2" column="1">
+                <widget class="QLabel" name="label_34">
+                 <property name="text">
+                  <string>chunk_size_y</string>
+                 </property>
+                 <property name="wordWrap">
+                  <bool>true</bool>
+                 </property>
+                </widget>
+               </item>
+               <item row="2" column="0">
+                <widget class="QSpinBox" name="spinBox_openchisel_chunk_size_y">
+                 <property name="suffix">
+                  <string/>
+                 </property>
+                 <property name="minimum">
+                  <number>1</number>
+                 </property>
+                 <property name="maximum">
+                  <number>99</number>
+                 </property>
+                 <property name="value">
+                  <number>16</number>
+                 </property>
+                </widget>
+               </item>
+               <item row="4" column="0">
+                <widget class="QDoubleSpinBox" name="doubleSpinBox_openchisel_truncation_constant">
+                 <property name="suffix">
+                  <string/>
+                 </property>
+                 <property name="decimals">
+                  <number>6</number>
+                 </property>
+                 <property name="minimum">
+                  <double>0.000001000000000</double>
+                 </property>
+                 <property name="maximum">
+                  <double>1.000000000000000</double>
+                 </property>
+                 <property name="singleStep">
+                  <double>0.001000000000000</double>
+                 </property>
+                 <property name="value">
+                  <double>0.001504000000000</double>
+                 </property>
+                </widget>
+               </item>
+               <item row="3" column="0">
+                <widget class="QSpinBox" name="spinBox_openchisel_chunk_size_z">
+                 <property name="suffix">
+                  <string/>
+                 </property>
+                 <property name="minimum">
+                  <number>1</number>
+                 </property>
+                 <property name="maximum">
+                  <number>99</number>
+                 </property>
+                 <property name="value">
+                  <number>16</number>
+                 </property>
+                </widget>
+               </item>
+               <item row="1" column="0">
+                <widget class="QSpinBox" name="spinBox_openchisel_chunk_size_x">
+                 <property name="suffix">
+                  <string/>
+                 </property>
+                 <property name="minimum">
+                  <number>1</number>
+                 </property>
+                 <property name="maximum">
+                  <number>99</number>
+                 </property>
+                 <property name="value">
+                  <number>16</number>
+                 </property>
+                </widget>
+               </item>
+               <item row="1" column="1">
+                <widget class="QLabel" name="label_31">
+                 <property name="text">
+                  <string>chunk_size_x</string>
+                 </property>
+                 <property name="wordWrap">
+                  <bool>true</bool>
+                 </property>
+                </widget>
+               </item>
+               <item row="7" column="1">
+                <widget class="QLabel" name="label_42">
+                 <property name="text">
+                  <string>truncation_scale</string>
+                 </property>
+                 <property name="wordWrap">
+                  <bool>true</bool>
+                 </property>
+                </widget>
+               </item>
+               <item row="6" column="1">
+                <widget class="QLabel" name="label_41">
+                 <property name="text">
+                  <string>truncation_quadratic</string>
+                 </property>
+                 <property name="wordWrap">
+                  <bool>true</bool>
+                 </property>
+                </widget>
+               </item>
+               <item row="9" column="1">
+                <widget class="QLabel" name="label_44">
+                 <property name="text">
+                  <string>use_voxel_carving</string>
+                 </property>
+                 <property name="wordWrap">
+                  <bool>true</bool>
+                 </property>
+                </widget>
+               </item>
+               <item row="10" column="1">
+                <widget class="QLabel" name="label_45">
+                 <property name="text">
+                  <string>carving_dist_m</string>
+                 </property>
+                 <property name="wordWrap">
+                  <bool>true</bool>
+                 </property>
+                </widget>
+               </item>
+               <item row="12" column="1">
+                <widget class="QLabel" name="label_47">
+                 <property name="text">
+                  <string>far_plane_dist</string>
+                 </property>
+                 <property name="wordWrap">
+                  <bool>true</bool>
+                 </property>
+                </widget>
+               </item>
+               <item row="5" column="0">
+                <widget class="QDoubleSpinBox" name="doubleSpinBox_openchisel_truncation_linear">
+                 <property name="suffix">
+                  <string/>
+                 </property>
+                 <property name="decimals">
+                  <number>6</number>
+                 </property>
+                 <property name="minimum">
+                  <double>0.000001000000000</double>
+                 </property>
+                 <property name="maximum">
+                  <double>1.000000000000000</double>
+                 </property>
+                 <property name="singleStep">
+                  <double>0.001000000000000</double>
+                 </property>
+                 <property name="value">
+                  <double>0.001504000000000</double>
+                 </property>
+                </widget>
+               </item>
+               <item row="6" column="0">
+                <widget class="QDoubleSpinBox" name="doubleSpinBox_openchisel_truncation_quadratic">
+                 <property name="suffix">
+                  <string/>
+                 </property>
+                 <property name="decimals">
+                  <number>6</number>
+                 </property>
+                 <property name="minimum">
+                  <double>0.000001000000000</double>
+                 </property>
+                 <property name="maximum">
+                  <double>1.000000000000000</double>
+                 </property>
+                 <property name="singleStep">
+                  <double>0.001000000000000</double>
+                 </property>
+                 <property name="value">
+                  <double>0.001504000000000</double>
+                 </property>
+                </widget>
+               </item>
+               <item row="7" column="0">
+                <widget class="QDoubleSpinBox" name="doubleSpinBox_openchisel_truncation_scale">
+                 <property name="suffix">
+                  <string/>
+                 </property>
+                 <property name="decimals">
+                  <number>1</number>
+                 </property>
+                 <property name="minimum">
+                  <double>0.100000000000000</double>
+                 </property>
+                 <property name="maximum">
+                  <double>100.000000000000000</double>
+                 </property>
+                 <property name="singleStep">
+                  <double>0.001000000000000</double>
+                 </property>
+                 <property name="value">
+                  <double>0.100000000000000</double>
+                 </property>
+                </widget>
+               </item>
+               <item row="8" column="0">
+                <widget class="QSpinBox" name="spinBox_openchisel_integration_weight">
+                 <property name="suffix">
+                  <string/>
+                 </property>
+                 <property name="minimum">
+                  <number>1</number>
+                 </property>
+                 <property name="maximum">
+                  <number>99</number>
+                 </property>
+                 <property name="value">
+                  <number>16</number>
+                 </property>
+                </widget>
+               </item>
+               <item row="9" column="0">
+                <widget class="QCheckBox" name="checkBox_openchisel_use_voxel_carving">
+                 <property name="text">
+                  <string/>
+                 </property>
+                 <property name="checked">
+                  <bool>true</bool>
+                 </property>
+                </widget>
+               </item>
+               <item row="10" column="0">
+                <widget class="QDoubleSpinBox" name="doubleSpinBox_openchisel_carving_dist_m">
+                 <property name="suffix">
+                  <string> m</string>
+                 </property>
+                 <property name="decimals">
+                  <number>2</number>
+                 </property>
+                 <property name="minimum">
+                  <double>0.010000000000000</double>
+                 </property>
+                 <property name="maximum">
+                  <double>1.000000000000000</double>
+                 </property>
+                 <property name="singleStep">
+                  <double>0.001000000000000</double>
+                 </property>
+                 <property name="value">
+                  <double>0.010000000000000</double>
+                 </property>
+                </widget>
+               </item>
+               <item row="11" column="0">
+                <widget class="QDoubleSpinBox" name="doubleSpinBox_openchisel_near_plane_dist">
+                 <property name="suffix">
+                  <string> m</string>
+                 </property>
+                 <property name="decimals">
+                  <number>2</number>
+                 </property>
+                 <property name="minimum">
+                  <double>0.000000000000000</double>
+                 </property>
+                 <property name="maximum">
+                  <double>100.000000000000000</double>
+                 </property>
+                 <property name="singleStep">
+                  <double>0.001000000000000</double>
+                 </property>
+                 <property name="value">
+                  <double>0.010000000000000</double>
+                 </property>
+                </widget>
+               </item>
+               <item row="12" column="0">
+                <widget class="QDoubleSpinBox" name="doubleSpinBox_openchisel_far_plane_dist">
+                 <property name="suffix">
+                  <string> m</string>
+                 </property>
+                 <property name="decimals">
+                  <number>2</number>
+                 </property>
+                 <property name="minimum">
+                  <double>0.000000000000000</double>
+                 </property>
+                 <property name="maximum">
+                  <double>100.000000000000000</double>
+                 </property>
+                 <property name="singleStep">
+                  <double>0.001000000000000</double>
+                 </property>
+                 <property name="value">
+                  <double>0.000000000000000</double>
+                 </property>
+                </widget>
+               </item>
+               <item row="0" column="1">
+                <widget class="QLabel" name="label_48">
+                 <property name="text">
+                  <string>Simplify generated mesh by merging close vertices together. Required for some post mesh filtering approaches to work properly.</string>
+                 </property>
+                 <property name="wordWrap">
+                  <bool>true</bool>
+                 </property>
+                </widget>
+               </item>
+               <item row="0" column="0">
+                <widget class="QCheckBox" name="checkBox_openchisel_mergeVertices">
+                 <property name="text">
+                  <string/>
+                 </property>
+                 <property name="checked">
+                  <bool>true</bool>
+                 </property>
+                </widget>
+               </item>
+              </layout>
+             </item>
+            </layout>
+           </widget>
+          </item>
           <item>
            <spacer name="verticalSpacer_8">
             <property name="orientation">

guilib/src/ui/preferencesDialog.ui

@@ -63,25 +63,16 @@
           <property name="geometry">
            <rect>
             <x>0</x>
-            <y>0</y>
+            <y>-770</y>
-            <width>678</width>
+            <width>673</width>
-            <height>2811</height>
+            <height>2834</height>
            </rect>
           </property>
           <layout class="QVBoxLayout" name="verticalLayout_16">
            <property name="spacing">
             <number>0</number>
            </property>
-           <property name="leftMargin">
+           <property name="margin">
-            <number>0</number>
-           </property>
-           <property name="topMargin">
-            <number>0</number>
-           </property>
-           <property name="rightMargin">
-            <number>0</number>
-           </property>
-           <property name="bottomMargin">
             <number>0</number>
            </property>
            <item>
@@ -95,7 +86,7 @@
                 <enum>QFrame::Raised</enum>
                </property>
                <property name="currentIndex">
-                <number>5</number>
+                <number>15</number>
                </property>
                <widget class="QWidget" name="page_22">
                 <layout class="QVBoxLayout" name="verticalLayout_29" stretch="0,1">
@@ -4826,16 +4817,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">
+                       <property name="margin">
-                        <number>0</number>
-                       </property>
-                       <property name="topMargin">
-                        <number>0</number>
-                       </property>
-                       <property name="rightMargin">
-                        <number>0</number>
-                       </property>
-                       <property name="bottomMargin">
                         <number>0</number>
                        </property>
                        <item>
@@ -8540,7 +8522,7 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag
                          </property>
                         </widget>
                        </item>
-                       <item row="9" column="1">
+                       <item row="10" 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>
@@ -8553,7 +8535,7 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag
                          </property>
                         </widget>
                        </item>
-                       <item row="5" column="1">
+                       <item row="6" column="1">
                         <widget class="QLabel" name="label_scanMatching">
                          <property name="text">
                           <string>Neighbor link refining. When a new node is added to the graph, the transformation of its neighbor link (odometry) with the previous node is refined using ICP registration approach (laser scans required).</string>
@@ -8566,7 +8548,7 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag
                          </property>
                         </widget>
                        </item>
-                       <item row="7" column="1">
+                       <item row="8" 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>
@@ -8579,7 +8561,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="QDoubleSpinBox" name="localDetection_radius">
                          <property name="suffix">
                           <string> m</string>
@@ -8602,7 +8584,7 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag
                          </property>
                         </widget>
                        </item>
-                       <item row="8" column="0">
+                       <item row="9" column="0">
                         <widget class="QDoubleSpinBox" name="rgdb_localImmunizationRatio">
                          <property name="suffix">
                           <string/>
@@ -8634,6 +8616,13 @@ 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="0" column="1">
                         <widget class="QLabel" name="label_153">
                          <property name="text">
@@ -8647,7 +8636,10 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag
                          </property>
                         </widget>
                        </item>
-                       <item row="4" column="1">
+                       <item row="8" column="0">
+                        <widget class="QSpinBox" name="spinBox_maxLocalLocationsRetrieved"/>
+                       </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>
@@ -8660,16 +8652,6 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag
                          </property>
                         </widget>
                        </item>
-                       <item row="5" column="0">
-                        <widget class="QCheckBox" name="odomScanHistory">
-                         <property name="text">
-                          <string/>
-                         </property>
-                        </widget>
-                       </item>
-                       <item row="7" column="0">
-                        <widget class="QSpinBox" name="spinBox_maxLocalLocationsRetrieved"/>
-                       </item>
                        <item row="1" column="0">
                         <widget class="QDoubleSpinBox" name="rgdb_angularUpdate">
                          <property name="suffix">
@@ -8686,7 +8668,7 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag
                          </property>
                         </widget>
                        </item>
-                       <item row="4" column="0">
+                       <item row="5" column="0">
                         <widget class="QDoubleSpinBox" name="rgdb_newMapOdomChange">
                          <property name="suffix">
                           <string> m</string>
@@ -8705,7 +8687,14 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag
                          </property>
                         </widget>
                        </item>
-                       <item row="8" column="1">
+                       <item row="7" column="0">
+                        <widget class="QCheckBox" name="loopClosure_reextract">
+                         <property name="text">
+                          <string/>
+                         </property>
+                        </widget>
+                       </item>
+                       <item row="9" 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>
@@ -8718,7 +8707,7 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag
                          </property>
                         </widget>
                        </item>
-                       <item row="6" column="1">
+                       <item row="7" column="1">
                         <widget class="QLabel" name="label_scanMatching_9">
                          <property name="text">
                           <string>Re-extract visual features when computing loop closure transformations.</string>
@@ -8731,13 +8720,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="loopClosure_reextract">
-                         <property name="text">
-                          <string/>
-                         </property>
-                        </widget>
-                       </item>
                        <item row="2" column="0">
                         <widget class="QDoubleSpinBox" name="rgdb_linearSpeedUpdate">
                          <property name="suffix">
@@ -8780,6 +8762,26 @@ 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="4" column="0">
+                        <widget class="QCheckBox" name="rgbd_savedLocalizationIgnored">
+                         <property name="text">
+                          <string/>
+                         </property>
+                        </widget>
+                       </item>
                       </layout>
                      </widget>
                     </item>
@@ -10037,35 +10039,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="doubleSpinBox_grid_unknownSpaceFilledMaxRange">
-                           <property name="suffix">
-                            <string> m</string>
-                           </property>
-                           <property name="decimals">
-                            <number>1</number>
-                           </property>
-                           <property name="singleStep">
-                            <double>1.000000000000000</double>
-                           </property>
-                           <property name="value">
-                            <double>6.000000000000000</double>
-                           </property>
-                          </widget>
-                         </item>
-                         <item row="1" column="1">
-                          <widget class="QLabel" name="label_329">
-                           <property name="text">
-                            <string>Unknown space filled maximum range. If 0, the laser scan maximum range is used.</string>
-                           </property>
-                           <property name="wordWrap">
-                            <bool>true</bool>
-                           </property>
-                           <property name="textInteractionFlags">
-                            <set>Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse</set>
-                           </property>
-                          </widget>
-                         </item>
                         </layout>
                        </item>
                       </layout>
@@ -13873,16 +13846,7 @@ Lower the ratio -&gt; higher the precision. 0 means disabled, matching the neare
                         <property name="spacing">
                          <number>0</number>
                         </property>
-                        <property name="leftMargin">
+                        <property name="margin">
-                         <number>0</number>
-                        </property>
-                        <property name="topMargin">
-                         <number>0</number>
-                        </property>
-                        <property name="rightMargin">
-                         <number>0</number>
-                        </property>
-                        <property name="bottomMargin">
                          <number>0</number>
                         </property>
                         <item>
@@ -13962,16 +13926,7 @@ Lower the ratio -&gt; higher the precision. 0 means disabled, matching the neare
                         <property name="spacing">
                          <number>0</number>
                         </property>
-                        <property name="leftMargin">
+                        <property name="margin">
-                         <number>0</number>
-                        </property>
-                        <property name="topMargin">
-                         <number>0</number>
-                        </property>
-                        <property name="rightMargin">
-                         <number>0</number>
-                        </property>
-                        <property name="bottomMargin">
                          <number>0</number>
                         </property>
                         <item>
@@ -14083,16 +14038,7 @@ Lower the ratio -&gt; higher the precision. 0 means disabled, matching the neare
                         <property name="spacing">
                          <number>0</number>
                         </property>
-                        <property name="leftMargin">
+                        <property name="margin">
-                         <number>0</number>
-                        </property>
-                        <property name="topMargin">
-                         <number>0</number>
-                        </property>
-                        <property name="rightMargin">
-                         <number>0</number>
-                        </property>
-                        <property name="bottomMargin">
                          <number>0</number>
                         </property>
                         <item>

package.xml

@@ -1,7 +1,7 @@
 <?xml version="1.0"?>
 <package>
   <name>rtabmap</name>
-  <version>0.16.3</version>
+  <version>0.17.0</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>
@@ -24,6 +24,7 @@
   <!-- libproj-dev needed due to error in vtk6 (kinetic)-->
   <build_depend>proj</build_depend>
   <build_depend>octomap</build_depend>
+  <build_depend>libg2o</build_depend>
   
   <run_depend>qt_gui_cpp</run_depend>
   <run_depend>libpcl-all-dev</run_depend>
@@ -35,6 +36,7 @@
   <!-- <run_depend>libopenni2-dev</run_depend> -->
   <run_depend>cv_bridge</run_depend>
   <run_depend>octomap</run_depend>
+  <run_depend>libg2o</run_depend>
 
   <export>
 	 <build_type>cmake</build_type>

tools/EurocDataset/main.cpp

@@ -10,6 +10,7 @@ modification, are permitted provided that the following conditions are met:
       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.
 
@@ -44,6 +45,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #include <yaml-cpp/yaml.h>
 #include <stdio.h>
 #include <signal.h>
+#include <fstream>
 
 using namespace rtabmap;
 
@@ -378,16 +380,16 @@ int main(int argc, char * argv[])
 					cv::Vec3d gyr;
 					for (int j = 0; j < 3; ++j) {
 						std::getline(stream, s, ',');
-						gyr[j] = std::stof(s);
+						gyr[j] = uStr2Double(s);
 					}
 
 					cv::Vec3d acc;
 					for (int j = 0; j < 3; ++j) {
 						std::getline(stream, s, ',');
-						acc[j] = std::stof(s);
+						acc[j] = uStr2Double(s);
 					}
 
-					t_imu = double(std::stoi(seconds)) + double(std::stoi(nanoseconds))*1e-9;
+					t_imu = double(uStr2Int(seconds)) + double(uStr2Int(nanoseconds))*1e-9;
 
 					if (t_imu - start + 1 > 0) {
 						SensorData dataImu(IMU(gyr, cv::Mat(3,3,CV_64FC1), acc, cv::Mat(3,3,CV_64FC1), baseToImu), 0, t_imu);