Freenect2: Fixed initRectifyMap error with custom calibration

Odometry: Added additional debug information on dt>0 assert

CMakeLists.txt

@@ -6,9 +6,10 @@ SET(PROJECT_PREFIX rtabmap)
 
 # Catkin doesn't support multiarch library path, 
 # fix to "lib" if not set by user.
-#IF(NOT DEFINED CMAKE_INSTALL_LIBDIR)
+OPTION(MULTI_ARCH "Activate multi-arch lib directory (debian)" OFF)
-#   set(CMAKE_INSTALL_LIBDIR "lib")
+IF(NOT MULTI_ARCH AND NOT DEFINED CMAKE_INSTALL_LIBDIR)
-#ENDIF(NOT DEFINED CMAKE_INSTALL_LIBDIR)
+   set(CMAKE_INSTALL_LIBDIR "lib")
+ENDIF(NOT MULTI_ARCH AND NOT DEFINED CMAKE_INSTALL_LIBDIR)
 
 INCLUDE(GNUInstallDirs)
 
@@ -20,7 +21,7 @@ SET(CMAKE_MODULE_PATH "${PROJECT_SOURCE_DIR}/cmake_modules")
 #######################
 SET(RTABMAP_MAJOR_VERSION 0)
 SET(RTABMAP_MINOR_VERSION 11)
-SET(RTABMAP_PATCH_VERSION 7)
+SET(RTABMAP_PATCH_VERSION 8)
 SET(RTABMAP_VERSION
   ${RTABMAP_MAJOR_VERSION}.${RTABMAP_MINOR_VERSION}.${RTABMAP_PATCH_VERSION})
   
@@ -137,6 +138,8 @@ option(WITH_VERTIGO       "Include Vertigo support"              ON)
 option(WITH_CVSBA         "Include cvsba support"                ON)
 option(WITH_FLYCAPTURE2   "Include FlyCapture2/Triclops support" ON)
 option(WITH_ZED           "Include ZED sdk support"              ON)
+option(WITH_OCTOMAP       "Include Octomap support"              ON)
+option(PCL_OMP            "With PCL OMP implementations"         ON)
 
 FIND_PACKAGE(OpenCV REQUIRED QUIET)
 FIND_PACKAGE(PCL 1.7 REQUIRED QUIET)
@@ -156,6 +159,9 @@ if(OPENMP_FOUND)
   set(CMAKE_C_FLAGS "${CMAKE_C_FLAGS} ${OpenMP_C_FLAGS}")
   set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} ${OpenMP_CXX_FLAGS}")
   message (STATUS "Found OpenMP")
+  if(PCL_OMP)
+    add_definitions(-DPCL_OMP)
+  endif(PCL_OMP)
 else(OPENMP_FOUND)
   message (STATUS "Not found OpenMP")
 endif()
@@ -264,7 +270,7 @@ IF(WITH_ZED)
 			LINK_DIRECTORIES( ${LINK_DIRECTORIES} ${ZED_LIBRARY_DIR})
 		ENDIF(ZED_LIBRARIES AND ZED_INCLUDE_DIRS)
 	ELSE() # Linux
-		find_package(ZED 0.9 QUIET)
+		find_package(ZED 1 QUIET)
 	ENDIF(WIN32)
 	
 	IF(ZED_FOUND)
@@ -279,6 +285,13 @@ IF(WITH_ZED)
     ENDIF(ZED_FOUND)
 ENDIF(WITH_ZED)
 
+IF(WITH_OCTOMAP)
+	FIND_PACKAGE(OCTOMAP QUIET)
+    IF(OCTOMAP_FOUND)
+       MESSAGE(STATUS "Found octomap: ${OCTOMAP_INCLUDE_DIRS}")
+    ENDIF(OCTOMAP_FOUND)
+ENDIF(WITH_OCTOMAP)
+
 IF(G2O_FOUND OR GTSAM_FOUND OR ZED_FOUND)
    #Newest versions require std11
    IF(NOT MSVC)
@@ -383,6 +396,11 @@ IF(NOT ZED_FOUND)
 ELSE()
    SET(CONF_DEPENDENCIES ${CONF_DEPENDENCIES} ${ZED_LIBRARIES} ${CUDA_LIBRARIES}) 
 ENDIF()
+IF(NOT OCTOMAP_FOUND)
+   SET(OCTOMAP "//")
+ELSE()
+   SET(CONF_DEPENDENCIES ${CONF_DEPENDENCIES} ${OCTOMAP_LIBRARIES}) 
+ENDIF()
 IF(NOT (OpenCV_FOUND AND OpenCV_VERSION_MAJOR EQUAL 3))
    SET(OPENCV3 "//")
 ENDIF(NOT (OpenCV_FOUND AND OpenCV_VERSION_MAJOR EQUAL 3))
@@ -669,6 +687,14 @@ ELSE()
 MESSAGE(STATUS "  With ZED                  = NO (ZED sdk not found)")
 ENDIF()
 
+IF(OCTOMAP_FOUND)
+MESSAGE(STATUS "  With OCTOMAP              = YES (License: BSD)")
+ELSEIF(NOT WITH_OCTOMAP)
+MESSAGE(STATUS "  With OCTOMAP              = NO (WITH_OCTOMAP=OFF)")
+ELSE()
+MESSAGE(STATUS "  With OCTOMAP              = NO (octomap not found)")
+ENDIF()
+
 IF(QT4_FOUND)
 MESSAGE(STATUS "  With Qt4                  = YES (License: Open Source or Commercial)")
 ELSEIF(Qt5_FOUND)

LICENSE

@@ -1,4 +1,4 @@
-Copyright (c) 2010-2015, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
+Copyright (c) 2010-2016, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
 All rights reserved.
 
 Redistribution and use in source and binary forms, with or without

Version.h.in

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

app/android/jni/CameraTango.cpp

@@ -464,7 +464,7 @@ rtabmap::Transform CameraTango::getPoseAtTimestamp(double timestamp, bool inOpen
 	return pose;
 }
 
-SensorData CameraTango::captureImage()
+SensorData CameraTango::captureImage(CameraInfo * info)
 {
 	LOGI("Capturing image...");
 

app/android/jni/CameraTango.h

@@ -86,7 +86,7 @@ public:
 	void tangoEventReceived(int type, const char * key, const char * value);
 
 protected:
-	virtual SensorData captureImage();
+	virtual SensorData captureImage(CameraInfo * info = 0);
 
 private:
 	rtabmap::Transform getPoseAtTimestamp(double timestamp, bool inOpenGLFrame);

app/src/CMakeLists.txt

@@ -150,7 +150,7 @@ IF((APPLE AND BUILD_AS_BUNDLE) OR WIN32)
 		get_filename_component(plugin_dir ${plugin_loc} DIRECTORY)
 		string(REPLACE "plugins" ";" loc_list ${plugin_dir})
 		list(GET loc_list 1 plugin_type)
-	    #MESSAGE(STATUS "Qt5 plugin "${plugin_loc}" installed in "${plugin_dest_dir}/plugins${plugin_type}"")
+	    #MESSAGE(STATUS "Qt5 plugin \"${plugin_loc}\" installed in \"${plugin_dest_dir}/plugins${plugin_type}\"")
 		INSTALL(FILES ${plugin_loc}
           DESTINATION ${plugin_dest_dir}/plugins${plugin_type}
           COMPONENT runtime)

app/src/ObjDeletionHandler.h

@@ -1,5 +1,5 @@
 /*
-Copyright (c) 2010-2014, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
+Copyright (c) 2010-2016, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
 All rights reserved.
 
 Redistribution and use in source and binary forms, with or without

app/src/main.cpp

@@ -1,5 +1,5 @@
 /*
-Copyright (c) 2010-2014, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
+Copyright (c) 2010-2016, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
 All rights reserved.
 
 Redistribution and use in source and binary forms, with or without

corelib/include/rtabmap/core/BayesFilter.h

@@ -1,5 +1,5 @@
 /*
-Copyright (c) 2010-2014, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
+Copyright (c) 2010-2016, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
 All rights reserved.
 
 Redistribution and use in source and binary forms, with or without

corelib/include/rtabmap/core/Camera.h

@@ -1,5 +1,5 @@
 /*
-Copyright (c) 2010-2014, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
+Copyright (c) 2010-2016, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
 All rights reserved.
 
 Redistribution and use in source and binary forms, with or without
@@ -56,6 +56,7 @@ public:
 	virtual bool init(const std::string & calibrationFolder = ".", const std::string & cameraName = "") = 0;
 	virtual bool isCalibrated() const = 0;
 	virtual std::string getSerial() const = 0;
+	virtual bool odomProvided() const { return false; }
 
 	//getters
 	float getImageRate() const {return _imageRate;}
@@ -76,7 +77,7 @@ protected:
 	/**
 	 * returned rgb and depth images should be already rectified if calibration was loaded
 	 */
-	virtual SensorData captureImage() = 0;
+	virtual SensorData captureImage(CameraInfo * info = 0) = 0;
 
 	int getNextSeqID() {return ++_seq;}
 

corelib/include/rtabmap/core/CameraEvent.h

@@ -1,5 +1,5 @@
 /*
-Copyright (c) 2010-2014, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
+Copyright (c) 2010-2016, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
 All rights reserved.
 
 Redistribution and use in source and binary forms, with or without

corelib/include/rtabmap/core/CameraInfo.h

@@ -1,5 +1,5 @@
 /*
-Copyright (c) 2010-2014, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
+Copyright (c) 2010-2016, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
 All rights reserved.
 
 Redistribution and use in source and binary forms, with or without
@@ -39,22 +39,27 @@ public:
 	CameraInfo() :
 		cameraName(""),
 		id(0),
+		stamp(0.0),
 		timeCapture(0.0f),
 		timeDisparity(0.0f),
 		timeMirroring(0.0f),
 		timeImageDecimation(0.0f),
-		timeScanFromDepth(0.0f)
+		timeScanFromDepth(0.0f),
+		odomCovariance(cv::Mat::eye(6,6,CV_64FC1))
 	{
 	}
 	virtual ~CameraInfo() {}
 
 	std::string cameraName;
 	int id;
+	double stamp;
 	float timeCapture;
 	float timeDisparity;
 	float timeMirroring;
 	float timeImageDecimation;
 	float timeScanFromDepth;
+	Transform odomPose;
+	cv::Mat odomCovariance;
 };
 
 } // namespace rtabmap

corelib/include/rtabmap/core/CameraModel.h

@@ -1,5 +1,5 @@
 /*
-Copyright (c) 2010-2014, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
+Copyright (c) 2010-2016, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
 All rights reserved.
 
 Redistribution and use in source and binary forms, with or without

corelib/include/rtabmap/core/CameraRGB.h

@@ -1,5 +1,5 @@
 /*
-Copyright (c) 2010-2014, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
+Copyright (c) 2010-2016, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
 All rights reserved.
 
 Redistribution and use in source and binary forms, with or without
@@ -119,7 +119,7 @@ public:
 	}
 
 protected:
-	virtual SensorData captureImage();
+	virtual SensorData captureImage(CameraInfo * info = 0);
 
 private:
 	std::string _path;
@@ -178,6 +178,7 @@ public:
 
 public:
 	CameraVideo(int usbDevice = 0,
+			bool rectifyImages = false,
 			float imageRate = 0,
 			const Transform & localTransform = Transform::getIdentity());
 	CameraVideo(const std::string & filePath,
@@ -193,7 +194,7 @@ public:
 	const std::string & getFilePath() const {return _filePath;}
 
 protected:
-	virtual SensorData captureImage();
+	virtual SensorData captureImage(CameraInfo * info = 0);
 
 private:
 	// File type

corelib/include/rtabmap/core/CameraRGBD.h

@@ -1,5 +1,5 @@
 /*
-Copyright (c) 2010-2014, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
+Copyright (c) 2010-2016, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
 All rights reserved.
 
 Redistribution and use in source and binary forms, with or without
@@ -97,7 +97,7 @@ public:
     virtual std::string getSerial() const;
 
 protected:
-	virtual SensorData captureImage();
+	virtual SensorData captureImage(CameraInfo * info = 0);
 
 private:
     pcl::Grabber* interface_;
@@ -131,7 +131,7 @@ public:
 	virtual std::string getSerial() const {return "";} // unknown with OpenCV
 
 protected:
-	virtual SensorData captureImage();
+	virtual SensorData captureImage(CameraInfo * info = 0);
 
 private:
 	bool _asus;
@@ -168,7 +168,7 @@ public:
 	void setOpenNI2StampsAndIDsUsed(bool used) {_openNI2StampsAndIDsUsed = used;}
 
 protected:
-	virtual SensorData captureImage();
+	virtual SensorData captureImage(CameraInfo * info = 0);
 
 private:
 	openni::Device * _device;
@@ -204,7 +204,7 @@ public:
 	virtual std::string getSerial() const;
 
 protected:
-	virtual SensorData captureImage();
+	virtual SensorData captureImage(CameraInfo * info = 0);
 
 private:
 	int deviceId_;
@@ -249,7 +249,7 @@ public:
 	virtual std::string getSerial() const;
 
 protected:
-	virtual SensorData captureImage();
+	virtual SensorData captureImage(CameraInfo * info = 0);
 
 private:
 	int deviceId_;
@@ -291,7 +291,7 @@ public:
 	virtual std::string getSerial() const;
 
 protected:
-	virtual SensorData captureImage();
+	virtual SensorData captureImage(CameraInfo * info = 0);
 
 private:
 	CameraImages cameraDepth_;

corelib/include/rtabmap/core/CameraStereo.h

@@ -1,5 +1,5 @@
 /*
-Copyright (c) 2010-2014, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
+Copyright (c) 2010-2016, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
 All rights reserved.
 
 Redistribution and use in source and binary forms, with or without
@@ -70,7 +70,7 @@ public:
 	virtual std::string getSerial() const;
 
 protected:
-	virtual SensorData captureImage();
+	virtual SensorData captureImage(CameraInfo * info = 0);
 
 private:
 	DC1394Device *device_;
@@ -95,7 +95,7 @@ public:
 	virtual std::string getSerial() const;
 
 protected:
-	virtual SensorData captureImage();
+	virtual SensorData captureImage(CameraInfo * info = 0);
 
 private:
 	FlyCapture2::Camera * camera_;
@@ -118,6 +118,7 @@ public:
 			int quality = 1,    // 0=NONE, 1=PERFORMANCE, 2=QUALITY
 			int sensingMode = 1,// 0=FULL, 1=RAW
 			int confidenceThr = 100,
+			bool computeOdometry = false,
 			float imageRate=0.0f,
 			const Transform & localTransform = Transform::getIdentity());
 	CameraStereoZed(
@@ -125,6 +126,7 @@ public:
 			int quality = 1,    // 0=NONE, 1=PERFORMANCE, 2=QUALITY
 			int sensingMode = 1,// 0=FULL, 1=RAW
 			int confidenceThr = 100,
+			bool computeOdometry = false,
 			float imageRate=0.0f,
 			const Transform & localTransform = Transform::getIdentity());
 	virtual ~CameraStereoZed();
@@ -132,9 +134,10 @@ public:
 	virtual bool init(const std::string & calibrationFolder = ".", const std::string & cameraName = "");
 	virtual bool isCalibrated() const;
 	virtual std::string getSerial() const;
+	virtual bool odomProvided() const { return computeOdometry_; }
 
 protected:
-	virtual SensorData captureImage();
+	virtual SensorData captureImage(CameraInfo * info = 0);
 
 private:
 	sl::zed::Camera * zed_;
@@ -146,6 +149,8 @@ private:
 	int quality_;
 	int sensingMode_;
 	int confidenceThr_;
+	bool computeOdometry_;
+	bool lost_;
 };
 
 /////////////////////////
@@ -177,7 +182,7 @@ public:
 	virtual std::string getSerial() const;
 
 protected:
-	virtual SensorData captureImage();
+	virtual SensorData captureImage(CameraInfo * info = 0);
 
 private:
 	CameraImages * camera2_;
@@ -203,6 +208,7 @@ public:
 			const Transform & localTransform = Transform::getIdentity());
 	CameraStereoVideo(
 			int device,
+			bool rectifyImages = false,
 			float imageRate = 0.0f,
 			const Transform & localTransform = Transform::getIdentity());
 	virtual ~CameraStereoVideo();
@@ -212,7 +218,7 @@ public:
 	virtual std::string getSerial() const;
 
 protected:
-	virtual SensorData captureImage();
+	virtual SensorData captureImage(CameraInfo * info = 0);
 
 private:
 	cv::VideoCapture capture_;

corelib/include/rtabmap/core/CameraThread.h

@@ -1,5 +1,5 @@
 /*
-Copyright (c) 2010-2014, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
+Copyright (c) 2010-2016, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
 All rights reserved.
 
 Redistribution and use in source and binary forms, with or without

corelib/include/rtabmap/core/Compression.h

@@ -1,5 +1,5 @@
 /*
-Copyright (c) 2010-2014, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
+Copyright (c) 2010-2016, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
 All rights reserved.
 
 Redistribution and use in source and binary forms, with or without

corelib/include/rtabmap/core/DBDriver.h

@@ -1,5 +1,5 @@
 /*
-Copyright (c) 2010-2014, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
+Copyright (c) 2010-2016, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
 All rights reserved.
 
 Redistribution and use in source and binary forms, with or without
@@ -88,7 +88,7 @@ public:
 	void updateLink(const Link & link);
 
 public:
-	void addStatisticsAfterRun(int stMemSize, int lastSignAdded, int processMemUsed, int databaseMemUsed, int dictionarySize) const;
+	void addStatisticsAfterRun(int stMemSize, int lastSignAdded, int processMemUsed, int databaseMemUsed, int dictionarySize, const ParametersMap & parameters) const;
 
 public:
 	// Mutex-protected methods of abstract versions below
@@ -107,6 +107,7 @@ public:
 	int getLastDictionarySize() const; // working memory
 	int getTotalNodesSize() const;
 	int getTotalDictionarySize() const;
+	ParametersMap getLastParameters() const;
 
 	void executeNoResult(const std::string & sql) const;
 
@@ -149,6 +150,7 @@ private:
 	virtual int getLastDictionarySizeQuery() const = 0;
 	virtual int getTotalNodesSizeQuery() const = 0;
 	virtual int getTotalDictionarySizeQuery() const = 0;
+	virtual ParametersMap getLastParametersQuery() const = 0;
 
 	virtual void executeNoResultQuery(const std::string & sql) const = 0;
 

corelib/include/rtabmap/core/DBReader.h

@@ -1,5 +1,5 @@
 /*
-Copyright (c) 2010-2014, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
+Copyright (c) 2010-2016, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
 All rights reserved.
 
 Redistribution and use in source and binary forms, with or without
@@ -30,11 +30,9 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 
 #include "rtabmap/core/RtabmapExp.h" // DLL export/import defines
 
-#include <rtabmap/utilite/UThreadNode.h>
 #include <rtabmap/utilite/UTimer.h>
-#include <rtabmap/utilite/UEventsSender.h>
 #include <rtabmap/core/Transform.h>
-#include <rtabmap/core/OdometryEvent.h>
+#include <rtabmap/core/Camera.h>
 
 #include <opencv2/core/core.hpp>
 
@@ -45,34 +43,45 @@ namespace rtabmap {
 
 class DBDriver;
 
-class RTABMAP_EXP DBReader : public UThreadNode, public UEventsSender {
+class RTABMAP_EXP DBReader : public Camera {
 public:
 	DBReader(const std::string & databasePath,
-			 float frameRate = 0.0f,
+			 float frameRate = 0.0f, // -1 = use Database stamps, 0 = inf
 			 bool odometryIgnored = false,
 			 bool ignoreGoalDelay = false,
-			 bool goalsIgnored = false);
+			 bool goalsIgnored = false,
+			 int startIndex = 0,
+			 int cameraIndex = -1);
 	DBReader(const std::list<std::string> & databasePaths,
-			 float frameRate = 0.0f,
+			 float frameRate = 0.0f, // -1 = use Database stamps, 0 = inf
 			 bool odometryIgnored = false,
 			 bool ignoreGoalDelay = false,
-			 bool goalsIgnored = false);
+			 bool goalsIgnored = false,
+			 int startIndex = 0,
+			 int cameraIndex = -1);
 	virtual ~DBReader();
 
-	bool init(int startIndex=0);
+	virtual bool init(
-	void setFrameRate(float frameRate);
+			const std::string & calibrationFolder = ".",
-	OdometryEvent getNextData();
+			const std::string & cameraName = "");
+
+	virtual bool isCalibrated() const;
+	virtual std::string getSerial() const;
+	virtual bool odomProvided() const {return !_odometryIgnored;}
 
 protected:
-	virtual void mainLoopBegin();
+	virtual SensorData captureImage(CameraInfo * info = 0);
-	virtual void mainLoop();
+
+private:
+	SensorData getNextData(CameraInfo * info = 0);
 
 private:
 	std::list<std::string> _paths;
-	float _frameRate; // -1 = use Database stamps, 0 = inf
 	bool _odometryIgnored;
 	bool _ignoreGoalDelay;
 	bool _goalsIgnored;
+	int _startIndex;
+	int _cameraIndex;
 
 	DBDriver * _dbDriver;
 	UTimer _timer;
@@ -80,6 +89,7 @@ private:
 	std::set<int>::iterator _currentId;
 	double _previousStamp;
 	int _previousMapID;
+	bool _calibrated;
 };
 
 } /* namespace rtabmap */

corelib/include/rtabmap/core/EpipolarGeometry.h

@@ -1,5 +1,5 @@
 /*
-Copyright (c) 2010-2014, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
+Copyright (c) 2010-2016, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
 All rights reserved.
 
 Redistribution and use in source and binary forms, with or without

corelib/include/rtabmap/core/Features2d.h

@@ -1,5 +1,5 @@
 /*
-Copyright (c) 2010-2014, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
+Copyright (c) 2010-2016, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
 All rights reserved.
 
 Redistribution and use in source and binary forms, with or without

corelib/include/rtabmap/core/Graph.h

@@ -1,5 +1,5 @@
 /*
-Copyright (c) 2010-2014, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
+Copyright (c) 2010-2016, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
 All rights reserved.
 
 Redistribution and use in source and binary forms, with or without

corelib/include/rtabmap/core/Image.h

@@ -1,5 +1,5 @@
 /*
-Copyright (c) 2010-2014, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
+Copyright (c) 2010-2016, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
 All rights reserved.
 
 Redistribution and use in source and binary forms, with or without

corelib/include/rtabmap/core/Link.h

@@ -1,5 +1,5 @@
 /*
-Copyright (c) 2010-2014, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
+Copyright (c) 2010-2016, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
 All rights reserved.
 
 Redistribution and use in source and binary forms, with or without

corelib/include/rtabmap/core/Memory.h

@@ -1,5 +1,5 @@
 /*
-Copyright (c) 2010-2014, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
+Copyright (c) 2010-2016, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
 All rights reserved.
 
 Redistribution and use in source and binary forms, with or without

corelib/include/rtabmap/core/OctoMap.h

@@ -0,0 +1,96 @@
+/*
+Copyright (c) 2010-2016, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
+All rights reserved.
+
+Redistribution and use in source and binary forms, with or without
+modification, are permitted provided that the following conditions are met:
+    * Redistributions of source code must retain the above copyright
+      notice, this list of conditions and the following disclaimer.
+    * Redistributions in binary form must reproduce the above copyright
+      notice, this list of conditions and the following disclaimer in the
+      documentation and/or other materials provided with the distribution.
+    * Neither the name of the Universite de Sherbrooke nor the
+      names of its contributors may be used to endorse or promote products
+      derived from this software without specific prior written permission.
+
+THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
+ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY
+DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+*/
+
+#ifndef SRC_OCTOMAP_H_
+#define SRC_OCTOMAP_H_
+
+#include "rtabmap/core/RtabmapExp.h" // DLL export/import defines
+
+#include <octomap/ColorOcTree.h>
+#include <octomap/OcTreeKey.h>
+
+#include <pcl/pcl_base.h>
+#include <pcl/point_types.h>
+
+#include <rtabmap/core/Transform.h>
+
+#include <map>
+#include <string>
+
+namespace rtabmap {
+
+class OcTreeNodeInfo
+{
+public:
+	OcTreeNodeInfo(int nodeRefId, const octomap::OcTreeKey & key, bool isObstacle) :
+		nodeRefId_(nodeRefId),
+		key_(key),
+		isObstacle_(isObstacle) {}
+	int nodeRefId_;
+	octomap::OcTreeKey key_;
+	bool isObstacle_;
+};
+
+class RTABMAP_EXP OctoMap {
+public:
+	OctoMap(float voxelSize = 0.1f);
+
+	const std::map<int, Transform> & addedNodes() const {return addedNodes_;}
+	void addToCache(int nodeId,
+			pcl::PointCloud<pcl::PointXYZRGB>::Ptr & ground,
+			pcl::PointCloud<pcl::PointXYZRGB>::Ptr & obstacles);
+	void update(const std::map<int, Transform> & poses);
+
+	const octomap::ColorOcTree * octree() const {return octree_;}
+
+	pcl::PointCloud<pcl::PointXYZRGB>::Ptr createCloud(
+			unsigned int treeDepth = 0,
+			std::vector<int> * obstacleIndices = 0,
+			std::vector<int> * emptyIndices = 0) const;
+
+	cv::Mat createProjectionMap(
+			float & xMin,
+			float & yMin,
+			float & gridCellSize,
+			float minGridSize);
+
+	bool writeBinary(const std::string & path);
+
+	virtual ~OctoMap();
+	void clear();
+
+private:
+	std::map<int, std::pair<pcl::PointCloud<pcl::PointXYZRGB>::Ptr, pcl::PointCloud<pcl::PointXYZRGB>::Ptr> > cache_;
+	octomap::ColorOcTree * octree_;
+	std::map<octomap::ColorOcTreeNode*, OcTreeNodeInfo> occupiedCells_;
+	std::map<int, Transform> addedNodes_;
+	octomap::KeyRay keyRay_;
+};
+
+} /* namespace rtabmap */
+
+#endif /* SRC_OCTOMAP_H_ */

corelib/include/rtabmap/core/Odometry.h

@@ -1,5 +1,5 @@
 /*
-Copyright (c) 2010-2014, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
+Copyright (c) 2010-2016, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
 All rights reserved.
 
 Redistribution and use in source and binary forms, with or without
@@ -55,12 +55,14 @@ public:
 public:
 	virtual ~Odometry();
 	Transform process(SensorData & data, OdometryInfo * info = 0);
+	Transform process(SensorData & data, const Transform & guess, OdometryInfo * info = 0);
 	virtual void reset(const Transform & initialPose = Transform::getIdentity());
 
 	//getters
 	const Transform & getPose() const {return _pose;}
 	bool isInfoDataFilled() const {return _fillInfoData;}
 	const Transform & previousVelocityTransform() const {return previousVelocityTransform_;}
+	double previousStamp() const {return previousStamp_;}
 
 private:
 	virtual Transform computeTransform(SensorData & data, const Transform & guess = Transform(), OdometryInfo * info = 0) = 0;
@@ -84,6 +86,7 @@ private:
 	float _kalmanProcessNoise;
 	float _kalmanMeasurementNoise;
 	int _imageDecimation;
+	bool _alignWithGround;
 	Transform _pose;
 	int _resetCurrentCount;
 	double previousStamp_;

corelib/include/rtabmap/core/OdometryEvent.h

@@ -1,5 +1,5 @@
 /*
-Copyright (c) 2010-2014, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
+Copyright (c) 2010-2016, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
 All rights reserved.
 
 Redistribution and use in source and binary forms, with or without

corelib/include/rtabmap/core/OdometryF2F.h

@@ -1,5 +1,5 @@
 /*
-Copyright (c) 2010-2014, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
+Copyright (c) 2010-2016, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
 All rights reserved.
 
 Redistribution and use in source and binary forms, with or without

corelib/include/rtabmap/core/OdometryF2M.h

@@ -1,5 +1,5 @@
 /*
-Copyright (c) 2010-2014, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
+Copyright (c) 2010-2016, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
 All rights reserved.
 
 Redistribution and use in source and binary forms, with or without
@@ -31,6 +31,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #include <rtabmap/core/Odometry.h>
 #include <pcl/point_cloud.h>
 #include <pcl/point_types.h>
+#include <pcl/pcl_base.h>
 
 namespace rtabmap {
 
@@ -57,13 +58,13 @@ private:
 	int maxNewFeatures_;
 	float scanKeyFrameThr_;
 	int scanMaximumMapSize_;
-	float scanSubstractRadius_;
+	float scanSubtractRadius_;
 	std::string fixedMapPath_;
 
 	Registration * regPipeline_;
 	Signature * map_;
 	Signature * lastFrame_;
-	std::map<int, pcl::PointCloud<pcl::PointNormal>::Ptr > scansBuffer_;
+	std::vector<std::pair<pcl::PointCloud<pcl::PointNormal>::Ptr, pcl::IndicesPtr> > scansBuffer_;
 };
 
 }

corelib/include/rtabmap/core/OdometryInfo.h

@@ -1,5 +1,5 @@
 /*
-Copyright (c) 2010-2014, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
+Copyright (c) 2010-2016, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
 All rights reserved.
 
 Redistribution and use in source and binary forms, with or without

corelib/include/rtabmap/core/OdometryMono.h

@@ -1,5 +1,5 @@
 /*
-Copyright (c) 2010-2014, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
+Copyright (c) 2010-2016, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
 All rights reserved.
 
 Redistribution and use in source and binary forms, with or without

corelib/include/rtabmap/core/OdometryThread.h

@@ -1,5 +1,5 @@
 /*
-Copyright (c) 2010-2014, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
+Copyright (c) 2010-2016, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
 All rights reserved.
 
 Redistribution and use in source and binary forms, with or without

corelib/include/rtabmap/core/Optimizer.h

@@ -1,5 +1,5 @@
 /*
-Copyright (c) 2010-2014, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
+Copyright (c) 2010-2016, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
 All rights reserved.
 
 Redistribution and use in source and binary forms, with or without

corelib/include/rtabmap/core/OptimizerCVSBA.h

@@ -1,5 +1,5 @@
 /*
-Copyright (c) 2010-2014, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
+Copyright (c) 2010-2016, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
 All rights reserved.
 
 Redistribution and use in source and binary forms, with or without

corelib/include/rtabmap/core/OptimizerG2O.h

@@ -1,5 +1,5 @@
 /*
-Copyright (c) 2010-2014, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
+Copyright (c) 2010-2016, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
 All rights reserved.
 
 Redistribution and use in source and binary forms, with or without

corelib/include/rtabmap/core/OptimizerGTSAM.h

@@ -1,5 +1,5 @@
 /*
-Copyright (c) 2010-2014, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
+Copyright (c) 2010-2016, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
 All rights reserved.
 
 Redistribution and use in source and binary forms, with or without

corelib/include/rtabmap/core/OptimizerTORO.h

@@ -1,5 +1,5 @@
 /*
-Copyright (c) 2010-2014, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
+Copyright (c) 2010-2016, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
 All rights reserved.
 
 Redistribution and use in source and binary forms, with or without

corelib/include/rtabmap/core/ParamEvent.h

@@ -1,5 +1,5 @@
 /*
-Copyright (c) 2010-2014, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
+Copyright (c) 2010-2016, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
 All rights reserved.
 
 Redistribution and use in source and binary forms, with or without

corelib/include/rtabmap/core/Parameters.h

@@ -1,5 +1,5 @@
 /*
-Copyright (c) 2010-2014, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
+Copyright (c) 2010-2016, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
 All rights reserved.
 
 Redistribution and use in source and binary forms, with or without
@@ -172,9 +172,9 @@ class RTABMAP_EXP Parameters
 	RTABMAP_PARAM(Rtabmap, PublishLastSignature, 	     bool, true, "Publishing last signature.");
 	RTABMAP_PARAM(Rtabmap, PublishPdf, 	                 bool, true, "Publishing pdf.");
 	RTABMAP_PARAM(Rtabmap, PublishLikelihood, 	         bool, true, "Publishing likelihood.");
-	RTABMAP_PARAM(Rtabmap, TimeThr, 		             float, 0.0, "Maximum time allowed for the detector (ms) (0 means infinity).");
+	RTABMAP_PARAM(Rtabmap, TimeThr, 		             float, 0,   "Maximum time allowed for the detector (ms) (0 means infinity).");
 	RTABMAP_PARAM(Rtabmap, MemoryThr, 		             int, 0, 	 "Maximum signatures in the Working Memory (ms) (0 means infinity).");
-	RTABMAP_PARAM(Rtabmap, DetectionRate,                float, 1.0, "Detection rate. RTAB-Map will filter input images to satisfy this rate.");
+	RTABMAP_PARAM(Rtabmap, DetectionRate,                float, 1,   "Detection rate. RTAB-Map will filter input images to satisfy this rate.");
 	RTABMAP_PARAM(Rtabmap, ImageBufferSize,              unsigned int, 1, "Data buffer size (0 min inf).");
 	RTABMAP_PARAM(Rtabmap, CreateIntermediateNodes,      bool, false, "Create intermediate nodes between loop closure detection. Only used when Rtabmap/DetectionRate>0.");
 	RTABMAP_PARAM_STR(Rtabmap, WorkingDirectory,         "", "Working directory.");
@@ -186,7 +186,7 @@ class RTABMAP_EXP Parameters
 
 	// Hypotheses selection
 	RTABMAP_PARAM(Rtabmap, LoopThr,    	     float, 0.11, 	"Loop closing threshold.");
-	RTABMAP_PARAM(Rtabmap, LoopRatio,    	 float, 0.0, 	"The loop closure hypothesis must be over LoopRatio x lastHypothesisValue.");
+	RTABMAP_PARAM(Rtabmap, LoopRatio,    	 float, 0, 	    "The loop closure hypothesis must be over LoopRatio x lastHypothesisValue.");
 
 	// Memory
 	RTABMAP_PARAM(Mem, RehearsalSimilarity,     float, 0.6, 	"Rehearsal similarity.");
@@ -194,7 +194,7 @@ class RTABMAP_EXP Parameters
 	RTABMAP_PARAM(Mem, BinDataKept, 		    bool, true, 	"Keep binary data in db.");
 	RTABMAP_PARAM(Mem, RawDescriptorsKept, 		bool, true, 	"Raw descriptors kept in memory.");
 	RTABMAP_PARAM(Mem, MapLabelsAdded, 		    bool, true, 	"Create map labels. The first node of a map will be labelled as \"map#\" where # is the map ID.");
-	RTABMAP_PARAM(Mem, SaveDepth16Format, 		bool, true, 	"Save depth image into 16 bits format to reduce memory used. Warning: values over ~65 meters are ignored (maximum 65535 millimeters).");
+	RTABMAP_PARAM(Mem, SaveDepth16Format, 		bool, false, 	"Save depth image into 16 bits format to reduce memory used. Warning: values over ~65 meters are ignored (maximum 65535 millimeters).");
 	RTABMAP_PARAM(Mem, NotLinkedNodesKept, 	    bool, true, 	"Keep not linked nodes in db (rehearsed nodes and deleted nodes).");
 	RTABMAP_PARAM(Mem, STMSize, 		        unsigned int, 10, "Short-term memory size.");
 	RTABMAP_PARAM(Mem, IncrementalMemory,       bool, true, 	"SLAM mode, otherwise it is Localization mode.");
@@ -215,8 +215,8 @@ class RTABMAP_EXP Parameters
 	RTABMAP_PARAM(Kp, NNStrategy,            int, 1,            "kNNFlannNaive=0, kNNFlannKdTree=1, kNNFlannLSH=2, kNNBruteForce=3, kNNBruteForceGPU=4");
 	RTABMAP_PARAM(Kp, IncrementalDictionary, bool, true, 		"");
 	RTABMAP_PARAM(Kp, IncrementalFlann,      bool, true, 		"When using FLANN based strategy, add/remove points to its index without always rebuilding the index (the index is built only when the dictionary doubles in size).");
-	RTABMAP_PARAM(Kp, MaxDepth,              float, 0.0, 		"Filter extracted keypoints by depth (0=inf).");
+	RTABMAP_PARAM(Kp, MaxDepth,              float, 0,   		"Filter extracted keypoints by depth (0=inf).");
-	RTABMAP_PARAM(Kp, MinDepth,              float, 0.0, 		"Filter extracted keypoints by depth.");
+	RTABMAP_PARAM(Kp, MinDepth,              float, 0,  		"Filter extracted keypoints by depth.");
 	RTABMAP_PARAM(Kp, MaxFeatures,           int, 400, 			"Maximum features extracted from the images (0 means not bounded, <0 means no extraction).");
 	RTABMAP_PARAM(Kp, BadSignRatio,          float, 0.5, 		"Bad signature ratio (less than Ratio x AverageWordsPerImage = bad).");
 	RTABMAP_PARAM(Kp, NndrRatio, 	         float, 0.8,		"NNDR ratio (A matching pair is detected, if its distance is closer than X times the distance of the second nearest neighbor.)");
@@ -243,7 +243,7 @@ class RTABMAP_EXP Parameters
 
 	// Keypoints descriptors/detectors
 	RTABMAP_PARAM(SURF, Extended, 		  bool, false, 	    "Extended descriptor flag (true - use extended 128-element descriptors; false - use 64-element descriptors).");
-	RTABMAP_PARAM(SURF, HessianThreshold, float, 500.0,		"Threshold for hessian keypoint detector used in SURF.");
+	RTABMAP_PARAM(SURF, HessianThreshold, float, 500,		"Threshold for hessian keypoint detector used in SURF.");
 	RTABMAP_PARAM(SURF, Octaves, 		  int, 4, 			"Number of pyramid octaves the keypoint detector will use.");
 	RTABMAP_PARAM(SURF, OctaveLayers, 	  int, 2, 			"Number of octave layers within each octave.");
 	RTABMAP_PARAM(SURF, Upright, 	      bool, false, 	"Up-right or rotated features flag (true - do not compute orientation of features; false - compute orientation).");
@@ -253,7 +253,7 @@ class RTABMAP_EXP Parameters
 	RTABMAP_PARAM(SIFT, NFeatures,          int, 0,			"The number of best features to retain. The features are ranked by their scores (measured in SIFT algorithm as the local contrast).");
 	RTABMAP_PARAM(SIFT, NOctaveLayers,      int, 3, 		"The number of layers in each octave. 3 is the value used in D. Lowe paper. The number of octaves is computed automatically from the image resolution.");
 	RTABMAP_PARAM(SIFT, ContrastThreshold, 	double, 0.04, 	"The contrast threshold used to filter out weak features in semi-uniform (low-contrast) regions. The larger the threshold, the less features are produced by the detector.");
-	RTABMAP_PARAM(SIFT, EdgeThreshold,      double, 10.0, 	"The threshold used to filter out edge-like features. Note that the its meaning is different from the contrastThreshold, i.e. the larger the edgeThreshold, the less features are filtered out (more features are retained).");
+	RTABMAP_PARAM(SIFT, EdgeThreshold,      double, 10, 	"The threshold used to filter out edge-like features. Note that the its meaning is different from the contrastThreshold, i.e. the larger the edgeThreshold, the less features are filtered out (more features are retained).");
 	RTABMAP_PARAM(SIFT, Sigma,              double, 1.6, 	"The sigma of the Gaussian applied to the input image at the octave #0. If your image is captured with a weak camera with soft lenses, you might want to reduce the number.");
 
 	RTABMAP_PARAM(BRIEF, Bytes,         	   int, 32, 	"Bytes is a length of descriptor in bytes. It can be equal 16, 32 or 64 bytes.");
@@ -284,12 +284,12 @@ class RTABMAP_EXP Parameters
 
 	RTABMAP_PARAM(FREAK, OrientationNormalized, bool, true,   "Enable orientation normalization.");
 	RTABMAP_PARAM(FREAK, ScaleNormalized,       bool, true,   "Enable scale normalization.");
-	RTABMAP_PARAM(FREAK, PatternScale,          float, 22.0,  "Scaling of the description pattern.");
+	RTABMAP_PARAM(FREAK, PatternScale,          float, 22,    "Scaling of the description pattern.");
 	RTABMAP_PARAM(FREAK, NOctaves,              int, 4,        "Number of octaves covered by the detected keypoints.");
 
 	RTABMAP_PARAM(BRISK, Thresh,                int, 30,      "FAST/AGAST detection threshold score.");
 	RTABMAP_PARAM(BRISK, Octaves,               int, 3,       "Detection octaves. Use 0 to do single scale.");
-	RTABMAP_PARAM(BRISK, PatternScale,          float, 1.0,  "Apply this scale to the pattern used for sampling the neighbourhood of a keypoint.");
+	RTABMAP_PARAM(BRISK, PatternScale,          float, 1,    "Apply this scale to the pattern used for sampling the neighbourhood of a keypoint.");
 
 	// BayesFilter
 	RTABMAP_PARAM(Bayes, VirtualPlacePriorThr,           float, 0.9, "Virtual place prior");
@@ -298,7 +298,7 @@ class RTABMAP_EXP Parameters
 
 	// Verify hypotheses
 	RTABMAP_PARAM(VhEp, MatchCountMin, int, 8, 		"Minimum of matching visual words pairs to accept the loop hypothesis.");
-	RTABMAP_PARAM(VhEp, RansacParam1,  float, 3.0, 	"Fundamental matrix (see cvFindFundamentalMat()): Max distance (in pixels) from the epipolar line for a point to be inlier.");
+	RTABMAP_PARAM(VhEp, RansacParam1,  float, 3, 	"Fundamental matrix (see cvFindFundamentalMat()): Max distance (in pixels) from the epipolar line for a point to be inlier.");
 	RTABMAP_PARAM(VhEp, RansacParam2,  float, 0.99, "Fundamental matrix (see cvFindFundamentalMat()): Performance of the RANSAC.");
 
 	// RGB-D SLAM
@@ -307,11 +307,11 @@ class RTABMAP_EXP Parameters
 	RTABMAP_PARAM(RGBD, AngularUpdate,            float, 0.1,  "Minimum angular displacement to update the map. Rehearsal is done prior to this, so weights are still updated.");
 	RTABMAP_PARAM(RGBD, NewMapOdomChangeDistance, float, 0,    "A new map is created if a change of odometry translation greater than X m is detected (0 m = disabled).");
 	RTABMAP_PARAM(RGBD, OptimizeFromGraphEnd,     bool, false, "Optimize graph from the newest node. If false, the graph is optimized from the oldest node of the current graph (this adds an overhead computation to detect to oldest mode of the current graph, but it can be useful to preserve the map referential from the oldest node). Warning when set to false: when some nodes are transferred, the first referential of the local map may change, resulting in momentary changes in robot/map position (which are annoying in teleoperation).");
-	RTABMAP_PARAM(RGBD, OptimizeMaxError,         float, 1.0,  "Reject loop closures if optimization error is greater than this value (0=disabled). This will help to detect when a wrong loop closure is added to the graph. Not compatible with \"Optimizer/Robust\" if enabled.");
+	RTABMAP_PARAM(RGBD, OptimizeMaxError,         float, 1,    "Reject loop closures if optimization error is greater than this value (0=disabled). This will help to detect when a wrong loop closure is added to the graph. Not compatible with \"Optimizer/Robust\" if enabled.");
 	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.0,  "Linear velocity (m/sec) used to compute path weights.");
+	RTABMAP_PARAM(RGBD, PlanLinearVelocity,       float, 0,    "Linear velocity (m/sec) used to compute path weights.");
-	RTABMAP_PARAM(RGBD, PlanAngularVelocity,      float, 0.0,  "Angular velocity (rad/sec) used to compute path weights.");
+	RTABMAP_PARAM(RGBD, PlanAngularVelocity,      float, 0,    "Angular velocity (rad/sec) used to compute path weights.");
 	RTABMAP_PARAM(RGBD, GoalsSavedInUserData,     bool, false, "When a goal is received and processed with success, it is saved in user data of the location with this format: \"GOAL:#\".");
 	RTABMAP_PARAM(RGBD, MaxLocalRetrieved,        unsigned int, 2, "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).");
 	RTABMAP_PARAM(RGBD, LocalRadius,              float, 10,   "Local radius (m) for nodes selection in the local map. This parameter is used in some approaches about the local map management.");
@@ -326,7 +326,7 @@ class RTABMAP_EXP Parameters
 	RTABMAP_PARAM(RGBD, ProximityMaxGraphDepth,       int, 50,      "Maximum depth from the current/last loop closure location and the local loop closure hypotheses. Set 0 to ignore.");
 	RTABMAP_PARAM(RGBD, ProximityPathFilteringRadius, float, 0.5,   "Path filtering radius.");
 	RTABMAP_PARAM(RGBD, ProximityPathRawPosesUsed,    bool, true,   "When comparing to a local path, merge the scan using the odometry poses (with neighbor link optimizations) instead of the ones in the optimized local graph.");
-	RTABMAP_PARAM(RGBD, ProximityAngle,               float, 45.0,  "Maximum angle (degrees) for visual proximity detection.");
+	RTABMAP_PARAM(RGBD, ProximityAngle,               float, 45,    "Maximum angle (degrees) for visual proximity detection.");
 
 	// Graph optimization
 #ifdef RTABMAP_GTSAM
@@ -346,7 +346,7 @@ class RTABMAP_EXP Parameters
 
 	RTABMAP_PARAM(g2o, Solver,                  int, 0,          "0=csparse 1=pcg 2=cholmod");
 	RTABMAP_PARAM(g2o, Optimizer,               int, 0,          "0=Levenberg 1=GaussNewton");
-	RTABMAP_PARAM(g2o, PixelVariance,           double, 1.0,     "Pixel variance used for SBA.");
+	RTABMAP_PARAM(g2o, PixelVariance,           double, 1,       "Pixel variance used for SBA.");
 
 	// Odometry
 	RTABMAP_PARAM(Odom, Strategy,           	int, 0, 		"0=Frame-to-Map (F2M) 1=Frame-to-Frame (F2F)");
@@ -366,12 +366,13 @@ class RTABMAP_EXP Parameters
 	RTABMAP_PARAM(Odom, KeyFrameThr,            float, 0.3,       "[Visual] Create a new keyframe when the number of inliers drops under this ratio of features in last frame. Setting the value to 0 means that a keyframe is created for each processed frame.");
 	RTABMAP_PARAM(Odom, ScanKeyFrameThr,        float, 0.7,       "[Geometry] Create a new keyframe when the number of ICP inliers drops under this ratio of points in last frame's scan. Setting the value to 0 means that a keyframe is created for each processed frame.");
 	RTABMAP_PARAM(Odom, ImageDecimation,        int, 1,           "Decimation of the images before registration.");
+	RTABMAP_PARAM(Odom, AlignWithGround,        bool, false,      "Align odometry with the ground on initialization.");
 
 	// Odometry Bag-of-words
 	RTABMAP_PARAM(OdomF2M, MaxSize,             int, 2000,    "[Visual] Local map size: If > 0 (example 5000), the odometry will maintain a local map of X maximum words.");
 	RTABMAP_PARAM(OdomF2M, MaxNewFeatures,      int, 0,       "[Visual] Maximum features (sorted by keypoint response) added to local map from a new key-frame. 0 means no limit.");
 	RTABMAP_PARAM(OdomF2M, ScanMaxSize,         int, 2000,    "[Geometry] Maximum local scan map size.");
-	RTABMAP_PARAM(OdomF2M, ScanSubstractRadius, float, 0.05,  "[Geometry] Radius used to filter points of a new added scan to local map. This could match the voxel size of the scans.");
+	RTABMAP_PARAM(OdomF2M, ScanSubtractRadius,  float, 0.05,  "[Geometry] Radius used to filter points of a new added scan to local map. This could match the voxel size of the scans.");
 	RTABMAP_PARAM_STR(OdomF2M, FixedMapPath,    "",           "Path to a fixed map (RTAB-Map's database) to be used for odometry. Odometry will be constraint to this map. RGB-only images can be used if odometry PnP estimation is used.")
 
 	// Odometry Mono
@@ -390,7 +391,7 @@ class RTABMAP_EXP Parameters
 	RTABMAP_PARAM(Vis, ForwardEstOnly,           bool, true, 	"Forward estimation only (A->B). If false, a transformation is also computed in backward direction (B->A), then the two resulting transforms are merged (middle interpolation between the transforms).");
 	RTABMAP_PARAM(Vis, InlierDistance,           float, 0.1,    "[Vis/EstimationType = 0] Maximum distance for feature correspondences. Used by 3D->3D estimation approach.");
 	RTABMAP_PARAM(Vis, RefineIterations,         int, 5,        "[Vis/EstimationType = 0] Number of iterations used to refine the transformation found by RANSAC. 0 means that the transformation is not refined.");
-	RTABMAP_PARAM(Vis, PnPReprojError, 	         float, 2.0,    "[Vis/EstimationType = 1] PnP reprojection error.");
+	RTABMAP_PARAM(Vis, PnPReprojError, 	         float, 2,      "[Vis/EstimationType = 1] PnP reprojection error.");
 	RTABMAP_PARAM(Vis, PnPFlags,                 int, 1,        "[Vis/EstimationType = 1] PnP flags: 0=Iterative, 1=EPNP, 2=P3P");
 	RTABMAP_PARAM(Vis, PnPRefineIterations,      int, 1,        "[Vis/EstimationType = 1] Refine iterations.");
 	RTABMAP_PARAM(Vis, EpipolarGeometryVar,      float, 0.02,   "[Vis/EstimationType = 2] Epipolar geometry maximum variance to accept the transformation.");
@@ -408,8 +409,8 @@ class RTABMAP_EXP Parameters
 #endif
 	
 	RTABMAP_PARAM(Vis, MaxFeatures,              int, 1000,     "0 no limits.");
-	RTABMAP_PARAM(Vis, MaxDepth, 	             float, 0.0,    "Max depth of the features (0 means no limit).");
+	RTABMAP_PARAM(Vis, MaxDepth, 	             float, 0,      "Max depth of the features (0 means no limit).");
-	RTABMAP_PARAM(Vis, MinDepth, 	             float, 0.0,    "Min depth of the features (0 means no limit).");
+	RTABMAP_PARAM(Vis, MinDepth, 	             float, 0,      "Min depth of the features (0 means no limit).");
 	RTABMAP_PARAM_STR(Vis, RoiRatios,        "0.0 0.0 0.0 0.0", "Region of interest ratios [left, right, top, bottom].");
     RTABMAP_PARAM(Vis, SubPixWinSize,            int, 3,        "See cv::cornerSubPix().");
 	RTABMAP_PARAM(Vis, SubPixIterations,         int, 0,        "See cv::cornerSubPix(). 0 disables sub pixel refining.");
@@ -430,7 +431,7 @@ class RTABMAP_EXP Parameters
 	RTABMAP_PARAM(Icp, DownsamplingStep,          int, 1, 	    "Downsampling step size (1=no sampling). This is done before uniform sampling.");
 	RTABMAP_PARAM(Icp, MaxCorrespondenceDistance, float, 0.05,  "Max distance for point correspondences.");
 	RTABMAP_PARAM(Icp, Iterations,                int, 30, 		"Max iterations.");
-	RTABMAP_PARAM(Icp, Epsilon,                   float, 0.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, 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.2,   "Ratio of matching correspondences to accept the transform.");
 	RTABMAP_PARAM(Icp, PointToPlane,              bool, false, 	"Use point to plane ICP.");
 	RTABMAP_PARAM(Icp, PointToPlaneNormalNeighbors, int, 20,    "Number of neighbors to compute normals for point to plane.");
@@ -441,7 +442,7 @@ class RTABMAP_EXP Parameters
 	RTABMAP_PARAM(Stereo, Iterations,            int, 30,       "Maximum iterations.");
 	RTABMAP_PARAM(Stereo, MaxLevel,              int, 3,        "Maximum pyramid level.");
 	RTABMAP_PARAM(Stereo, MinDisparity,          int, 1,        "Minimum disparity.");
-	RTABMAP_PARAM(Stereo, MaxDisparity,          int, 64,       "Maximum disparity.");
+	RTABMAP_PARAM(Stereo, MaxDisparity,          int, 128,      "Maximum disparity.");
 	RTABMAP_PARAM(Stereo, OpticalFlow,           bool, true,    "Use optical flow to find stereo correspondences, otherwise a simple block matching approach is used.");
 	RTABMAP_PARAM(Stereo, SSD,                   bool, true,    "[Stereo/OpticalFlow = false] Use Sum of Squared Differences (SSD) window, otherwise Sum of Absolute Differences (SAD) window is used.");
 	RTABMAP_PARAM(Stereo, Eps,                   double, 0.01,  "[Stereo/OpticalFlow = true] Epsilon stop criterion.");
@@ -494,6 +495,9 @@ public:
 	static std::string getVersion();
 	static std::string getDefaultDatabaseName();
 	
+	static std::string serialize(const ParametersMap & parameters);
+	static ParametersMap deserialize(const std::string & parameters);
+
 	static bool isFeatureParameter(const std::string & param);
 	static ParametersMap getDefaultOdometryParameters(bool stereo = false);
 	static ParametersMap getDefaultParameters(const std::string & group);

corelib/include/rtabmap/core/ParticleFilter.h

@@ -1,5 +1,5 @@
 /*
-Copyright (c) 2010-2015, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
+Copyright (c) 2010-2016, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
 All rights reserved.
 
 Redistribution and use in source and binary forms, with or without

corelib/include/rtabmap/core/Registration.h

@@ -1,5 +1,5 @@
 /*
-Copyright (c) 2010-2014, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
+Copyright (c) 2010-2016, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
 All rights reserved.
 
 Redistribution and use in source and binary forms, with or without

corelib/include/rtabmap/core/RegistrationIcp.h

@@ -1,5 +1,5 @@
 /*
-Copyright (c) 2010-2014, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
+Copyright (c) 2010-2016, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
 All rights reserved.
 
 Redistribution and use in source and binary forms, with or without

corelib/include/rtabmap/core/RegistrationInfo.h

@@ -1,9 +1,29 @@
 /*
- * RegistrationInfo.h
+Copyright (c) 2010-2016, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
- *
+All rights reserved.
- *  Created on: Jan 5, 2016
+
- *      Author: mathieu
+Redistribution and use in source and binary forms, with or without
- */
+modification, are permitted provided that the following conditions are met:
+    * Redistributions of source code must retain the above copyright
+      notice, this list of conditions and the following disclaimer.
+    * Redistributions in binary form must reproduce the above copyright
+      notice, this list of conditions and the following disclaimer in the
+      documentation and/or other materials provided with the distribution.
+    * Neither the name of the Universite de Sherbrooke nor the
+      names of its contributors may be used to endorse or promote products
+      derived from this software without specific prior written permission.
+
+THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
+ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY
+DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+*/
 
 #ifndef REGISTRATIONINFO_H_
 #define REGISTRATIONINFO_H_
@@ -18,7 +38,9 @@ public:
 		variance(0),
 		inliers(0),
 		matches(0),
-		icpInliersRatio(0)
+		icpInliersRatio(0),
+		icpTranslation(0.0f),
+		icpRotation(0.0f)
 	{
 	}
 
@@ -33,6 +55,8 @@ public:
 
 	// RegistrationIcp
 	float icpInliersRatio;
+	float icpTranslation;
+	float icpRotation;
 };
 
 }

corelib/include/rtabmap/core/RegistrationVis.h

@@ -1,5 +1,5 @@
 /*
-Copyright (c) 2010-2014, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
+Copyright (c) 2010-2016, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
 All rights reserved.
 
 Redistribution and use in source and binary forms, with or without

corelib/include/rtabmap/core/Rtabmap.h

@@ -1,5 +1,5 @@
 /*
-Copyright (c) 2010-2014, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
+Copyright (c) 2010-2016, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
 All rights reserved.
 
 Redistribution and use in source and binary forms, with or without
@@ -70,6 +70,7 @@ public:
 	void close(bool databaseSaved = true);
 
 	const std::string & getWorkingDir() const {return _wDir;}
+	bool isRGBDMode() const { return _rgbdSlamMode; }
 	int getLoopClosureId() const {return _loopClosureHypothesis.first;}
 	float getLoopClosureValue() const {return _loopClosureHypothesis.second;}
 	int getHighestHypothesisId() const {return _highestHypothesis.first;}
@@ -245,6 +246,7 @@ private:
 	unsigned int _pathGoalIndex;
 	Transform _pathTransformToGoal;
 	int _pathStuckCount;
+	float _pathStuckDistance;
 
 };
 

corelib/include/rtabmap/core/RtabmapEvent.h

@@ -1,5 +1,5 @@
 /*
-Copyright (c) 2010-2014, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
+Copyright (c) 2010-2016, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
 All rights reserved.
 
 Redistribution and use in source and binary forms, with or without

corelib/include/rtabmap/core/RtabmapExp.h

@@ -1,5 +1,5 @@
 /*
-Copyright (c) 2010-2014, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
+Copyright (c) 2010-2016, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
 All rights reserved.
 
 Redistribution and use in source and binary forms, with or without

corelib/include/rtabmap/core/RtabmapThread.h

@@ -1,5 +1,5 @@
 /*
-Copyright (c) 2010-2014, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
+Copyright (c) 2010-2016, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
 All rights reserved.
 
 Redistribution and use in source and binary forms, with or without

corelib/include/rtabmap/core/SensorData.h

@@ -1,5 +1,5 @@
 /*
-Copyright (c) 2010-2014, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
+Copyright (c) 2010-2016, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
 All rights reserved.
 
 Redistribution and use in source and binary forms, with or without
@@ -194,6 +194,8 @@ public:
 	void setGroundTruth(const Transform & pose) {groundTruth_ = pose;}
 	const Transform & groundTruth() const {return groundTruth_;}
 
+	long getMemoryUsed() const; // Return memory usage in Bytes
+
 private:
 	int _id;
 	double _stamp;

corelib/include/rtabmap/core/Signature.h

@@ -1,5 +1,5 @@
 /*
-Copyright (c) 2010-2014, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
+Copyright (c) 2010-2016, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
 All rights reserved.
 
 Redistribution and use in source and binary forms, with or without

corelib/include/rtabmap/core/Statistics.h

@@ -1,5 +1,5 @@
 /*
-Copyright (c) 2010-2014, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
+Copyright (c) 2010-2016, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
 All rights reserved.
 
 Redistribution and use in source and binary forms, with or without
@@ -145,6 +145,7 @@ public:
 	void setRefImageId(int refImageId) {_refImageId = refImageId;}
 	void setLoopClosureId(int loopClosureId) {_loopClosureId = loopClosureId;}
 	void setProximityDetectionId(int id) {_proximiyDetectionId = id;}
+	void setStamp(double stamp) {_stamp = stamp;}
 
 	void setSignatures(const std::map<int, Signature> & signatures) {_signatures = signatures;}
 
@@ -165,6 +166,7 @@ public:
 	int refImageId() const {return _refImageId;}
 	int loopClosureId() const {return _loopClosureId;}
 	int proximityDetectionId() const {return _proximiyDetectionId;}
+	double stamp() const {return _stamp;}
 
 	const std::map<int, Signature> & getSignatures() const {return _signatures;}
 
@@ -188,6 +190,7 @@ private:
 	int _refImageId;
 	int _loopClosureId;
 	int _proximiyDetectionId;
+	double _stamp;
 
 	std::map<int, Signature> _signatures;
 

corelib/include/rtabmap/core/Stereo.h

@@ -1,5 +1,5 @@
 /*
-Copyright (c) 2010-2014, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
+Copyright (c) 2010-2016, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
 All rights reserved.
 
 Redistribution and use in source and binary forms, with or without

corelib/include/rtabmap/core/StereoCameraModel.h

@@ -1,5 +1,5 @@
 /*
-Copyright (c) 2010-2014, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
+Copyright (c) 2010-2016, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
 All rights reserved.
 
 Redistribution and use in source and binary forms, with or without

corelib/include/rtabmap/core/StereoDense.h

@@ -1,5 +1,5 @@
 /*
-Copyright (c) 2010-2014, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
+Copyright (c) 2010-2016, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
 All rights reserved.
 
 Redistribution and use in source and binary forms, with or without

corelib/include/rtabmap/core/Transform.h

@@ -1,5 +1,5 @@
 /*
-Copyright (c) 2010-2014, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
+Copyright (c) 2010-2016, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
 All rights reserved.
 
 Redistribution and use in source and binary forms, with or without

corelib/include/rtabmap/core/UserDataEvent.h

@@ -1,5 +1,5 @@
 /*
-Copyright (c) 2010-2014, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
+Copyright (c) 2010-2016, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
 All rights reserved.
 
 Redistribution and use in source and binary forms, with or without

corelib/include/rtabmap/core/VWDictionary.h

@@ -1,5 +1,5 @@
 /*
-Copyright (c) 2010-2014, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
+Copyright (c) 2010-2016, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
 All rights reserved.
 
 Redistribution and use in source and binary forms, with or without

corelib/include/rtabmap/core/VisualWord.h

@@ -1,5 +1,5 @@
 /*
-Copyright (c) 2010-2014, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
+Copyright (c) 2010-2016, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
 All rights reserved.
 
 Redistribution and use in source and binary forms, with or without

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

@@ -1,9 +1,29 @@
 /*
- * util3d_mapping.hpp
+Copyright (c) 2010-2016, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
- *
+All rights reserved.
- *  Created on: 2015-05-13
+
- *      Author: mathieu
+Redistribution and use in source and binary forms, with or without
- */
+modification, are permitted provided that the following conditions are met:
+    * Redistributions of source code must retain the above copyright
+      notice, this list of conditions and the following disclaimer.
+    * Redistributions in binary form must reproduce the above copyright
+      notice, this list of conditions and the following disclaimer in the
+      documentation and/or other materials provided with the distribution.
+    * Neither the name of the Universite de Sherbrooke nor the
+      names of its contributors may be used to endorse or promote products
+      derived from this software without specific prior written permission.
+
+THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
+ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY
+DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+*/
 
 #ifndef UTIL3D_MAPPING_HPP_
 #define UTIL3D_MAPPING_HPP_
@@ -17,6 +37,19 @@
 namespace rtabmap{
 namespace util3d{
 
+template<typename PointT>
+typename pcl::PointCloud<PointT>::Ptr projectCloudOnXYPlane(
+		const typename pcl::PointCloud<PointT> & cloud)
+{
+	typename pcl::PointCloud<PointT>::Ptr output(new pcl::PointCloud<PointT>);
+	*output = cloud;
+	for(unsigned int i=0; i<output->size(); ++i)
+	{
+		output->at(i).z = 0;
+	}
+	return output;
+}
+
 template<typename PointT>
 void segmentObstaclesFromGround(
 		const typename pcl::PointCloud<PointT>::Ptr & cloud,
@@ -29,7 +62,8 @@ void segmentObstaclesFromGround(
 		int minClusterSize,
 		bool segmentFlatObstacles,
 		float maxGroundHeight,
-		pcl::IndicesPtr * flatObstacles)
+		pcl::IndicesPtr * flatObstacles,
+		const Eigen::Vector4f & viewPoint)
 {
 	ground.reset(new std::vector<int>);
 	obstacles.reset(new std::vector<int>);
@@ -47,7 +81,7 @@ void segmentObstaclesFromGround(
 				groundNormalAngle,
 				Eigen::Vector4f(0,0,1,0),
 				normalKSearch,
-				Eigen::Vector4f(0,0,100,0));
+				viewPoint);
 
 		if(segmentFlatObstacles)
 		{
@@ -116,14 +150,17 @@ void segmentObstaclesFromGround(
 			}
 
 			//Cluster remaining stuff (obstacles)
-			std::vector<pcl::IndicesPtr> clusteredObstaclesSurfaces = util3d::extractClusters(
+			if(otherStuffIndices->size())
-					cloud,
+			{
-					otherStuffIndices,
+				std::vector<pcl::IndicesPtr> clusteredObstaclesSurfaces = util3d::extractClusters(
-					clusterRadius,
+						cloud,
-					minClusterSize);
+						otherStuffIndices,
+						clusterRadius,
+						minClusterSize);
 
-			// merge indices
+				// merge indices
-			obstacles = util3d::concatenate(clusteredObstaclesSurfaces);
+				obstacles = util3d::concatenate(clusteredObstaclesSurfaces);
+			}
 		}
 	}
 }
@@ -139,7 +176,8 @@ void segmentObstaclesFromGround(
 		int minClusterSize,
 		bool segmentFlatObstacles,
 		float maxGroundHeight,
-		pcl::IndicesPtr * flatObstacles)
+		pcl::IndicesPtr * flatObstacles,
+		const Eigen::Vector4f & viewPoint)
 {
 	pcl::IndicesPtr indices(new std::vector<int>);
 	segmentObstaclesFromGround<PointT>(
@@ -153,7 +191,81 @@ void segmentObstaclesFromGround(
 			minClusterSize,
 			segmentFlatObstacles,
 			maxGroundHeight,
-			flatObstacles);
+			flatObstacles,
+			viewPoint);
+}
+
+template<typename PointT>
+void occupancy2DFromGroundObstacles(
+		const typename pcl::PointCloud<PointT>::Ptr & cloud,
+		const pcl::IndicesPtr & groundIndices,
+		const pcl::IndicesPtr & obstaclesIndices,
+		cv::Mat & ground,
+		cv::Mat & obstacles,
+		float cellSize)
+{
+	typename pcl::PointCloud<PointT>::Ptr groundCloud(new pcl::PointCloud<PointT>);
+	typename pcl::PointCloud<PointT>::Ptr obstaclesCloud(new pcl::PointCloud<PointT>);
+
+	if(groundIndices->size())
+	{
+		pcl::copyPointCloud(*cloud, *groundIndices, *groundCloud);
+	}
+
+	if(obstaclesIndices->size())
+	{
+		pcl::copyPointCloud(*cloud, *obstaclesIndices, *obstaclesCloud);
+	}
+
+	occupancy2DFromGroundObstacles<PointT>(
+			groundCloud,
+			obstaclesCloud,
+			ground,
+			obstacles,
+			cellSize);
+}
+
+template<typename PointT>
+void occupancy2DFromGroundObstacles(
+		const typename pcl::PointCloud<PointT>::Ptr & groundCloud,
+		const typename pcl::PointCloud<PointT>::Ptr & obstaclesCloud,
+		cv::Mat & ground,
+		cv::Mat & obstacles,
+		float cellSize)
+{
+	ground = cv::Mat();
+	if(groundCloud->size())
+	{
+		//project on XY plane
+		typename pcl::PointCloud<PointT>::Ptr groundCloudProjected;
+		groundCloudProjected = util3d::projectCloudOnXYPlane(*groundCloud);
+		//voxelize to grid cell size
+		groundCloudProjected = util3d::voxelize(groundCloudProjected, cellSize);
+
+		ground = cv::Mat((int)groundCloudProjected->size(), 1, CV_32FC2);
+		for(unsigned int i=0;i<groundCloudProjected->size(); ++i)
+		{
+			ground.at<cv::Vec2f>(i)[0] = groundCloudProjected->at(i).x;
+			ground.at<cv::Vec2f>(i)[1] = groundCloudProjected->at(i).y;
+		}
+	}
+
+	obstacles = cv::Mat();
+	if(obstaclesCloud->size())
+	{
+		//project on XY plane
+		typename pcl::PointCloud<PointT>::Ptr obstaclesCloudProjected;
+		obstaclesCloudProjected = util3d::projectCloudOnXYPlane(*obstaclesCloud);
+		//voxelize to grid cell size
+		obstaclesCloudProjected = util3d::voxelize(obstaclesCloudProjected, cellSize);
+
+		obstacles = cv::Mat((int)obstaclesCloudProjected->size(), 1, CV_32FC2);
+		for(unsigned int i=0;i<obstaclesCloudProjected->size(); ++i)
+		{
+			obstacles.at<cv::Vec2f>(i)[0] = obstaclesCloudProjected->at(i).x;
+			obstacles.at<cv::Vec2f>(i)[1] = obstaclesCloudProjected->at(i).y;
+		}
+	}
 }
 
 template<typename PointT>
@@ -186,48 +298,13 @@ void occupancy2DFromCloud3D(
 			segmentFlatObstacles,
 			maxGroundHeight);
 
-	pcl::PointCloud<pcl::PointXYZ>::Ptr groundCloud(new pcl::PointCloud<pcl::PointXYZ>);
+	occupancy2DFromGroundObstacles<PointT>(
-	pcl::PointCloud<pcl::PointXYZ>::Ptr obstaclesCloud(new pcl::PointCloud<pcl::PointXYZ>);
+			cloud,
-
+			groundIndices,
-	if(groundIndices->size())
+			obstaclesIndices,
-	{
+			ground,
-		pcl::copyPointCloud(*cloud, *groundIndices, *groundCloud);
+			obstacles,
-		//project on XY plane
+			cellSize);
-		util3d::projectCloudOnXYPlane(groundCloud);
-		//voxelize to grid cell size
-		groundCloud = util3d::voxelize(groundCloud, cellSize);
-	}
-
-	if(obstaclesIndices->size())
-	{
-		pcl::copyPointCloud(*cloud, *obstaclesIndices, *obstaclesCloud);
-		//project on XY plane
-		util3d::projectCloudOnXYPlane(obstaclesCloud);
-		//voxelize to grid cell size
-		obstaclesCloud = util3d::voxelize(obstaclesCloud, cellSize);
-	}
-
-	ground = cv::Mat();
-	if(groundCloud->size())
-	{
-		ground = cv::Mat((int)groundCloud->size(), 1, CV_32FC2);
-		for(unsigned int i=0;i<groundCloud->size(); ++i)
-		{
-			ground.at<cv::Vec2f>(i)[0] = groundCloud->at(i).x;
-			ground.at<cv::Vec2f>(i)[1] = groundCloud->at(i).y;
-		}
-	}
-
-	obstacles = cv::Mat();
-	if(obstaclesCloud->size())
-	{
-		obstacles = cv::Mat((int)obstaclesCloud->size(), 1, CV_32FC2);
-		for(unsigned int i=0;i<obstaclesCloud->size(); ++i)
-		{
-			obstacles.at<cv::Vec2f>(i)[0] = obstaclesCloud->at(i).x;
-			obstacles.at<cv::Vec2f>(i)[1] = obstaclesCloud->at(i).y;
-		}
-	}
 }
 
 template<typename PointT>

corelib/include/rtabmap/core/util2d.h

@@ -1,5 +1,5 @@
 /*
-Copyright (c) 2010-2014, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
+Copyright (c) 2010-2016, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
 All rights reserved.
 
 Redistribution and use in source and binary forms, with or without

corelib/include/rtabmap/core/util3d.h

@@ -1,5 +1,5 @@
 /*
-Copyright (c) 2010-2014, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
+Copyright (c) 2010-2016, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
 All rights reserved.
 
 Redistribution and use in source and binary forms, with or without
@@ -74,16 +74,23 @@ pcl::PointXYZ RTABMAP_EXP projectDepthTo3D(
 		bool smoothing,
 		float maxZError = 0.02f);
 
-pcl::PointCloud<pcl::PointXYZ>::Ptr RTABMAP_EXP cloudFromDepth(
+RTABMAP_DEPRECATED (pcl::PointCloud<pcl::PointXYZ>::Ptr RTABMAP_EXP cloudFromDepth(
 		const cv::Mat & imageDepth,
 		float cx, float cy,
 		float fx, float fy,
 		int decimation = 1,
 		float maxDepth = 0.0f,
 		float minDepth = 0.0f,
+		std::vector<int> * validIndices = 0), "Use cloudFromDepth with CameraModel interface.");
+pcl::PointCloud<pcl::PointXYZ>::Ptr RTABMAP_EXP cloudFromDepth(
+		const cv::Mat & imageDepth,
+		const CameraModel & model,
+		int decimation = 1,
+		float maxDepth = 0.0f,
+		float minDepth = 0.0f,
 		std::vector<int> * validIndices = 0);
 
-pcl::PointCloud<pcl::PointXYZRGB>::Ptr RTABMAP_EXP cloudFromDepthRGB(
+RTABMAP_DEPRECATED (pcl::PointCloud<pcl::PointXYZRGB>::Ptr RTABMAP_EXP cloudFromDepthRGB(
 		const cv::Mat & imageRgb,
 		const cv::Mat & imageDepth,
 		float cx, float cy,
@@ -91,6 +98,14 @@ pcl::PointCloud<pcl::PointXYZRGB>::Ptr RTABMAP_EXP cloudFromDepthRGB(
 		int decimation = 1,
 		float maxDepth = 0.0f,
 		float minDepth = 0.0f,
+		std::vector<int> * validIndices = 0), "Use cloudFromDepthRGB with CameraModel interface.");
+pcl::PointCloud<pcl::PointXYZRGB>::Ptr RTABMAP_EXP cloudFromDepthRGB(
+		const cv::Mat & imageRgb,
+		const cv::Mat & imageDepth,
+		const CameraModel & model,
+		int decimation = 1,
+		float maxDepth = 0.0f,
+		float minDepth = 0.0f,
 		std::vector<int> * validIndices = 0);
 
 pcl::PointCloud<pcl::PointXYZ>::Ptr RTABMAP_EXP cloudFromDisparity(

corelib/include/rtabmap/core/util3d_correspondences.h

@@ -1,5 +1,5 @@
 /*
-Copyright (c) 2010-2014, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
+Copyright (c) 2010-2016, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
 All rights reserved.
 
 Redistribution and use in source and binary forms, with or without

corelib/include/rtabmap/core/util3d_features.h

@@ -1,5 +1,5 @@
 /*
-Copyright (c) 2010-2014, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
+Copyright (c) 2010-2016, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
 All rights reserved.
 
 Redistribution and use in source and binary forms, with or without

corelib/include/rtabmap/core/util3d_filtering.h

@@ -1,5 +1,5 @@
 /*
-Copyright (c) 2010-2014, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
+Copyright (c) 2010-2016, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
 All rights reserved.
 
 Redistribution and use in source and binary forms, with or without
@@ -34,6 +34,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #include <pcl/point_cloud.h>
 #include <pcl/point_types.h>
 #include <pcl/pcl_base.h>
+#include <pcl/ModelCoefficients.h>
 
 namespace rtabmap
 {
@@ -485,6 +486,18 @@ pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr RTABMAP_EXP extractIndices(
 		bool negative,
 		bool keepOrganized);
 
+pcl::IndicesPtr extractPlane(
+		const pcl::PointCloud<pcl::PointXYZ>::Ptr & cloud,
+		float distanceThreshold,
+		int maxIterations = 100,
+		pcl::ModelCoefficients * coefficientsOut = 0);
+pcl::IndicesPtr extractPlane(
+		const pcl::PointCloud<pcl::PointXYZ>::Ptr & cloud,
+		const pcl::IndicesPtr & indices,
+		float distanceThreshold,
+		int maxIterations = 100,
+		pcl::ModelCoefficients * coefficientsOut = 0);
+
 } // namespace util3d
 } // namespace rtabmap
 

corelib/include/rtabmap/core/util3d_mapping.h

@@ -1,5 +1,5 @@
 /*
-Copyright (c) 2010-2014, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
+Copyright (c) 2010-2016, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
 All rights reserved.
 
 Redistribution and use in source and binary forms, with or without
@@ -76,8 +76,9 @@ void RTABMAP_EXP rayTrace(const cv::Point2i & start,
 
 cv::Mat RTABMAP_EXP convertMap2Image8U(const cv::Mat & map8S);
 
-void RTABMAP_EXP projectCloudOnXYPlane(
+template<typename PointT>
-		pcl::PointCloud<pcl::PointXYZ>::Ptr & cloud);
+typename pcl::PointCloud<PointT>::Ptr projectCloudOnXYPlane(
+		const typename pcl::PointCloud<PointT> & cloud);
 
 // templated methods
 template<typename PointT>
@@ -92,7 +93,8 @@ void segmentObstaclesFromGround(
 		int minClusterSize,
 		bool segmentFlatObstacles = false,
 		float maxGroundHeight = 0.0f,
-		pcl::IndicesPtr * flatObstacles = 0);
+		pcl::IndicesPtr * flatObstacles = 0,
+		const Eigen::Vector4f & viewPoint = Eigen::Vector4f(0,0,100,0));
 template<typename PointT>
 void segmentObstaclesFromGround(
 		const typename pcl::PointCloud<PointT>::Ptr & cloud,
@@ -104,7 +106,25 @@ void segmentObstaclesFromGround(
 		int minClusterSize,
 		bool segmentFlatObstacles = false,
 		float maxGroundHeight = 0.0f,
-		pcl::IndicesPtr * flatObstacles = 0);
+		pcl::IndicesPtr * flatObstacles = 0,
+		const Eigen::Vector4f & viewPoint = Eigen::Vector4f(0,0,100,0));
+
+template<typename PointT>
+void occupancy2DFromGroundObstacles(
+		const typename pcl::PointCloud<PointT>::Ptr & cloud,
+		const pcl::IndicesPtr & groundIndices,
+		const pcl::IndicesPtr & obstaclesIndices,
+		cv::Mat & ground,
+		cv::Mat & obstacles,
+		float cellSize);
+
+template<typename PointT>
+void occupancy2DFromGroundObstacles(
+		const typename pcl::PointCloud<PointT>::Ptr & groundCloud,
+		const typename pcl::PointCloud<PointT>::Ptr & obstaclesCloud,
+		cv::Mat & ground,
+		cv::Mat & obstacles,
+		float cellSize);
 
 template<typename PointT>
 void occupancy2DFromCloud3D(

corelib/include/rtabmap/core/util3d_motion_estimation.h

@@ -1,5 +1,5 @@
 /*
-Copyright (c) 2010-2014, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
+Copyright (c) 2010-2016, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
 All rights reserved.
 
 Redistribution and use in source and binary forms, with or without

corelib/include/rtabmap/core/util3d_registration.h

@@ -1,5 +1,5 @@
 /*
-Copyright (c) 2010-2014, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
+Copyright (c) 2010-2016, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
 All rights reserved.
 
 Redistribution and use in source and binary forms, with or without
@@ -92,18 +92,6 @@ Transform RTABMAP_EXP icpPointToPlane(
 		float epsilon = 0.0f,
 		bool icp2D = false);
 
-pcl::PointCloud<pcl::PointXYZ>::Ptr RTABMAP_EXP getICPReadyCloud(
-		const cv::Mat & depth,
-		float fx,
-		float fy,
-		float cx,
-		float cy,
-		int decimation,
-		double maxDepth,
-		float voxel,
-		int samples,
-		const Transform & transform = Transform::getIdentity());
-
 } // namespace util3d
 } // namespace rtabmap
 

corelib/include/rtabmap/core/util3d_surface.h

@@ -1,5 +1,5 @@
 /*
-Copyright (c) 2010-2014, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
+Copyright (c) 2010-2016, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
 All rights reserved.
 
 Redistribution and use in source and binary forms, with or without
@@ -121,32 +121,39 @@ pcl::TextureMesh::Ptr RTABMAP_EXP createTextureMesh(
 		const std::map<int, Transform> & poses,
 		const std::map<int, CameraModel> & cameraModels,
 		const std::map<int, cv::Mat> & images,
-		const std::string & tmpDirectory = ".");
+		const std::string & tmpDirectory = ".",
+		int kNormalSearch = 20); // if mesh doesn't have normals, compute them with k neighbors
 
-pcl::PointCloud<pcl::PointNormal>::Ptr RTABMAP_EXP computeNormals(
+pcl::PointCloud<pcl::Normal>::Ptr RTABMAP_EXP computeNormals(
 		const pcl::PointCloud<pcl::PointXYZ>::Ptr & cloud,
-		int normalKSearch = 20);
+		int normalKSearch = 20,
-pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr RTABMAP_EXP computeNormals(
+		const Eigen::Vector3f & viewPoint = Eigen::Vector3f(0,0,0));
+pcl::PointCloud<pcl::Normal>::Ptr RTABMAP_EXP computeNormals(
 		const pcl::PointCloud<pcl::PointXYZRGB>::Ptr & cloud,
-		int normalKSearch = 20);
+		int normalKSearch = 20,
-pcl::PointCloud<pcl::PointNormal>::Ptr RTABMAP_EXP computeNormals(
+		const Eigen::Vector3f & viewPoint = Eigen::Vector3f(0,0,0));
+pcl::PointCloud<pcl::Normal>::Ptr RTABMAP_EXP computeNormals(
 		const pcl::PointCloud<pcl::PointXYZ>::Ptr & cloud,
 		const pcl::IndicesPtr & indices,
-		int normalKSearch = 20);
+		int normalKSearch = 20,
-pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr RTABMAP_EXP computeNormals(
+		const Eigen::Vector3f & viewPoint = Eigen::Vector3f(0,0,0));
+pcl::PointCloud<pcl::Normal>::Ptr RTABMAP_EXP computeNormals(
 		const pcl::PointCloud<pcl::PointXYZRGB>::Ptr & cloud,
 		const pcl::IndicesPtr & indices,
-		int normalKSearch = 20);
+		int normalKSearch = 20,
+		const Eigen::Vector3f & viewPoint = Eigen::Vector3f(0,0,0));
 
-pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr computeFastOrganizedNormals(
+pcl::PointCloud<pcl::Normal>::Ptr computeFastOrganizedNormals(
 		const pcl::PointCloud<pcl::PointXYZRGB>::Ptr & cloud,
 		float maxDepthChangeFactor = 0.02f,
-		float normalSmoothingSize = 10.0f);
+		float normalSmoothingSize = 10.0f,
-pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr computeFastOrganizedNormals(
+		const Eigen::Vector3f & viewPoint = Eigen::Vector3f(0,0,0));
+pcl::PointCloud<pcl::Normal>::Ptr computeFastOrganizedNormals(
 		const pcl::PointCloud<pcl::PointXYZRGB>::Ptr & cloud,
 		const pcl::IndicesPtr & indices,
 		float maxDepthChangeFactor = 0.02f,
-		float normalSmoothingSize = 10.0f);
+		float normalSmoothingSize = 10.0f,
+		const Eigen::Vector3f & viewPoint = Eigen::Vector3f(0,0,0));
 
 pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr RTABMAP_EXP mls(
 		const pcl::PointCloud<pcl::PointXYZRGB>::Ptr & cloud,

corelib/include/rtabmap/core/util3d_transforms.h

@@ -1,5 +1,5 @@
 /*
-Copyright (c) 2010-2014, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
+Copyright (c) 2010-2016, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
 All rights reserved.
 
 Redistribution and use in source and binary forms, with or without

corelib/src/BayesFilter.cpp

@@ -1,5 +1,5 @@
 /*
-Copyright (c) 2010-2014, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
+Copyright (c) 2010-2016, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
 All rights reserved.
 
 Redistribution and use in source and binary forms, with or without

corelib/src/CMakeLists.txt

@@ -188,10 +188,17 @@ IF(G2O_FOUND)
 ENDIF(G2O_FOUND)
 
 IF(GTSAM_FOUND)
-	SET(INCLUDE_DIRS 
+	IF(GTSAM_INCLUDE_DIR)
-		${INCLUDE_DIRS} 
+		SET(INCLUDE_DIRS 
-		${GTSAM_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()
 	SET(LIBRARIES
 		${LIBRARIES}
 		gtsam
@@ -230,6 +237,21 @@ IF(ZED_FOUND)
 	ENDIF(CUDA_FOUND)
 ENDIF(ZED_FOUND)
 
+IF(OCTOMAP_FOUND)
+    SET(INCLUDE_DIRS
+		${INCLUDE_DIRS}
+		${OCTOMAP_INCLUDE_DIRS}
+	)
+	SET(LIBRARIES
+		${LIBRARIES}
+		${OCTOMAP_LIBRARIES}
+	)
+	SET(SRC_FILES
+    	${SRC_FILES}
+		OctoMap.cpp
+	)
+ENDIF(OCTOMAP_FOUND)
+
 ####################################
 # Generate resources files
 ####################################

corelib/src/Camera.cpp

@@ -1,5 +1,5 @@
 /*
-Copyright (c) 2010-2014, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
+Copyright (c) 2010-2016, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
 All rights reserved.
 
 Redistribution and use in source and binary forms, with or without
@@ -88,7 +88,7 @@ SensorData Camera::takeImage(CameraInfo * info)
 	}
 
 	UTimer timer;
-	SensorData data  = this->captureImage();
+	SensorData data  = this->captureImage(info);
 	double captureTime = timer.ticks();
 	if(warnFrameRateTooHigh)
 	{
@@ -102,6 +102,7 @@ SensorData Camera::takeImage(CameraInfo * info)
 	if(info)
 	{
 		info->id = data.id();
+		info->stamp = data.stamp();
 		info->timeCapture = captureTime;
 	}
 	return data;

corelib/src/CameraModel.cpp

@@ -1,5 +1,5 @@
 /*
-Copyright (c) 2010-2014, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
+Copyright (c) 2010-2016, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
 All rights reserved.
 
 Redistribution and use in source and binary forms, with or without
@@ -371,6 +371,8 @@ CameraModel CameraModel::scaled(double scale) const
 			P.at<double>(1,1) *= scale;
 			P.at<double>(0,2) *= scale;
 			P.at<double>(1,2) *= scale;
+			P.at<double>(0,3) *= scale;
+			P.at<double>(1,3) *= scale;
 		}
 		scaledModel = CameraModel(name_, cv::Size(double(imageSize_.width)*scale, double(imageSize_.height)*scale), K, D_, R_, P, localTransform_);
 	}

corelib/src/CameraRGB.cpp

@@ -1,5 +1,5 @@
 /*
-Copyright (c) 2010-2014, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
+Copyright (c) 2010-2016, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
 All rights reserved.
 
 Redistribution and use in source and binary forms, with or without
@@ -42,6 +42,8 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #include <rtabmap/core/util3d_filtering.h>
 #include <rtabmap/core/util3d_surface.h>
 
+#include <pcl/common/io.h>
+
 #include <iostream>
 #include <fstream>
 #include <cmath>
@@ -436,7 +438,7 @@ std::vector<std::string> CameraImages::filenames() const
 	return std::vector<std::string>();
 }
 
-SensorData CameraImages::captureImage()
+SensorData CameraImages::captureImage(CameraInfo * info)
 {
 	if(syncImageRateWithStamps_ && _captureDelay>0.0)
 	{
@@ -661,7 +663,9 @@ SensorData CameraImages::captureImage()
 			}
 			if(_scanNormalsK > 0 && cloud->size())
 			{
-				pcl::PointCloud<pcl::PointNormal>::Ptr cloudNormals = util3d::computeNormals(cloud, _scanNormalsK);
+				pcl::PointCloud<pcl::Normal>::Ptr normals = util3d::computeNormals(cloud, _scanNormalsK);
+				pcl::PointCloud<pcl::PointNormal>::Ptr cloudNormals(new pcl::PointCloud<pcl::PointNormal>);
+				pcl::concatenateFields(*cloud, *normals, *cloudNormals);
 				scan = util3d::laserScanFromPointCloud(*cloudNormals);
 			}
 			else
@@ -692,10 +696,11 @@ SensorData CameraImages::captureImage()
 /////////////////////////
 CameraVideo::CameraVideo(
 		int usbDevice,
+		bool rectifyImages,
 		float imageRate,
 		const Transform & localTransform) :
 	Camera(imageRate, localTransform),
-	_rectifyImages(false),
+	_rectifyImages(rectifyImages),
 	_src(kUsbDevice),
 	_usbDevice(usbDevice)
 {
@@ -796,7 +801,7 @@ std::string CameraVideo::getSerial() const
 	return _guid;
 }
 
-SensorData CameraVideo::captureImage()
+SensorData CameraVideo::captureImage(CameraInfo * info)
 {
 	cv::Mat img;
 	if(_capture.isOpened())
@@ -808,7 +813,7 @@ SensorData CameraVideo::captureImage()
 				_model.setImageSize(img.size());
 			}
 
-			if(_model.isValidForRectification() && (_src != kVideoFile || _rectifyImages))
+			if(_model.isValidForRectification() && _rectifyImages)
 			{
 				img = _model.rectifyImage(img);
 			}

corelib/src/CameraRGBD.cpp

@@ -1,5 +1,5 @@
 /*
-Copyright (c) 2010-2014, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
+Copyright (c) 2010-2016, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
 All rights reserved.
 
 Redistribution and use in source and binary forms, with or without
@@ -203,7 +203,7 @@ std::string CameraOpenni::getSerial() const
 	return "";
 }
 
-SensorData CameraOpenni::captureImage()
+SensorData CameraOpenni::captureImage(CameraInfo * info)
 {
 	SensorData data;
 #ifdef HAVE_OPENNI
@@ -317,7 +317,7 @@ bool CameraOpenNICV::isCalibrated() const
 	return true;
 }
 
-SensorData CameraOpenNICV::captureImage()
+SensorData CameraOpenNICV::captureImage(CameraInfo * info)
 {
 	SensorData data;
 	if(_capture.isOpened())
@@ -671,7 +671,7 @@ std::string CameraOpenNI2::getSerial() const
 	return "";
 }
 
-SensorData CameraOpenNI2::captureImage()
+SensorData CameraOpenNI2::captureImage(CameraInfo * info)
 {
 	SensorData data;
 #ifdef RTABMAP_OPENNI2
@@ -1033,7 +1033,7 @@ std::string CameraFreenect::getSerial() const
 	return "";
 }
 
-SensorData CameraFreenect::captureImage()
+SensorData CameraFreenect::captureImage(CameraInfo * info)
 {
 	SensorData data;
 #ifdef RTABMAP_FREENECT
@@ -1272,9 +1272,10 @@ bool CameraFreenect2::init(const std::string & calibrationFolder, const std::str
 				const CameraModel & l = stereoModel_.left();
 				const CameraModel & r = stereoModel_.right();
 				stereoModel_ = StereoCameraModel(stereoModel_.name(),
-						depthSize, l.K(), l.D(), l.R(), depthP,
+						depthSize, l.K_raw(), l.D_raw(), l.R(), depthP,
-						colorSize, r.K(), r.D(), r.R(), colorP,
+						colorSize, r.K_raw(), r.D_raw(), r.R(), colorP,
 						stereoModel_.R(), stereoModel_.T(), stereoModel_.E(), stereoModel_.F());
+				stereoModel_.initRectificationMap();
 			}
 		}
 
@@ -1307,7 +1308,7 @@ std::string CameraFreenect2::getSerial() const
 	return "";
 }
 
-SensorData CameraFreenect2::captureImage()
+SensorData CameraFreenect2::captureImage(CameraInfo * info)
 {
 	SensorData data;
 #ifdef RTABMAP_FREENECT2
@@ -1720,12 +1721,12 @@ std::string CameraRGBDImages::getSerial() const
 	return this->cameraModel().name();
 }
 
-SensorData CameraRGBDImages::captureImage()
+SensorData CameraRGBDImages::captureImage(CameraInfo * info)
 {
 	SensorData data;
 
 	SensorData rgb, depth;
-	rgb = CameraImages::captureImage();
+	rgb = CameraImages::captureImage(info);
 	if(!rgb.imageRaw().empty())
 	{
 		depth = cameraDepth_.takeImage();

corelib/src/CameraStereo.cpp

@@ -1,5 +1,5 @@
 /*
-Copyright (c) 2010-2014, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
+Copyright (c) 2010-2016, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
 All rights reserved.
 
 Redistribution and use in source and binary forms, with or without
@@ -415,7 +415,7 @@ std::string CameraStereoDC1394::getSerial() const
 	return "";
 }
 
-SensorData CameraStereoDC1394::captureImage()
+SensorData CameraStereoDC1394::captureImage(CameraInfo * info)
 {
 	SensorData data;
 #ifdef RTABMAP_DC1394
@@ -610,7 +610,7 @@ struct ImageContainer
 } ;
 #endif
 
-SensorData CameraStereoFlyCapture2::captureImage()
+SensorData CameraStereoFlyCapture2::captureImage(CameraInfo * info)
 {
 	SensorData data;
 #ifdef RTABMAP_FLYCAPTURE2
@@ -753,6 +753,7 @@ CameraStereoZed::CameraStereoZed(
 		int quality,
 		int sensingMode,
 		int confidenceThr,
+		bool computeOdometry,
 		float imageRate,
 		const Transform & localTransform) :
 	Camera(imageRate, localTransform),
@@ -763,12 +764,14 @@ CameraStereoZed::CameraStereoZed(
 	resolution_(resolution),
 	quality_(quality),
 	sensingMode_(sensingMode),
-	confidenceThr_(confidenceThr)
+	confidenceThr_(confidenceThr),
+	computeOdometry_(computeOdometry),
+	lost_(true)
 {
 #ifdef RTABMAP_ZED
-	UASSERT(resolution_ >= sl::zed::HD2K && resolution_ <=sl::zed::VGA);
+	UASSERT(resolution_ >= sl::zed::HD2K && resolution_ <sl::zed::LAST_RESOLUTION);
-	UASSERT(quality_ >= sl::zed::NONE && quality_ <=sl::zed::QUALITY);
+	UASSERT(quality_ >= sl::zed::NONE && quality_ <sl::zed::LAST_MODE);
-	UASSERT(sensingMode_ >= sl::zed::FULL && sensingMode_ <=sl::zed::RAW);
+	UASSERT(sensingMode_ >= sl::zed::FILL && sensingMode_ <sl::zed::LAST_SENSING_MODE);
 	UASSERT(confidenceThr_ >= 0 && confidenceThr_ <=100);
 #endif
 }
@@ -778,6 +781,7 @@ CameraStereoZed::CameraStereoZed(
 		int quality,
 		int sensingMode,
 		int confidenceThr,
+		bool computeOdometry,
 		float imageRate,
 		const Transform & localTransform) :
 	Camera(imageRate, localTransform),
@@ -788,12 +792,14 @@ CameraStereoZed::CameraStereoZed(
 	resolution_(2),
 	quality_(quality),
 	sensingMode_(sensingMode),
-	confidenceThr_(confidenceThr)
+	confidenceThr_(confidenceThr),
+	computeOdometry_(computeOdometry),
+	lost_(true)
 {
 #ifdef RTABMAP_ZED
-	UASSERT(resolution_ >= sl::zed::HD2K && resolution_ <=sl::zed::VGA);
+	UASSERT(resolution_ >= sl::zed::HD2K && resolution_ <sl::zed::LAST_RESOLUTION);
-	UASSERT(quality_ >= sl::zed::NONE && quality_ <=sl::zed::QUALITY);
+	UASSERT(quality_ >= sl::zed::NONE && quality_ <sl::zed::LAST_MODE);
-	UASSERT(sensingMode_ >= sl::zed::FULL && sensingMode_ <=sl::zed::RAW);
+	UASSERT(sensingMode_ >= sl::zed::FILL && sensingMode_ <sl::zed::LAST_SENSING_MODE);
 	UASSERT(confidenceThr_ >= 0 && confidenceThr_ <=100);
 #endif
 }
@@ -817,7 +823,7 @@ bool CameraStereoZed::init(const std::string & calibrationFolder, const std::str
 		zed_ = 0;
 	}
 	
-
+	lost_ = true;
 	if(src_ == CameraVideo::kVideoFile)
 	{
 		zed_ = new sl::zed::Camera(svoFilePath_); // Use in SVO playback mode
@@ -835,11 +841,17 @@ bool CameraStereoZed::init(const std::string & calibrationFolder, const std::str
 		}
 	}
 
-	//init WITH self-calibration (- last parameter to false -)
+	//init WITH self-calibration
-	sl::zed::ERRCODE err = zed_->init(
+	sl::zed::InitParams parameters(
-			(sl::zed::MODE)quality_,
+		(sl::zed::MODE)quality_, //MODE
-			-1, // search for any GPU
+		sl::zed::METER,  //UNIT
-			true, false, false);
+		sl::zed::IMAGE,  //COORDINATE_SYSTEM
+		false,  // verbose
+		-1,     //device
+		-1.,    //minDist
+		false,  //disableSelfCalib
+		false); //vflip
+	sl::zed::ERRCODE err = zed_->init(parameters);
 
 	// Quit if an error occurred
 	if (err != sl::zed::SUCCESS)
@@ -852,6 +864,13 @@ bool CameraStereoZed::init(const std::string & calibrationFolder, const std::str
 
 	zed_->setConfidenceThreshold(confidenceThr_);
 
+	if (computeOdometry_)
+	{
+		Eigen::Matrix4f initPose;
+		initPose.setIdentity(4, 4);
+		zed_->enableTracking(initPose, false);
+	}
+
 	sl::zed::StereoParameters * stereoParams = zed_->getParameters();
 	sl::zed::resolution res = zed_->getImageSize();
 				
@@ -860,7 +879,7 @@ bool CameraStereoZed::init(const std::string & calibrationFolder, const std::str
 		stereoParams->LeftCam.fy, 
 		stereoParams->LeftCam.cx, 
 		stereoParams->LeftCam.cy, 
-		stereoParams->baseline/1000.0f,
+		stereoParams->baseline,
 		this->getLocalTransform(),
 		cv::Size(res.width, res.height));
 
@@ -887,7 +906,7 @@ std::string CameraStereoZed::getSerial() const
 	return "";
 }
 
-SensorData CameraStereoZed::captureImage()
+SensorData CameraStereoZed::captureImage(CameraInfo * info)
 {
 	SensorData data;
 #ifdef RTABMAP_ZED
@@ -913,7 +932,6 @@ SensorData CameraStereoZed::captureImage()
 				// get depth image
 				cv::Mat depth;
 				slMat2cvMat(zed_->retrieveMeasure(sl::zed::MEASURE::DEPTH)).copyTo(depth);
-				depth /= 1000.0; // to meters
 
 				data = SensorData(left, depth, stereoModel_.left(), this->getNextSeqID(), UTimer::now());
 			}
@@ -926,6 +944,40 @@ SensorData CameraStereoZed::captureImage()
 			
 				data = SensorData(left, right, stereoModel_, this->getNextSeqID(), UTimer::now());
 			}
+
+			if (computeOdometry_ && info)
+			{
+				Eigen::Matrix4f path;
+				int trackingConfidence = zed_->getTrackingConfidence();
+				if (trackingConfidence)
+				{
+					zed_->getPosition(path);
+					info->odomPose = Transform::fromEigen4f(path);
+					if (!info->odomPose.isNull())
+					{
+						//transform x->forward, y->left, z->up
+						Transform opticalTransform(0, 0, 1, 0, -1, 0, 0, 0, 0, -1, 0, 0);
+						info->odomPose = opticalTransform * info->odomPose * opticalTransform.inverse();
+					}
+					if (lost_)
+					{
+						info->odomCovariance = cv::Mat::eye(6, 6, CV_64FC1) * 9999.0f; // don't know transform with previous pose
+						lost_ = false;
+						UDEBUG("Init %s (var=%f)", info->odomPose.prettyPrint().c_str(), 9999.0f);
+					}
+					else
+					{
+						info->odomCovariance = cv::Mat::eye(6, 6, CV_64FC1) * 1.0f / float(trackingConfidence);
+						UDEBUG("Run %s (var=%f)", info->odomPose.prettyPrint().c_str(), 1.0f / float(trackingConfidence));
+					}
+				}
+				else
+				{
+					info->odomCovariance = cv::Mat::eye(6, 6, CV_64FC1) * 9999.0f; // lost
+					lost_ = true;
+					UWARN("ZED lost!");
+				}
+			}
 		}
 		else if(src_ == CameraVideo::kUsbDevice)
 		{
@@ -1072,21 +1124,21 @@ std::string CameraStereoImages::getSerial() const
 	return stereoModel_.name();
 }
 
-SensorData CameraStereoImages::captureImage()
+SensorData CameraStereoImages::captureImage(CameraInfo * info)
 {
 	SensorData data;
 
 	SensorData left, right;
-	left = CameraImages::captureImage();
+	left = CameraImages::captureImage(info);
 	if(!left.imageRaw().empty())
 	{
 		if(camera2_)
 		{
-			right = camera2_->takeImage();
+			right = camera2_->takeImage(info);
 		}
 		else
 		{
-			right = this->takeImage();
+			right = this->takeImage(info);
 		}
 
 		if(!right.imageRaw().empty())
@@ -1141,11 +1193,12 @@ CameraStereoVideo::CameraStereoVideo(
 
 CameraStereoVideo::CameraStereoVideo(
 	int device,
+	bool rectifyImages,
 	float imageRate,
 	const Transform & localTransform) :
 	Camera(imageRate, localTransform),
 	path_(""),
-	rectifyImages_(false),
+	rectifyImages_(rectifyImages),
 	src_(CameraVideo::kUsbDevice),
 	usbDevice_(device)
 {
@@ -1234,7 +1287,7 @@ std::string CameraStereoVideo::getSerial() const
 	return cameraName_;
 }
 
-SensorData CameraStereoVideo::captureImage()
+SensorData CameraStereoVideo::captureImage(CameraInfo * info)
 {
 	SensorData data;
 
@@ -1255,7 +1308,7 @@ SensorData CameraStereoVideo::captureImage()
 				rightCvt = true;
 			}
 
-			if((src_ != CameraVideo::kVideoFile || rectifyImages_) && stereoModel_.left().isValidForRectification() && stereoModel_.right().isValidForRectification())
+			if(rectifyImages_ && stereoModel_.left().isValidForRectification() && stereoModel_.right().isValidForRectification())
 			{
 				leftImage = stereoModel_.left().rectifyImage(leftImage);
 				rightImage = stereoModel_.right().rectifyImage(rightImage);

corelib/src/CameraThread.cpp

@@ -1,5 +1,5 @@
 /*
-Copyright (c) 2010-2014, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
+Copyright (c) 2010-2016, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
 All rights reserved.
 
 Redistribution and use in source and binary forms, with or without
@@ -177,27 +177,50 @@ void CameraThread::mainLoop()
 				pcl::IndicesPtr validIndices(new std::vector<int>);
 				pcl::PointCloud<pcl::PointXYZ>::Ptr cloud = util3d::cloudFromSensorData(data, _scanDecimation, _scanMaxDepth, _scanMinDepth, validIndices.get());
 				float maxPoints = (data.depthRaw().rows/_scanDecimation)*(data.depthRaw().cols/_scanDecimation);
-				if(_scanVoxelSize>0.0f)
-				{
-					cloud = util3d::voxelize(cloud, validIndices, _scanVoxelSize);
-					float ratio = float(cloud->size()) / float(validIndices->size());
-					maxPoints = ratio * maxPoints;
-				}
-				else if(!cloud->is_dense)
-				{
-					pcl::PointCloud<pcl::PointXYZ>::Ptr denseCloud(new pcl::PointCloud<pcl::PointXYZ>);
-					pcl::copyPointCloud(*cloud, *validIndices, *denseCloud);
-					cloud = denseCloud;
-				}
-
 				cv::Mat scan;
-				if(_scanNormalsK>0)
+				if(validIndices->size())
 				{
-					scan = util3d::laserScanFromPointCloud(*util3d::computeNormals(cloud, _scanNormalsK));
+					if(_scanVoxelSize>0.0f)
-				}
+					{
-				else
+						cloud = util3d::voxelize(cloud, validIndices, _scanVoxelSize);
-				{
+						float ratio = float(cloud->size()) / float(validIndices->size());
-					scan = util3d::laserScanFromPointCloud(*cloud);
+						maxPoints = ratio * maxPoints;
+					}
+					else if(!cloud->is_dense)
+					{
+						pcl::PointCloud<pcl::PointXYZ>::Ptr denseCloud(new pcl::PointCloud<pcl::PointXYZ>);
+						pcl::copyPointCloud(*cloud, *validIndices, *denseCloud);
+						cloud = denseCloud;
+					}
+
+					if(cloud->size())
+					{
+						if(_scanNormalsK>0)
+						{
+							// view point
+							Eigen::Vector3f viewPoint(0.0f,0.0f,0.0f);
+							if(data.cameraModels().size() && !data.cameraModels()[0].localTransform().isNull())
+							{
+								viewPoint[0] = data.cameraModels()[0].localTransform().x();
+								viewPoint[1] = data.cameraModels()[0].localTransform().y();
+								viewPoint[2] = data.cameraModels()[0].localTransform().z();
+							}
+							else if(!data.stereoCameraModel().localTransform().isNull())
+							{
+								viewPoint[0] = data.stereoCameraModel().localTransform().x();
+								viewPoint[1] = data.stereoCameraModel().localTransform().y();
+								viewPoint[2] = data.stereoCameraModel().localTransform().z();
+							}
+							pcl::PointCloud<pcl::Normal>::Ptr normals = util3d::computeNormals(cloud, _scanNormalsK, viewPoint);
+							pcl::PointCloud<pcl::PointNormal>::Ptr cloudNormals(new pcl::PointCloud<pcl::PointNormal>);
+							pcl::concatenateFields(*cloud, *normals, *cloudNormals);
+							scan = util3d::laserScanFromPointCloud(*cloudNormals);
+						}
+						else
+						{
+							scan = util3d::laserScanFromPointCloud(*cloud);
+						}
+					}
 				}
 				data.setLaserScanRaw(scan, (int)maxPoints, _scanMaxDepth);
 				info.timeScanFromDepth = timer.ticks();

corelib/src/Compression.cpp

@@ -1,5 +1,5 @@
 /*
-Copyright (c) 2010-2014, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
+Copyright (c) 2010-2016, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
 All rights reserved.
 
 Redistribution and use in source and binary forms, with or without

corelib/src/DBDriver.cpp

@@ -1,5 +1,5 @@
 /*
-Copyright (c) 2010-2014, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
+Copyright (c) 2010-2016, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
 All rights reserved.
 
 Redistribution and use in source and binary forms, with or without
@@ -188,6 +188,14 @@ int DBDriver::getTotalDictionarySize() const
 	_dbSafeAccessMutex.unlock();
 	return words;
 }
+ParametersMap DBDriver::getLastParameters() const
+{
+	ParametersMap parameters;
+	_dbSafeAccessMutex.lock();
+	parameters = getLastParametersQuery();
+	_dbSafeAccessMutex.unlock();
+	return parameters;
+}
 
 std::string DBDriver::getDatabaseVersion() const
 {
@@ -834,18 +842,38 @@ void DBDriver::getAllLabels(std::map<int, std::string> & labels) const
 	_dbSafeAccessMutex.unlock();
 }
 
-void DBDriver::addStatisticsAfterRun(int stMemSize, int lastSignAdded, int processMemUsed, int databaseMemUsed, int dictionarySize) const
+void DBDriver::addStatisticsAfterRun(
+		int stMemSize,
+		int lastSignAdded,
+		int processMemUsed,
+		int databaseMemUsed,
+		int dictionarySize,
+		const ParametersMap & parameters) const
 {
 	ULOGGER_DEBUG("");
 	if(this->isConnected())
 	{
 		std::stringstream query;
-		query << "INSERT INTO Statistics(STM_size,last_sign_added,process_mem_used,database_mem_used,dictionary_size) values("
+		if(uStrNumCmp(this->getDatabaseVersion(), "0.11.8") >= 0)
-			  << stMemSize << ","
+		{
-		      << lastSignAdded << ","
+			std::string param = Parameters::serialize(parameters);
-		      << processMemUsed << ","
+			query << "INSERT INTO Statistics(STM_size,last_sign_added,process_mem_used,database_mem_used,dictionary_size,parameters) values("
-		      << databaseMemUsed << ","
+				  << stMemSize << ","
-			  << dictionarySize << ");";
+				  << lastSignAdded << ","
+				  << processMemUsed << ","
+				  << databaseMemUsed << ","
+				  << dictionarySize << ","
+				  "\"" << param.c_str() << "\");";
+		}
+		else
+		{
+			query << "INSERT INTO Statistics(STM_size,last_sign_added,process_mem_used,database_mem_used,dictionary_size) values("
+				  << stMemSize << ","
+				  << lastSignAdded << ","
+				  << processMemUsed << ","
+				  << databaseMemUsed << ","
+				  << dictionarySize << ");";
+		}
 
 		this->executeNoResultQuery(query.str());
 	}

corelib/src/DBDriverSqlite3.cpp

@@ -1,5 +1,5 @@
 /*
-Copyright (c) 2010-2014, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
+Copyright (c) 2010-2016, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
 All rights reserved.
 
 Redistribution and use in source and binary forms, with or without
@@ -373,6 +373,15 @@ bool DBDriverSqlite3::connectDatabaseQuery(const std::string & url, bool overwri
 	UASSERT(this->getDatabaseVersionQuery(_version)); // must be true!
 	UINFO("Database version = %s", _version.c_str());
 
+	if(uStrNumCmp(_version, RTABMAP_VERSION) > 0)
+	{
+		UERROR("Opened database version (%s) is more recent than rtabmap "
+			   "installed version (%s). Please update rtabmap to new version!",
+			   _version.c_str(), RTABMAP_VERSION);
+		this->disconnectDatabaseQuery(false);
+		return false;
+	}
+
 	//Set database optimizations
 	this->setCacheSize(_cacheSize); // this will call the SQL
 	this->setJournalMode(_journalMode); // this will call the SQL
@@ -699,6 +708,42 @@ int DBDriverSqlite3::getTotalDictionarySizeQuery() const
 	return size;
 }
 
+ParametersMap DBDriverSqlite3::getLastParametersQuery() const
+{
+	UDEBUG("");
+	ParametersMap parameters;
+	if(_ppDb)
+	{
+		if(uStrNumCmp(_version, "0.11.8") >= 0)
+		{
+			std::string query = "SELECT parameters "
+					 "FROM Statistics "
+					 "WHERE time_enter >= (SELECT MAX(time_enter) FROM Statistics);";
+
+			int rc = SQLITE_OK;
+			sqlite3_stmt * ppStmt = 0;
+			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());
+			rc = sqlite3_step(ppStmt);
+			if(rc == SQLITE_ROW)
+			{
+				std::string text((const char *)sqlite3_column_text(ppStmt, 0));
+
+				if(text.size())
+				{
+					parameters = Parameters::deserialize(text);
+				}
+
+				rc = sqlite3_step(ppStmt);
+			}
+			UASSERT_MSG(rc == SQLITE_DONE, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
+			rc = sqlite3_finalize(ppStmt);
+			UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
+		}
+	}
+	return parameters;
+}
+
 void DBDriverSqlite3::loadNodeDataQuery(std::list<Signature *> & signatures) const
 {
 	UDEBUG("load data for %d signatures", (int)signatures.size());
@@ -846,13 +891,39 @@ void DBDriverSqlite3::loadNodeDataQuery(std::list<Signature *> & signatures) con
 					if(dataSize > 0 && data)
 					{
 						float * dataFloat = (float*)data;
-						if((unsigned int)dataSize % (4+localTransform.size())*sizeof(float) == 0)
+						if(uStrNumCmp(_version, "0.11.2") >= 0 &&
+						   (unsigned int)dataSize % (6+localTransform.size())*sizeof(float) == 0)
+						{
+							int cameraCount = dataSize / ((6+localTransform.size())*sizeof(float));
+							UDEBUG("Loading calibration for %d cameras (%d bytes)", cameraCount, dataSize);
+							int max = cameraCount*(6+localTransform.size());
+							for(int i=0; i<max; i+=6+localTransform.size())
+							{
+								// Reinitialize to a new Transform, to avoid copying in the same memory than the previous one
+								localTransform = Transform::getIdentity();
+								memcpy(localTransform.data(), dataFloat+i+6, localTransform.size()*sizeof(float));
+								models.push_back(CameraModel(
+										(double)dataFloat[i],
+										(double)dataFloat[i+1],
+										(double)dataFloat[i+2],
+										(double)dataFloat[i+3],
+										localTransform));
+								models.back().setImageSize(cv::Size(dataFloat[i+4], dataFloat[i+5]));
+								UDEBUG("%f %f %f %f %f %f %s", dataFloat[i], dataFloat[i+1], dataFloat[i+2],
+										dataFloat[i+3], dataFloat[i+4], dataFloat[i+5],
+										localTransform.prettyPrint().c_str());
+							}
+						}
+						else if(uStrNumCmp(_version, "0.11.2") < 0 &&
+								(unsigned int)dataSize % (4+localTransform.size())*sizeof(float) == 0)
 						{
 							int cameraCount = dataSize / ((4+localTransform.size())*sizeof(float));
 							UDEBUG("Loading calibration for %d cameras (%d bytes)", cameraCount, dataSize);
 							int max = cameraCount*(4+localTransform.size());
 							for(int i=0; i<max; i+=4+localTransform.size())
 							{
+								// Reinitialize to a new Transform, to avoid copying in the same memory than the previous one
+								localTransform = Transform::getIdentity();
 								memcpy(localTransform.data(), dataFloat+i+4, localTransform.size()*sizeof(float));
 								models.push_back(CameraModel(
 										(double)dataFloat[i],
@@ -874,26 +945,6 @@ void DBDriverSqlite3::loadNodeDataQuery(std::list<Signature *> & signatures) con
 									dataFloat[4], // baseline
 									localTransform);
 						}
-						else if((unsigned int)dataSize % (6+localTransform.size())*sizeof(float) == 0)
-						{
-							int cameraCount = dataSize / ((6+localTransform.size())*sizeof(float));
-							UDEBUG("Loading calibration for %d cameras (%d bytes)", cameraCount, dataSize);
-							int max = cameraCount*(6+localTransform.size());
-							for(int i=0; i<max; i+=6+localTransform.size())
-							{
-								memcpy(localTransform.data(), dataFloat+i+6, localTransform.size()*sizeof(float));
-								models.push_back(CameraModel(
-										(double)dataFloat[i],
-										(double)dataFloat[i+1],
-										(double)dataFloat[i+2],
-										(double)dataFloat[i+3],
-										localTransform));
-								models.back().setImageSize(cv::Size(dataFloat[i+4], dataFloat[i+5]));
-								UDEBUG("%f %f %f %f %f %f %s", dataFloat[i], dataFloat[i+1], dataFloat[i+2],
-										dataFloat[i+3], dataFloat[i+4], dataFloat[i+5],
-										localTransform.prettyPrint().c_str());
-							}
-						}
 						else
 						{
 							UFATAL("Wrong format of the Data.calibration field (size=%d bytes)", dataSize);
@@ -1067,13 +1118,39 @@ bool DBDriverSqlite3::getCalibrationQuery(
 				if(dataSize > 0 && data)
 				{
 					float * dataFloat = (float*)data;
-					if((unsigned int)dataSize % (4+localTransform.size())*sizeof(float) == 0)
+					if(uStrNumCmp(_version, "0.11.2") >= 0 &&
+					  (unsigned int)dataSize % (6+localTransform.size())*sizeof(float) == 0)
+					{
+						int cameraCount = dataSize / ((6+localTransform.size())*sizeof(float));
+						UDEBUG("Loading calibration for %d cameras (%d bytes)", cameraCount, dataSize);
+						int max = cameraCount*(6+localTransform.size());
+						for(int i=0; i<max; i+=6+localTransform.size())
+						{
+							// Reinitialize to a new Transform, to avoid copying in the same memory than the previous one
+							localTransform = Transform::getIdentity();
+							memcpy(localTransform.data(), dataFloat+i+6, localTransform.size()*sizeof(float));
+							models.push_back(CameraModel(
+									(double)dataFloat[i],
+									(double)dataFloat[i+1],
+									(double)dataFloat[i+2],
+									(double)dataFloat[i+3],
+									localTransform));
+							models.back().setImageSize(cv::Size(dataFloat[i+4], dataFloat[i+5]));
+							UDEBUG("%f %f %f %f %f %f %s", dataFloat[i], dataFloat[i+1], dataFloat[i+2],
+									dataFloat[i+3], dataFloat[i+4], dataFloat[i+5],
+									localTransform.prettyPrint().c_str());
+						}
+					}
+					else if(uStrNumCmp(_version, "0.11.2") < 0 &&
+							(unsigned int)dataSize % (4+localTransform.size())*sizeof(float) == 0)
 					{
 						int cameraCount = dataSize / ((4+localTransform.size())*sizeof(float));
 						UDEBUG("Loading calibration for %d cameras (%d bytes)", cameraCount, dataSize);
 						int max = cameraCount*(4+localTransform.size());
 						for(int i=0; i<max; i+=4+localTransform.size())
 						{
+							// Reinitialize to a new Transform, to avoid copying in the same memory than the previous one
+							localTransform = Transform::getIdentity();
 							memcpy(localTransform.data(), dataFloat+i+4, localTransform.size()*sizeof(float));
 							models.push_back(CameraModel(
 									(double)dataFloat[i],
@@ -1095,26 +1172,6 @@ bool DBDriverSqlite3::getCalibrationQuery(
 								dataFloat[4], // baseline
 								localTransform);
 					}
-					else if((unsigned int)dataSize % (6+localTransform.size())*sizeof(float) == 0)
-					{
-						int cameraCount = dataSize / ((6+localTransform.size())*sizeof(float));
-						UDEBUG("Loading calibration for %d cameras (%d bytes)", cameraCount, dataSize);
-						int max = cameraCount*(6+localTransform.size());
-						for(int i=0; i<max; i+=6+localTransform.size())
-						{
-							memcpy(localTransform.data(), dataFloat+i+6, localTransform.size()*sizeof(float));
-							models.push_back(CameraModel(
-									(double)dataFloat[i],
-									(double)dataFloat[i+1],
-									(double)dataFloat[i+2],
-									(double)dataFloat[i+3],
-									localTransform));
-							models.back().setImageSize(cv::Size(dataFloat[i+4], dataFloat[i+5]));
-							UDEBUG("%f %f %f %f %f %f %s", dataFloat[i], dataFloat[i+1], dataFloat[i+2],
-									dataFloat[i+3], dataFloat[i+4], dataFloat[i+5],
-									localTransform.prettyPrint().c_str());
-						}
-					}
 					else
 					{
 						UFATAL("Wrong format of the Data.calibration field (size=%d bytes)", dataSize);
@@ -1861,7 +1918,7 @@ void DBDriverSqlite3::loadSignaturesQuery(const std::list<int> & ids, std::list<
 						int index=0;
 						const void * data = 0;
 						int dataSize = 0;
-						Transform localTransform = Transform::getIdentity();
+						Transform localTransform;
 						std::vector<CameraModel> models;
 						StereoCameraModel stereoModel;
 
@@ -1874,13 +1931,39 @@ void DBDriverSqlite3::loadSignaturesQuery(const std::list<int> & ids, std::list<
 						{
 							++calibrationsLoaded;
 							float * dataFloat = (float*)data;
-							if((unsigned int)dataSize % (4+localTransform.size())*sizeof(float) == 0)
+							if(uStrNumCmp(_version, "0.11.2") >= 0 &&
+							   (unsigned int)dataSize % (6+localTransform.size())*sizeof(float) == 0)
+							{
+								int cameraCount = dataSize / ((6+localTransform.size())*sizeof(float));
+								UDEBUG("Loading calibration for %d cameras (%d bytes)", cameraCount, dataSize);
+								int max = cameraCount*(6+localTransform.size());
+								for(int i=0; i<max; i+=6+localTransform.size())
+								{
+									// Reinitialize to a new Transform, to avoid copying in the same memory than the previous one
+									localTransform = Transform::getIdentity();
+									memcpy(localTransform.data(), dataFloat+i+6, localTransform.size()*sizeof(float));
+									models.push_back(CameraModel(
+											(double)dataFloat[i],
+											(double)dataFloat[i+1],
+											(double)dataFloat[i+2],
+											(double)dataFloat[i+3],
+											localTransform));
+									models.back().setImageSize(cv::Size(dataFloat[i+4], dataFloat[i+5]));
+									UDEBUG("%f %f %f %f %f %f %s", dataFloat[i], dataFloat[i+1], dataFloat[i+2],
+											dataFloat[i+3], dataFloat[i+4], dataFloat[i+5],
+											localTransform.prettyPrint().c_str());
+								}
+							}
+							else if(uStrNumCmp(_version, "0.11.2") < 0 &&
+									(unsigned int)dataSize % (4+localTransform.size())*sizeof(float) == 0)
 							{
 								int cameraCount = dataSize / ((4+localTransform.size())*sizeof(float));
 								UDEBUG("Loading calibration for %d cameras (%d bytes)", cameraCount, dataSize);
 								int max = cameraCount*(4+localTransform.size());
 								for(int i=0; i<max; i+=4+localTransform.size())
 								{
+									// Reinitialize to a new Transform, to avoid copying in the same memory than the previous one
+									localTransform = Transform::getIdentity();
 									memcpy(localTransform.data(), dataFloat+i+4, localTransform.size()*sizeof(float));
 									models.push_back(CameraModel(
 											(double)dataFloat[i],
@@ -1902,26 +1985,6 @@ void DBDriverSqlite3::loadSignaturesQuery(const std::list<int> & ids, std::list<
 										dataFloat[4], // baseline
 										localTransform);
 							}
-							else if((unsigned int)dataSize % (6+localTransform.size())*sizeof(float) == 0)
-							{
-								int cameraCount = dataSize / ((6+localTransform.size())*sizeof(float));
-								UDEBUG("Loading calibration for %d cameras (%d bytes)", cameraCount, dataSize);
-								int max = cameraCount*(6+localTransform.size());
-								for(int i=0; i<max; i+=6+localTransform.size())
-								{
-									memcpy(localTransform.data(), dataFloat+i+6, localTransform.size()*sizeof(float));
-									models.push_back(CameraModel(
-											(double)dataFloat[i],
-											(double)dataFloat[i+1],
-											(double)dataFloat[i+2],
-											(double)dataFloat[i+3],
-											localTransform));
-									models.back().setImageSize(cv::Size(dataFloat[i+4], dataFloat[i+5]));
-									UDEBUG("%f %f %f %f %f %f %s", dataFloat[i], dataFloat[i+1], dataFloat[i+2],
-											dataFloat[i+3], dataFloat[i+4], dataFloat[i+5],
-											localTransform.prettyPrint().c_str());
-								}
-							}
 							else
 							{
 								UFATAL("Wrong format of the Data.calibration field (size=%d bytes)", dataSize);
@@ -3180,19 +3243,37 @@ void DBDriverSqlite3::stepSensorData(sqlite3_stmt * ppStmt,
 	std::vector<float> calibration;
 	// multi-cameras [fx,fy,cx,cy,width,height,local_transform, ... ,fx,fy,cx,cy,width,height,local_transform] (6+12)*float * numCameras
 	// stereo [fx, fy, cx, cy, baseline, local_transform] (5+12)*float
-	if(sensorData.cameraModels().size())
+	if(sensorData.cameraModels().size() && sensorData.cameraModels()[0].isValidForProjection())
-	{
+	{
-		calibration.resize(sensorData.cameraModels().size() * (6+Transform().size()));
+		if(uStrNumCmp(_version, "0.11.2") >= 0)
-		for(unsigned int i=0; i<sensorData.cameraModels().size(); ++i)
 		{
-			const Transform & localTransform = sensorData.cameraModels()[i].localTransform();
+			calibration.resize(sensorData.cameraModels().size() * (6+Transform().size()));
-			calibration[i*(6+localTransform.size())] = sensorData.cameraModels()[i].fx();
+			for(unsigned int i=0; i<sensorData.cameraModels().size(); ++i)
-			calibration[i*(6+localTransform.size())+1] = sensorData.cameraModels()[i].fy();
+			{
-			calibration[i*(6+localTransform.size())+2] = sensorData.cameraModels()[i].cx();
+				UASSERT(sensorData.cameraModels()[i].isValidForProjection());
-			calibration[i*(6+localTransform.size())+3] = sensorData.cameraModels()[i].cy();
+				const Transform & localTransform = sensorData.cameraModels()[i].localTransform();
-			calibration[i*(6+localTransform.size())+4] = sensorData.cameraModels()[i].imageWidth();
+				calibration[i*(6+localTransform.size())] = sensorData.cameraModels()[i].fx();
-			calibration[i*(6+localTransform.size())+5] = sensorData.cameraModels()[i].imageHeight();
+				calibration[i*(6+localTransform.size())+1] = sensorData.cameraModels()[i].fy();
-			memcpy(calibration.data()+i*(6+localTransform.size())+6, localTransform.data(), localTransform.size()*sizeof(float));
+				calibration[i*(6+localTransform.size())+2] = sensorData.cameraModels()[i].cx();
+				calibration[i*(6+localTransform.size())+3] = sensorData.cameraModels()[i].cy();
+				calibration[i*(6+localTransform.size())+4] = sensorData.cameraModels()[i].imageWidth();
+				calibration[i*(6+localTransform.size())+5] = sensorData.cameraModels()[i].imageHeight();
+				memcpy(calibration.data()+i*(6+localTransform.size())+6, localTransform.data(), localTransform.size()*sizeof(float));
+			}
+		}
+		else
+		{
+			calibration.resize(sensorData.cameraModels().size() * (4+Transform().size()));
+			for(unsigned int i=0; i<sensorData.cameraModels().size(); ++i)
+			{
+				UASSERT(sensorData.cameraModels()[i].isValidForProjection());
+				const Transform & localTransform = sensorData.cameraModels()[i].localTransform();
+				calibration[i*(4+localTransform.size())] = sensorData.cameraModels()[i].fx();
+				calibration[i*(4+localTransform.size())+1] = sensorData.cameraModels()[i].fy();
+				calibration[i*(4+localTransform.size())+2] = sensorData.cameraModels()[i].cx();
+				calibration[i*(4+localTransform.size())+3] = sensorData.cameraModels()[i].cy();
+				memcpy(calibration.data()+i*(4+localTransform.size())+4, localTransform.data(), localTransform.size()*sizeof(float));
+			}
 		}
 	}
 	else if(sensorData.stereoCameraModel().isValidForProjection())

corelib/src/DBDriverSqlite3.h

@@ -1,5 +1,5 @@
 /*
-Copyright (c) 2010-2014, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
+Copyright (c) 2010-2016, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
 All rights reserved.
 
 Redistribution and use in source and binary forms, with or without
@@ -62,6 +62,7 @@ private:
 	virtual int getLastDictionarySizeQuery() const;
 	virtual int getTotalNodesSizeQuery() const;
 	virtual int getTotalDictionarySizeQuery() const;
+	virtual ParametersMap getLastParametersQuery() const;
 
 	virtual void executeNoResultQuery(const std::string & sql) const;
 

corelib/src/DBReader.cpp

@@ -1,5 +1,5 @@
 /*
-Copyright (c) 2010-2014, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
+Copyright (c) 2010-2016, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
 All rights reserved.
 
 Redistribution and use in source and binary forms, with or without
@@ -33,6 +33,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #include <rtabmap/utilite/UFile.h>
 #include <rtabmap/utilite/UStl.h>
 #include <rtabmap/utilite/UConversion.h>
+#include <rtabmap/utilite/UEventsManager.h>
 
 #include "rtabmap/core/CameraEvent.h"
 #include "rtabmap/core/RtabmapEvent.h"
@@ -46,16 +47,21 @@ DBReader::DBReader(const std::string & databasePath,
 				   float frameRate,
 				   bool odometryIgnored,
 				   bool ignoreGoalDelay,
-				   bool goalsIgnored) :
+				   bool goalsIgnored,
+				   int startIndex,
+				   int cameraIndex) :
+	Camera(frameRate),
 	_paths(uSplit(databasePath, ';')),
-	_frameRate(frameRate),
 	_odometryIgnored(odometryIgnored),
 	_ignoreGoalDelay(ignoreGoalDelay),
 	_goalsIgnored(goalsIgnored),
+	_startIndex(startIndex),
+	_cameraIndex(cameraIndex),
 	_dbDriver(0),
 	_currentId(_ids.end()),
 	_previousStamp(0),
-	_previousMapID(0)
+	_previousMapID(0),
+	_calibrated(false)
 {
 }
 
@@ -63,16 +69,21 @@ DBReader::DBReader(const std::list<std::string> & databasePaths,
 				   float frameRate,
 				   bool odometryIgnored,
 				   bool ignoreGoalDelay,
-				   bool goalsIgnored) :
+				   bool goalsIgnored,
+				   int startIndex,
+				   int cameraIndex) :
+	Camera(frameRate),
    _paths(databasePaths),
-   _frameRate(frameRate),
 	_odometryIgnored(odometryIgnored),
 	_ignoreGoalDelay(ignoreGoalDelay),
 	_goalsIgnored(goalsIgnored),
+	_startIndex(startIndex),
+	_cameraIndex(cameraIndex),
 	_dbDriver(0),
 	_currentId(_ids.end()),
 	_previousStamp(0),
-	_previousMapID(0)
+	_previousMapID(0),
+	_calibrated(false)
 {
 }
 
@@ -85,7 +96,9 @@ DBReader::~DBReader()
 	}
 }
 
-bool DBReader::init(int startIndex)
+bool DBReader::init(
+		const std::string & calibrationFolder,
+		const std::string & cameraName)
 {
 	if(_dbDriver)
 	{
@@ -97,6 +110,7 @@ bool DBReader::init(int startIndex)
 	_currentId=_ids.end();
 	_previousStamp = 0;
 	_previousMapID = 0;
+	_calibrated = false;
 
 	if(_paths.size() == 0)
 	{
@@ -129,12 +143,12 @@ bool DBReader::init(int startIndex)
 
 	_dbDriver->getAllNodeIds(_ids);
 	_currentId = _ids.begin();
-	if(startIndex>0 && _ids.size())
+	if(_startIndex>0 && _ids.size())
 	{
-		std::set<int>::iterator iter = uIteratorAt(_ids, startIndex);
+		std::set<int>::iterator iter = uIteratorAt(_ids, _startIndex);
 		if(iter == _ids.end())
 		{
-			UWARN("Start index is too high (%d), the last in database is %d. Starting from beginning...", startIndex, _ids.size()-1);
+			UWARN("Start index is too high (%d), the last in database is %d. Starting from beginning...", _startIndex, _ids.size()-1);
 		}
 		else
 		{
@@ -142,142 +156,139 @@ bool DBReader::init(int startIndex)
 		}
 	}
 
+	if(_ids.size())
+	{
+		std::vector<CameraModel> models;
+		StereoCameraModel stereoModel;
+		if(_dbDriver->getCalibration(*_ids.begin(), models, stereoModel))
+		{
+			if(models.size() && models.at(0).isValidForProjection())
+			{
+				_calibrated = true;
+			}
+			else if(stereoModel.isValidForProjection())
+			{
+				_calibrated = true;
+			}
+		}
+	}
+
+	_timer.start();
+
 	return true;
 }
 
-void DBReader::setFrameRate(float frameRate)
+bool DBReader::isCalibrated() const
 {
-	_frameRate = frameRate;
+	return _calibrated;
 }
 
-void DBReader::mainLoopBegin()
+std::string DBReader::getSerial() const
 {
-	_timer.start();
+	return "DBReader";
 }
 
-void DBReader::mainLoop()
+SensorData DBReader::captureImage(CameraInfo * info)
 {
-	OdometryEvent odom = this->getNextData();
+	SensorData data = this->getNextData(info);
-	if(odom.data().id())
+	if(data.id() == 0)
-	{
-		std::string goalId;
-		double previousStamp = odom.data().stamp();
-		if(previousStamp == 0)
-		{
-			odom.data().setStamp(UTimer::now());
-		}
-
-		if(!_goalsIgnored &&
-		   odom.data().userDataRaw().type() == CV_8SC1 &&
-		   odom.data().userDataRaw().cols >= 7 && // including null str ending
-		   odom.data().userDataRaw().rows == 1 &&
-		   memcmp(odom.data().userDataRaw().data, "GOAL:", 5) == 0)
-		{
-			//GOAL format detected, remove it from the user data and send it as goal event
-			std::string goalStr = (const char *)odom.data().userDataRaw().data;
-			if(!goalStr.empty())
-			{
-				std::list<std::string> strs = uSplit(goalStr, ':');
-				if(strs.size() == 2)
-				{
-					goalId = *strs.rbegin();
-					odom.data().setUserData(cv::Mat());
-				}
-			}
-		}
-		if(!_odometryIgnored)
-		{
-			if(odom.pose().isNull())
-			{
-				UWARN("Reading the database: odometry is null! "
-					  "Please set \"Ignore odometry = true\" if there is "
-					  "no odometry in the database.");
-			}
-			this->post(new OdometryEvent(odom));
-		}
-		else
-		{
-			this->post(new CameraEvent(odom.data()));
-		}
-
-		if(!goalId.empty())
-		{
-			double delay = 0.0;
-			if(!_ignoreGoalDelay && _currentId != _ids.end())
-			{
-				// get stamp for the next signature to compute the delay
-				// that was used originally for planning
-				int weight;
-				std::string label;
-				double stamp;
-				int mapId;
-				Transform localTransform, pose, groundTruth;
-				_dbDriver->getNodeInfo(*_currentId, pose, mapId, weight, label, stamp, groundTruth);
-				if(previousStamp && stamp && stamp > previousStamp)
-				{
-					delay = stamp - previousStamp;
-				}
-			}
-
-			if(delay > 0.0)
-			{
-				UWARN("Goal \"%s\" detected, posting it! Waiting %f seconds before sending next data...",
-					   goalId.c_str(), delay);
-			}
-			else
-			{
-				UWARN("Goal \"%s\" detected, posting it!", goalId.c_str());
-			}
-
-			if(uIsInteger(goalId))
-			{
-				this->post(new RtabmapEventCmd(RtabmapEventCmd::kCmdGoal, atoi(goalId.c_str())));
-			}
-			else
-			{
-				this->post(new RtabmapEventCmd(RtabmapEventCmd::kCmdGoal, goalId));
-			}
-
-			if(delay > 0.0)
-			{
-				uSleep(delay*1000);
-			}
-		}
-
-	}
-	else if(!this->isKilled())
 	{
 		UINFO("no more images...");
-		if(_paths.size() > 1)
+		while(_paths.size() > 1 && data.id() == 0)
 		{
 			_paths.pop_front();
 			UWARN("Loading next database \"%s\"...", _paths.front().c_str());
 			if(!this->init())
 			{
 				UERROR("Failed to initialize the next database \"%s\"", _paths.front().c_str());
-				this->kill();
+				return data;
-				this->post(new CameraEvent());
+			}
+			else
+			{
+				data = this->getNextData(info);
 			}
 		}
-		else
+	}
+	if(data.id())
+	{
+		std::string goalId;
+		double previousStamp = data.stamp();
+		if(previousStamp == 0)
 		{
-			this->kill();
+			data.setStamp(UTimer::now());
-			this->post(new CameraEvent());
 		}
 
-	}
+		if(!_goalsIgnored &&
+		   data.userDataRaw().type() == CV_8SC1 &&
+		   data.userDataRaw().cols >= 7 && // including null str ending
+		   data.userDataRaw().rows == 1 &&
+		   memcmp(data.userDataRaw().data, "GOAL:", 5) == 0)
+		{
+			//GOAL format detected, remove it from the user data and send it as goal event
+			std::string goalStr = (const char *)data.userDataRaw().data;
+			if(!goalStr.empty())
+			{
+				std::list<std::string> strs = uSplit(goalStr, ':');
+				if(strs.size() == 2)
+				{
+					goalId = *strs.rbegin();
+					data.setUserData(cv::Mat());
 
+					double delay = 0.0;
+					if(!_ignoreGoalDelay && _currentId != _ids.end())
+					{
+						// get stamp for the next signature to compute the delay
+						// that was used originally for planning
+						int weight;
+						std::string label;
+						double stamp;
+						int mapId;
+						Transform localTransform, pose, groundTruth;
+						_dbDriver->getNodeInfo(*_currentId, pose, mapId, weight, label, stamp, groundTruth);
+						if(previousStamp && stamp && stamp > previousStamp)
+						{
+							delay = stamp - previousStamp;
+						}
+					}
+
+					if(delay > 0.0)
+					{
+						UWARN("Goal \"%s\" detected, posting it! Waiting %f seconds before sending next data...",
+							   goalId.c_str(), delay);
+					}
+					else
+					{
+						UWARN("Goal \"%s\" detected, posting it!", goalId.c_str());
+					}
+
+					if(uIsInteger(goalId))
+					{
+						UEventsManager::post(new RtabmapEventCmd(RtabmapEventCmd::kCmdGoal, atoi(goalId.c_str())));
+					}
+					else
+					{
+						UEventsManager::post(new RtabmapEventCmd(RtabmapEventCmd::kCmdGoal, goalId));
+					}
+
+					if(delay > 0.0)
+					{
+						uSleep(delay*1000);
+					}
+				}
+			}
+		}
+	}
+	return data;
 }
 
-OdometryEvent DBReader::getNextData()
+SensorData DBReader::getNextData(CameraInfo * info)
 {
-	OdometryEvent odom;
+	SensorData data;
 	if(_dbDriver)
 	{
-		if(!this->isKilled() && _currentId != _ids.end())
+		if(_currentId != _ids.end())
 		{
 			int mapId;
-			SensorData data;
 			_dbDriver->getNodeData(*_currentId, data);
 
 			// info
@@ -312,12 +323,12 @@ OdometryEvent DBReader::getNextData()
 			}
 
 			// Frame rate
-			if(_frameRate < 0.0f)
+			if(this->getImageRate() < 0.0f)
 			{
 				if(stamp == 0)
 				{
 					UERROR("The option to use database stamps is set (framerate<0), but there are no stamps saved in the database! Aborting...");
-					this->kill();
+					return data;
 				}
 				else if(_previousMapID == mapId && _previousStamp > 0)
 				{
@@ -346,38 +357,61 @@ OdometryEvent DBReader::getNextData()
 				_previousStamp = stamp;
 				_previousMapID = mapId;
 			}
-			else if(_frameRate>0.0f)
+
+			data.uncompressData();
+			if(data.cameraModels().size() > 1 &&
+				_cameraIndex >= 0)
 			{
-				int sleepTime = (1000.0f/_frameRate - 1000.0f*_timer.getElapsedTime());
+				if(_cameraIndex < (int)data.cameraModels().size())
-				if(sleepTime > 2)
 				{
-					uSleep(sleepTime-2);
+					// select one camera
-				}
+					int subImageWidth = data.imageRaw().cols/data.cameraModels().size();
+					UASSERT(!data.imageRaw().empty() &&
+							data.imageRaw().cols % data.cameraModels().size() == 0 &&
+							_cameraIndex*subImageWidth < data.imageRaw().cols);
+					data.setImageRaw(
+							cv::Mat(data.imageRaw(),
+							cv::Rect(_cameraIndex*subImageWidth, 0, subImageWidth, data.imageRaw().rows)).clone());
 
-				// Add precision at the cost of a small overhead
+					if(!data.depthOrRightRaw().empty())
-				while(_timer.getElapsedTime() < 1.0/double(_frameRate)-0.000001)
+					{
+						UASSERT(data.depthOrRightRaw().cols % data.cameraModels().size() == 0 &&
+								subImageWidth == data.depthOrRightRaw().cols/(int)data.cameraModels().size() &&
+								_cameraIndex*subImageWidth < data.depthOrRightRaw().cols);
+						data.setDepthOrRightRaw(
+								cv::Mat(data.depthOrRightRaw(),
+								cv::Rect(_cameraIndex*subImageWidth, 0, subImageWidth, data.depthOrRightRaw().rows)).clone());
+					}
+					CameraModel model = data.cameraModels().at(_cameraIndex);
+					data.setCameraModel(model);
+				}
+				else
 				{
-					//
+					UWARN("DBReader: Camera index %d doesn't exist! Camera models = %d.", _cameraIndex, (int)data.cameraModels().size());
 				}
-
-				double slept = _timer.getElapsedTime();
-				_timer.start();
-				UDEBUG("slept=%fs vs target=%fs", slept, 1.0/double(_frameRate));
 			}
+			data.setId(seq);
+			data.setStamp(stamp);
+			data.setGroundTruth(groundTruth);
+			UDEBUG("Laser=%d RGB/Left=%d Depth/Right=%d, UserData=%d",
+					data.laserScanRaw().empty()?0:1,
+					data.imageRaw().empty()?0:1,
+					data.depthOrRightRaw().empty()?0:1,
+					data.userDataRaw().empty()?0:1);
 
-			if(!this->isKilled())
+			if(!_odometryIgnored)
 			{
-				data.uncompressData();
+				if(pose.isNull())
-				data.setId(seq);
+				{
-				data.setStamp(stamp);
+					UWARN("Reading the database: odometry is null! "
-				data.setGroundTruth(groundTruth);
+						  "Please set \"Ignore odometry = true\" if there is "
-				UDEBUG("Laser=%d RGB/Left=%d Depth/Right=%d, UserData=%d",
+						  "no odometry in the database.");
-						data.laserScanRaw().empty()?0:1,
+				}
-						data.imageRaw().empty()?0:1,
+				if(info)
-						data.depthOrRightRaw().empty()?0:1,
+				{
-						data.userDataRaw().empty()?0:1);
+					info->odomPose = pose;
-
+					info->odomCovariance = infMatrix.inv();
-				odom = OdometryEvent(data, pose, infMatrix.inv());
+				}
 			}
 		}
 	}
@@ -385,7 +419,7 @@ OdometryEvent DBReader::getNextData()
 	{
 		UERROR("Not initialized...");
 	}
-	return odom;
+	return data;
 }
 
 } /* namespace rtabmap */

corelib/src/EpipolarGeometry.cpp

@@ -1,5 +1,5 @@
 /*
-Copyright (c) 2010-2014, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
+Copyright (c) 2010-2016, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
 All rights reserved.
 
 Redistribution and use in source and binary forms, with or without

corelib/src/Features2d.cpp

@@ -1,5 +1,5 @@
 /*
-Copyright (c) 2010-2014, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
+Copyright (c) 2010-2016, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
 All rights reserved.
 
 Redistribution and use in source and binary forms, with or without
@@ -492,30 +492,42 @@ std::vector<cv::KeyPoint> Feature2D::generateKeypoints(const cv::Mat & image, co
 	UASSERT(image.type() == CV_8UC1);
 
 	cv::Mat mask;
-	if(maskIn.type()==CV_16UC1 || maskIn.type() == CV_32FC1)
+	if(!maskIn.empty())
 	{
-		mask = cv::Mat::zeros(maskIn.rows, maskIn.cols, CV_8UC1);
+		if(maskIn.type()==CV_16UC1 || maskIn.type() == CV_32FC1)
-		for(int i=0; i<(int)mask.total(); ++i)
 		{
-			float value = 0.0f;
+			mask = cv::Mat::zeros(maskIn.rows, maskIn.cols, CV_8UC1);
-			if(maskIn.type()==CV_16UC1)
+			for(int i=0; i<(int)mask.total(); ++i)
 			{
-				if(((unsigned short*)maskIn.data)[i] > 0 &&
+				float value = 0.0f;
-				   ((unsigned short*)maskIn.data)[i] < std::numeric_limits<unsigned short>::max())
+				if(maskIn.type()==CV_16UC1)
 				{
-					value = float(((unsigned short*)maskIn.data)[i])*0.001f;
+					if(((unsigned short*)maskIn.data)[i] > 0 &&
+					   ((unsigned short*)maskIn.data)[i] < std::numeric_limits<unsigned short>::max())
+					{
+						value = float(((unsigned short*)maskIn.data)[i])*0.001f;
+					}
+				}
+				else
+				{
+					value = ((float*)maskIn.data)[i];
+				}
+
+				if(value>_minDepth &&
+				   (_maxDepth == 0.0f || value <= _maxDepth))
+				{
+					((unsigned char*)mask.data)[i] = 255; // ORB uses 255 to handle pyramids
 				}
 			}
-			else
+		}
-			{
+		else if(maskIn.type()==CV_8UC1)
-				value = ((float*)maskIn.data)[i];
+		{
-			}
+			// assume a standard mask
-
+			mask = maskIn;
-			if(value>_minDepth &&
+		}
-			   (_maxDepth == 0.0f || value <= _maxDepth))
+		else
-			{
+		{
-				((unsigned char*)mask.data)[i] = 1;
+			UERROR("Wrong mask type (%d)! Should be 8UC1, 16UC1 or 32FC1.", maskIn.type());
-			}
 		}
 	}
 
@@ -527,7 +539,7 @@ std::vector<cv::KeyPoint> Feature2D::generateKeypoints(const cv::Mat & image, co
 	// Get keypoints
 	cv::Rect roi = Feature2D::computeRoi(image, _roiRatios);
 	keypoints = this->generateKeypointsImpl(image, roi.width && roi.height?roi:cv::Rect(0,0,image.cols, image.rows), mask);
-	UDEBUG("Keypoints extraction time = %f s, keypoints extracted = %d", timer.ticks(), keypoints.size());
+	UDEBUG("Keypoints extraction time = %f s, keypoints extracted = %d (mask empty=%d)", timer.ticks(), keypoints.size(), mask.empty()?1:0);
 
 	limitKeypoints(keypoints, maxFeatures_);
 

corelib/src/Graph.cpp

@@ -1,5 +1,5 @@
 /*
-Copyright (c) 2010-2014, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
+Copyright (c) 2010-2016, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
 All rights reserved.
 
 Redistribution and use in source and binary forms, with or without

corelib/src/Link.cpp

@@ -1,5 +1,5 @@
 /*
-Copyright (c) 2010-2014, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
+Copyright (c) 2010-2016, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
 All rights reserved.
 
 Redistribution and use in source and binary forms, with or without

corelib/src/Memory.cpp

@@ -1,5 +1,5 @@
 /*
-Copyright (c) 2010-2014, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
+Copyright (c) 2010-2016, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
 All rights reserved.
 
 Redistribution and use in source and binary forms, with or without
@@ -451,7 +451,7 @@ void Memory::parseParameters(const ParametersMap & parameters)
 	else if(_feature2D)
 	{
 		_feature2D->parseParameters(parameters);
-	}
+	}
 
 	Registration::Type regStrategy = Registration::kTypeUndef;
 	if((iter=parameters.find(Parameters::kRegStrategy())) != parameters.end())
@@ -478,29 +478,29 @@ void Memory::parseParameters(const ParametersMap & parameters)
 	{
 		_registrationIcp->parseParameters(parameters);
 	}
-
+
-	// do this after all parameters are parsed
+	// do this after all parameters are parsed
-	// SLAM mode vs Localization mode
+	// SLAM mode vs Localization mode
-	iter = parameters.find(Parameters::kMemIncrementalMemory());
+	iter = parameters.find(Parameters::kMemIncrementalMemory());
-	if(iter != parameters.end())
+	if(iter != parameters.end())
-	{
+	{
-		bool value = uStr2Bool(iter->second.c_str());
+		bool value = uStr2Bool(iter->second.c_str());
-		if(value == false && _incrementalMemory)
+		if(value == false && _incrementalMemory)
-		{
+		{
-			// From SLAM to localization, change map id
+			// From SLAM to localization, change map id
-			this->incrementMapId();
+			this->incrementMapId();
-
+
-			// The easiest way to make sure that the mapping session is saved
+			// The easiest way to make sure that the mapping session is saved
-			// is to save the memory in the database and reload it.
+			// is to save the memory in the database and reload it.
-			if((_memoryChanged || _linksChanged) && _dbDriver)
+			if((_memoryChanged || _linksChanged) && _dbDriver)
-			{
+			{
-				UWARN("Switching from Mapping to Localization mode, the database will be saved and reloaded.");
+				UWARN("Switching from Mapping to Localization mode, the database will be saved and reloaded.");
 				this->init(_dbDriver->getUrl());
-				UWARN("Switching from Mapping to Localization mode, the database is reloaded!");
+				UWARN("Switching from Mapping to Localization mode, the database is reloaded!");
-			}
+			}
-		}
+		}
-		_incrementalMemory = value;
+		_incrementalMemory = value;
-	}
+	}
 }
 
 void Memory::preUpdate()
@@ -1275,7 +1275,8 @@ void Memory::clear()
 					_lastSignature?_lastSignature->id():0,
 					UProcessInfo::getMemoryUsage(),
 					_dbDriver->getMemoryUsed(),
-					(int)_vwd->getVisualWords().size());
+					(int)_vwd->getVisualWords().size(),
+					parameters_);
 		}
 	}
 	UDEBUG("");
@@ -1902,6 +1903,7 @@ bool Memory::labelSignature(int id, const std::string & label)
 		if(s)
 		{
 			s->setLabel(label);
+			_linksChanged = s->isSaved(); // HACK to get label updated in Localization mode
 			UWARN("Label \"%s\" set to node %d", label.c_str(), id);
 			return true;
 		}
@@ -2123,32 +2125,13 @@ Transform Memory::computeTransform(
 
 	// compute transform fromId -> toId
 	std::vector<int> inliersV;
-	if(_reextractLoopClosureFeatures || (fromS.getWords().size() && toS.getWords().size()))
+	if(_reextractLoopClosureFeatures ||
+		(fromS.getWords().size() && toS.getWords().size()) ||
+		(!guess.isNull() && !_registrationPipeline->isImageRequired()))
 	{
 		Signature tmpFrom = fromS;
 		Signature tmpTo = toS;
 
-		// make a guess fast with known correspondences (if there are)
-		RegistrationVis regVis(parameters_);
-		if(tmpFrom.getWords().size() &&
-			tmpTo.getWords().size() &&
-			tmpFrom.getWords3().size() &&
-			tmpTo.getWords3().size())
-		{
-			UDEBUG("");
-			// Remove descriptors, this will avoid recomputation of the correspondences in regVis
-			tmpFrom.setWordsDescriptors(std::multimap<int, cv::Mat>());
-			tmpTo.setWordsDescriptors(std::multimap<int, cv::Mat>());
-			Transform t = regVis.computeTransformation(tmpFrom, tmpTo, guess, info);
-			// set back descriptors
-			tmpFrom.setWordsDescriptors(fromS.getWordsDescriptors());
-			tmpTo.setWordsDescriptors(toS.getWordsDescriptors());
-			if(!t.isNull())
-			{
-				guess = t;
-			}
-		}
-
 		if(_reextractLoopClosureFeatures)
 		{
 			UDEBUG("");
@@ -2166,10 +2149,17 @@ Transform Memory::computeTransform(
 		{
 			UDEBUG("");
 			// no visual in the pipeline, make visual registration for guess
+			RegistrationVis regVis(parameters_);
 			guess = regVis.computeTransformation(tmpFrom, tmpTo, guess, info);
+			if(!guess.isNull())
+			{
+				transform = _registrationPipeline->computeTransformation(tmpFrom, tmpTo, guess, info);
+			}
+		}
+		else
+		{
+			transform = _registrationPipeline->computeTransformation(tmpFrom, tmpTo, guess, info);
 		}
-
-		transform = _registrationPipeline->computeTransformation(tmpFrom, tmpTo, guess, info);
 
 		if(!transform.isNull())
 		{
@@ -3232,8 +3222,7 @@ Signature * Memory::createSignature(const SensorData & data, const Transform & p
 			}
 
 			cv::Mat depthMask;
-			if(!decimatedData.depthRaw().empty() &&
+			if(!decimatedData.depthRaw().empty())
-			   _feature2D->getType() != Feature2D::kFeatureOrb) // ORB's mask pyramids don't seem to work well
 			{
 				if(imageMono.rows % decimatedData.depthRaw().rows == 0 &&
 					imageMono.cols % decimatedData.depthRaw().cols == 0 &&
@@ -3264,34 +3253,6 @@ Signature * Memory::createSignature(const SensorData & data, const Transform & p
 					(!decimatedData.rightRaw().empty() && decimatedData.stereoCameraModel().isValidForProjection()))
 			{
 				keypoints3D = _feature2D->generateKeypoints3D(decimatedData, keypoints);
-				if(_feature2D->getMinDepth() > 0.0f || _feature2D->getMaxDepth() > 0.0f)
-				{
-					UDEBUG("");
-					//remove all keypoints/descriptors with no valid 3D points
-					UASSERT((int)keypoints.size() == descriptors.rows &&
-							keypoints3D.size() == keypoints.size());
-					std::vector<cv::KeyPoint> validKeypoints(keypoints.size());
-					std::vector<cv::Point3f> validKeypoints3D(keypoints.size());
-					cv::Mat validDescriptors(descriptors.size(), descriptors.type());
-
-					int oi=0;
-					for(unsigned int i=0; i<keypoints3D.size(); ++i)
-					{
-						if(util3d::isFinite(keypoints3D[i]))
-						{
-							validKeypoints[oi] = keypoints[i];
-							validKeypoints3D[oi] = keypoints3D[i];
-							descriptors.row(i).copyTo(validDescriptors.row(oi));
-							++oi;
-						}
-					}
-					UDEBUG("Removed %d invalid 3D points", (int)keypoints3D.size()-oi);
-					validKeypoints.resize(oi);
-					validKeypoints3D.resize(oi);
-					keypoints = validKeypoints;
-					keypoints3D = validKeypoints3D;
-					descriptors = validDescriptors.rowRange(0, oi).clone();
-				}
 				t = timer.ticks();
 				if(stats) stats->addStatistic(Statistics::kTimingMemKeypoints_3D(), t*1000.0f);
 				UDEBUG("time keypoints 3D (%d) = %fs", (int)keypoints3D.size(), t);

corelib/src/OctoMap.cpp

@@ -0,0 +1,511 @@
+/*
+Copyright (c) 2010-2016, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
+All rights reserved.
+
+Redistribution and use in source and binary forms, with or without
+modification, are permitted provided that the following conditions are met:
+    * Redistributions of source code must retain the above copyright
+      notice, this list of conditions and the following disclaimer.
+    * Redistributions in binary form must reproduce the above copyright
+      notice, this list of conditions and the following disclaimer in the
+      documentation and/or other materials provided with the distribution.
+    * Neither the name of the Universite de Sherbrooke nor the
+      names of its contributors may be used to endorse or promote products
+      derived from this software without specific prior written permission.
+
+THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
+ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY
+DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+*/
+
+#include <rtabmap/core/OctoMap.h>
+#include <rtabmap/utilite/ULogger.h>
+#include <rtabmap/utilite/UStl.h>
+#include <rtabmap/core/util3d_transforms.h>
+#include <rtabmap/core/util3d_filtering.h>
+#include <rtabmap/core/util3d_mapping.h>
+
+namespace rtabmap {
+
+OctoMap::OctoMap(float voxelSize) :
+		octree_(new octomap::ColorOcTree(voxelSize))
+{
+	UASSERT(voxelSize>0.0f);
+}
+
+OctoMap::~OctoMap()
+{
+	this->clear();
+	delete octree_;
+}
+
+void OctoMap::clear()
+{
+	octree_->clear();
+	occupiedCells_.clear();
+	cache_.clear();
+	addedNodes_.clear();
+	keyRay_ = octomap::KeyRay();
+}
+
+void OctoMap::addToCache(int nodeId,
+		pcl::PointCloud<pcl::PointXYZRGB>::Ptr & ground,
+		pcl::PointCloud<pcl::PointXYZRGB>::Ptr & obstacles)
+{
+	UDEBUG("nodeId=%d", nodeId);
+	cache_.insert(std::make_pair(nodeId, std::make_pair(ground, obstacles)));
+}
+
+void OctoMap::update(const std::map<int, Transform> & poses)
+{
+	UDEBUG("Update (poses=%d addedNodes_=%d)", (int)poses.size(), (int)addedNodes_.size());
+
+	// First, check of the graph has changed. If so, re-create the octree by moving all occupied nodes.
+	bool graphChanged = false;
+	std::map<int, Transform> transforms;
+	std::map<int, Transform> updatedAddedNodes;
+	for(std::map<int, Transform>::iterator iter=addedNodes_.begin(); iter!=addedNodes_.end(); ++iter)
+	{
+		std::map<int, Transform>::const_iterator jter = poses.find(iter->first);
+		if(jter != poses.end())
+		{
+			UASSERT(!iter->second.isNull() && !jter->second.isNull());
+			Transform t = Transform::getIdentity();
+			if(iter->second.getDistanceSquared(jter->second) > 0.0001)
+			{
+				t = jter->second * iter->second.inverse();
+				graphChanged = true;
+			}
+			transforms.insert(std::make_pair(jter->first, t));
+			updatedAddedNodes.insert(std::make_pair(jter->first, jter->second));
+		}
+		else
+		{
+			UWARN("Updated pose for node %d is not found, some points may not be copied.", jter->first);
+		}
+	}
+	if(graphChanged)
+	{
+		UINFO("Graph changed!");
+		octomap::ColorOcTree * newOcTree = new octomap::ColorOcTree(octree_->getResolution());
+		std::map<octomap::ColorOcTreeNode*, OcTreeNodeInfo > newOccupiedCells;
+		int copied=0;
+		for(std::map<octomap::ColorOcTreeNode*, OcTreeNodeInfo >::iterator iter = occupiedCells_.begin();
+				iter!=occupiedCells_.end();
+				++iter)
+		{
+			std::map<int, Transform>::iterator jter = transforms.find(iter->second.nodeRefId_);
+			if(jter != transforms.end())
+			{
+				octomap::point3d pt = octree_->keyToCoord(iter->second.key_);
+				std::map<int, Transform>::iterator pter = addedNodes_.find(iter->second.nodeRefId_);
+				UASSERT(pter != addedNodes_.end());
+
+				cv::Point3f cvPt(pt.x(), pt.y(), pt.z());
+				cvPt = util3d::transformPoint(cvPt, jter->second);
+
+				octomap::OcTreeKey key;
+				if(newOcTree->coordToKeyChecked(cvPt.x, cvPt.y, cvPt.z, key))
+				{
+					octomap::ColorOcTreeNode * n = newOcTree->updateNode(key, iter->second.isObstacle_);
+					if(n)
+					{
+						++copied;
+						uInsert(newOccupiedCells, std::make_pair(n, OcTreeNodeInfo(jter->first, key, iter->second.isObstacle_)));
+						newOcTree->setNodeColor(key, iter->first->getColor().r, iter->first->getColor().g, iter->first->getColor().b);
+					}
+					else
+					{
+						UERROR("Could not update node at (%f,%f,%f)", cvPt.x, cvPt.y, cvPt.z);
+					}
+				}
+				else
+				{
+					UERROR("Could not find key for (%f,%f,%f)", cvPt.x, cvPt.y, cvPt.z);
+				}
+			}
+			else if(jter == transforms.end() && iter->second.nodeRefId_ > 0)
+			{
+				UWARN("Could not find a transform for point linked to node %d (transforms=%d)", iter->second.nodeRefId_, (int)transforms.size());
+			}
+		}
+		UDEBUG("%d/%d", copied, (int)occupiedCells_.size());
+		delete octree_;
+		octree_ = newOcTree;
+		occupiedCells_ = newOccupiedCells;
+
+		//update added poses
+		addedNodes_ = updatedAddedNodes;
+	}
+
+	// Original version from A. Hornung:
+	// https://github.com/OctoMap/octomap_mapping/blob/jade-devel/octomap_server/src/OctomapServer.cpp#L356
+	//
+	int lastId = addedNodes_.size()?addedNodes_.rbegin()->first:0;
+	UDEBUG("Last id = %d", lastId);
+	if(lastId >= 0)
+	{
+		std::list<std::pair<int, Transform> > orderedPoses;
+		for(std::map<int, Transform>::const_iterator iter=poses.upper_bound(lastId); iter!=poses.end(); ++iter)
+		{
+			orderedPoses.push_back(*iter);
+		}
+		// insert negative after
+		for(std::map<int, Transform>::const_iterator iter=poses.begin(); iter!=poses.end(); ++iter)
+		{
+			if(iter->first < 0)
+			{
+				orderedPoses.push_back(*iter);
+			}
+			else
+			{
+				break;
+			}
+		}
+
+		UDEBUG("orderedPoses = %d", (int)orderedPoses.size());
+		for(std::list<std::pair<int, Transform> >::const_iterator iter=orderedPoses.begin(); iter!=orderedPoses.end(); ++iter)
+		{
+			std::map<int, std::pair<pcl::PointCloud<pcl::PointXYZRGB>::Ptr, pcl::PointCloud<pcl::PointXYZRGB>::Ptr> >::iterator cloudIter;
+			cloudIter = cache_.find(iter->first);
+			if(cloudIter != cache_.end())
+			{
+				UDEBUG("Adding %d to octomap (resolution=%f)", iter->first, octree_->getResolution());
+
+				octomap::point3d sensorOrigin(iter->second.x(), iter->second.y(), iter->second.z());
+				octomap::OcTreeKey tmpKey;
+				if (!octree_->coordToKeyChecked(sensorOrigin, tmpKey)
+						|| !octree_->coordToKeyChecked(sensorOrigin, tmpKey))
+				{
+					UERROR("Could not generate Key for origin ", sensorOrigin.x(), sensorOrigin.y(), sensorOrigin.z());
+				}
+
+				// instead of direct scan insertion, compute update to filter ground:
+				octomap::KeySet free_cells, occupied_cells, ground_cells;
+				// insert ground points only as free:
+				UDEBUG("%d: compute free cells (from %d ground points)", iter->first, (int)cloudIter->second.first->size());
+				for (unsigned int i=0; i<cloudIter->second.first->size(); ++i)
+				{
+					pcl::PointXYZRGB pt = util3d::transformPoint(cloudIter->second.first->at(i), iter->second);
+
+					octomap::point3d point(pt.x, pt.y, pt.z);
+
+					// only clear space (ground points)
+					if (octree_->computeRayKeys(sensorOrigin, point, keyRay_))
+					{
+						free_cells.insert(keyRay_.begin(), keyRay_.end());
+					}
+					// occupied endpoint
+					octomap::OcTreeKey key;
+					if (octree_->coordToKeyChecked(point, key))
+					{
+						ground_cells.insert(key);
+
+						octomap::ColorOcTreeNode * n = octree_->updateNode(key, false);
+						if(n)
+						{
+							octree_->averageNodeColor(key, pt.r, pt.g, pt.b);
+							if(iter->first > 0)
+							{
+								uInsert(occupiedCells_, std::make_pair(n, OcTreeNodeInfo(iter->first, key, false)));
+							}
+							else
+							{
+								occupiedCells_.insert(std::make_pair(n, OcTreeNodeInfo(iter->first, key, false)));
+							}
+						}
+					}
+				}
+				UDEBUG("%d: free cells = %d", iter->first, (int)free_cells.size());
+
+				// all other points: free on ray, occupied on endpoint:
+				UDEBUG("%d: compute occupied cells (from %d obstacle points)", iter->first, (int) cloudIter->second.second->size());
+				for (unsigned int i=0; i<cloudIter->second.second->size(); ++i)
+				{
+					pcl::PointXYZRGB pt = util3d::transformPoint(cloudIter->second.second->at(i), iter->second);
+
+					octomap::point3d point(pt.x, pt.y, pt.z);
+
+					// free cells
+					if (octree_->computeRayKeys(sensorOrigin, point, keyRay_))
+					{
+						free_cells.insert(keyRay_.begin(), keyRay_.end());
+					}
+					// occupied endpoint
+					octomap::OcTreeKey key;
+					if (octree_->coordToKeyChecked(point, key))
+					{
+						occupied_cells.insert(key);
+
+						octomap::ColorOcTreeNode * n = octree_->updateNode(key, true);
+						if(n)
+						{
+							octree_->averageNodeColor(key, pt.r, pt.g, pt.b);
+							if(iter->first > 0)
+							{
+								uInsert(occupiedCells_, std::make_pair(n, OcTreeNodeInfo(iter->first, key, true)));
+							}
+							else
+							{
+								occupiedCells_.insert(std::make_pair(n, OcTreeNodeInfo(iter->first, key, true)));
+							}
+						}
+					}
+				}
+				UDEBUG("%d: occupied cells=%d free cells=%d", iter->first, (int)occupied_cells.size(), (int)free_cells.size());
+
+
+				// mark free cells only if not seen occupied in this cloud
+				for(octomap::KeySet::iterator it = free_cells.begin(), end=free_cells.end(); it!= end; ++it)
+				{
+					if (occupied_cells.find(*it) == occupied_cells.end() &&
+						ground_cells.find(*it) == ground_cells.end())
+					{
+						octomap::ColorOcTreeNode * n = octree_->updateNode(*it, false);
+						if(n)
+						{
+							std::map<octomap::ColorOcTreeNode*, OcTreeNodeInfo>::iterator gter;
+							gter = occupiedCells_.find(n);
+							if(gter != occupiedCells_.end() && gter->second.isObstacle_)
+							{
+								occupiedCells_.erase(gter);
+							}
+						}
+					}
+				}
+
+				// compress map
+				//octree_->prune();
+
+				// ignore negative ids as they are temporary clouds
+				if(iter->first > 0)
+				{
+					addedNodes_.insert(*iter);
+				}
+				UDEBUG("%d: end", iter->first);
+			}
+			else
+			{
+				UDEBUG("Did not find %d in cache", iter->first);
+			}
+		}
+	}
+	cache_.clear();
+}
+
+void HSVtoRGB( float *r, float *g, float *b, float h, float s, float v )
+{
+	int i;
+	float f, p, q, t;
+	if( s == 0 ) {
+		// achromatic (grey)
+		*r = *g = *b = v;
+		return;
+	}
+	h /= 60;			// sector 0 to 5
+	i = floor( h );
+	f = h - i;			// factorial part of h
+	p = v * ( 1 - s );
+	q = v * ( 1 - s * f );
+	t = v * ( 1 - s * ( 1 - f ) );
+	switch( i ) {
+		case 0:
+			*r = v;
+			*g = t;
+			*b = p;
+			break;
+		case 1:
+			*r = q;
+			*g = v;
+			*b = p;
+			break;
+		case 2:
+			*r = p;
+			*g = v;
+			*b = t;
+			break;
+		case 3:
+			*r = p;
+			*g = q;
+			*b = v;
+			break;
+		case 4:
+			*r = t;
+			*g = p;
+			*b = v;
+			break;
+		default:		// case 5:
+			*r = v;
+			*g = p;
+			*b = q;
+			break;
+	}
+}
+
+pcl::PointCloud<pcl::PointXYZRGB>::Ptr OctoMap::createCloud(
+		unsigned int treeDepth,
+		std::vector<int> * obstacleIndices,
+		std::vector<int> * emptyIndices) const
+{
+	UASSERT(treeDepth <= octree_->getTreeDepth());
+	pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZRGB>);
+	UDEBUG("depth=%d (maxDepth=%d) octree = %d",
+			(int)treeDepth, (int)octree_->getTreeDepth(), (int)octree_->size());
+	cloud->resize(octree_->size());
+	if(obstacleIndices)
+	{
+		obstacleIndices->resize(octree_->size());
+	}
+	if(emptyIndices)
+	{
+		emptyIndices->resize(octree_->size());
+	}
+
+	if(treeDepth == 0)
+	{
+		treeDepth = octree_->getTreeDepth();
+	}
+
+	double minX, minY, minZ, maxX, maxY, maxZ;
+	octree_->getMetricMin(minX, minY, minZ);
+	octree_->getMetricMax(maxX, maxY, maxZ);
+
+	int oi=0;
+	int si=0;
+	int gi=0;
+	for (octomap::ColorOcTree::iterator it = octree_->begin(treeDepth); it != octree_->end(); ++it)
+	{
+		if(octree_->isNodeOccupied(*it))
+		{
+			octomap::point3d pt = octree_->keyToCoord(it.getKey());
+			if(octree_->getTreeDepth() == it.getDepth())
+			{
+				(*cloud)[oi]  = pcl::PointXYZRGB(it->getColor().r, it->getColor().g, it->getColor().b);
+			}
+			else
+			{
+				// Gradiant color on z axis
+				float H = (maxZ - pt.z())*299.0f/(maxZ-minZ);
+				float r,g,b;
+				HSVtoRGB(&r, &g, &b, H, 1, 1);
+				(*cloud)[oi].r = r*255.0f;
+				(*cloud)[oi].g = g*255.0f;
+				(*cloud)[oi].b = b*255.0f;
+			}
+			(*cloud)[oi].x = pt.x();
+			(*cloud)[oi].y = pt.y();
+			(*cloud)[oi].z = pt.z();
+			if(obstacleIndices)
+			{
+				obstacleIndices->at(si++) = oi;
+			}
+			++oi;
+		}
+		else
+		{
+			octomap::point3d pt = octree_->keyToCoord(it.getKey());
+			(*cloud)[oi]  = pcl::PointXYZRGB(it->getColor().r, it->getColor().g, it->getColor().b);
+			(*cloud)[oi].x = pt.x();
+			(*cloud)[oi].y = pt.y();
+			(*cloud)[oi].z = pt.z();
+			if(emptyIndices)
+			{
+				emptyIndices->at(gi++) = oi;
+			}
+			++oi;
+		}
+	}
+
+	cloud->resize(oi);
+	if(obstacleIndices)
+	{
+		obstacleIndices->resize(si);
+	}
+	if(emptyIndices)
+	{
+		emptyIndices->resize(gi);
+	}
+
+	UDEBUG("");
+	return cloud;
+}
+
+cv::Mat OctoMap::createProjectionMap(float & xMin, float & yMin, float & gridCellSize, float minGridSize)
+{
+	gridCellSize = octree_->getResolution();
+
+	pcl::PointCloud<pcl::PointXYZ>::Ptr ground(new pcl::PointCloud<pcl::PointXYZ>);
+	pcl::PointCloud<pcl::PointXYZ>::Ptr obstacles(new pcl::PointCloud<pcl::PointXYZ>);
+
+	ground->resize(occupiedCells_.size());
+	obstacles->resize(occupiedCells_.size());
+	int gi=0;
+	int oi=0;
+	for(std::map<octomap::ColorOcTreeNode*, OcTreeNodeInfo>::const_iterator iter = occupiedCells_.begin();
+		iter!=occupiedCells_.end();
+		++iter)
+	{
+		if(iter->second.isObstacle_ && octree_->isNodeOccupied(iter->first))
+		{
+			octomap::point3d pt = octree_->keyToCoord(iter->second.key_);
+			(*obstacles)[oi++]  = pcl::PointXYZ(pt.x(), pt.y(), 0); // projected on ground
+		}
+		else if(!iter->second.isObstacle_)
+		{
+			octomap::point3d pt = octree_->keyToCoord(iter->second.key_);
+			(*ground)[gi++]  = pcl::PointXYZ(pt.x(), pt.y(), 0); // projected on ground
+		}
+	}
+	obstacles->resize(oi);
+	ground->resize(gi);
+
+	if(obstacles->size())
+	{
+		obstacles = util3d::voxelize(obstacles, gridCellSize);
+	}
+	if(ground->size())
+	{
+		ground = util3d::voxelize(ground, gridCellSize);
+	}
+
+	cv::Mat obstaclesMat = cv::Mat((int)obstacles->size(), 1, CV_32FC2);
+	for(unsigned int i=0;i<obstacles->size(); ++i)
+	{
+		obstaclesMat.at<cv::Vec2f>(i)[0] = obstacles->at(i).x;
+		obstaclesMat.at<cv::Vec2f>(i)[1] = obstacles->at(i).y;
+	}
+
+	cv::Mat groundMat = cv::Mat((int)ground->size(), 1, CV_32FC2);
+	for(unsigned int i=0;i<ground->size(); ++i)
+	{
+		groundMat.at<cv::Vec2f>(i)[0] = ground->at(i).x;
+		groundMat.at<cv::Vec2f>(i)[1] = ground->at(i).y;
+	}
+
+	std::map<int, Transform> poses;
+	poses.insert(std::make_pair(1, Transform::getIdentity()));
+	std::map<int, std::pair<cv::Mat, cv::Mat> > maps;
+	maps.insert(std::make_pair(1, std::make_pair(groundMat, obstaclesMat)));
+
+	return util3d::create2DMapFromOccupancyLocalMaps(
+			poses,
+			maps,
+			gridCellSize,
+			xMin, yMin,
+			minGridSize,
+			false);
+}
+
+bool OctoMap::writeBinary(const std::string & path)
+{
+	return octree_->writeBinary(path);
+}
+
+} /* namespace rtabmap */

corelib/src/Odometry.cpp

@@ -1,5 +1,5 @@
 /*
-Copyright (c) 2010-2014, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
+Copyright (c) 2010-2016, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
 All rights reserved.
 
 Redistribution and use in source and binary forms, with or without
@@ -29,12 +29,17 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #include "rtabmap/core/Odometry.h"
 #include "rtabmap/core/OdometryF2F.h"
 #include "rtabmap/core/OdometryInfo.h"
+#include "rtabmap/core/util3d.h"
+#include "rtabmap/core/util3d_mapping.h"
+#include "rtabmap/core/util3d_filtering.h"
 #include "rtabmap/utilite/ULogger.h"
 #include "rtabmap/utilite/UTimer.h"
 #include "rtabmap/utilite/UConversion.h"
 #include "rtabmap/core/ParticleFilter.h"
 #include "rtabmap/core/util2d.h"
 
+#include <pcl/pcl_base.h>
+
 namespace rtabmap {
 
 Odometry * Odometry::create(const ParametersMap & parameters)
@@ -77,6 +82,8 @@ Odometry::Odometry(const rtabmap::ParametersMap & parameters) :
 		_kalmanProcessNoise(Parameters::defaultOdomKalmanProcessNoise()),
 		_kalmanMeasurementNoise(Parameters::defaultOdomKalmanMeasurementNoise()),
 		_imageDecimation(Parameters::defaultOdomImageDecimation()),
+		_alignWithGround(Parameters::defaultOdomAlignWithGround()),
+		_pose(Transform::getIdentity()),
 		_resetCurrentCount(0),
 		previousStamp_(0),
 		distanceTravelled_(0)
@@ -100,6 +107,7 @@ Odometry::Odometry(const rtabmap::ParametersMap & parameters) :
 	Parameters::parse(parameters, Parameters::kOdomKalmanProcessNoise(), _kalmanProcessNoise);
 	Parameters::parse(parameters, Parameters::kOdomKalmanMeasurementNoise(), _kalmanMeasurementNoise);
 	Parameters::parse(parameters, Parameters::kOdomImageDecimation(), _imageDecimation);
+	Parameters::parse(parameters, Parameters::kOdomAlignWithGround(), _alignWithGround);
 	UASSERT(_imageDecimation>=1);
 
 	if(_filteringStrategy == 2)
@@ -135,6 +143,7 @@ Odometry::~Odometry()
 
 void Odometry::reset(const Transform & initialPose)
 {
+	UASSERT(!initialPose.isNull());
 	previousVelocityTransform_.setNull();
 	previousGroundTruthPose_.setNull();
 	_resetCurrentCount = 0;
@@ -186,13 +195,69 @@ void Odometry::reset(const Transform & initialPose)
 
 Transform Odometry::process(SensorData & data, OdometryInfo * info)
 {
-	if(_pose.isNull())
+	return process(data, Transform(), info);
-	{
+}
-		_pose.setIdentity(); // initialized
-	}
 
+Transform Odometry::process(SensorData & data, const Transform & guessIn, OdometryInfo * info)
+{
 	UASSERT(!data.imageRaw().empty());
 
+	// Ground alignment
+	if(_pose.isIdentity() && _alignWithGround)
+	{
+		UTimer alignTimer;
+		pcl::IndicesPtr indices(new std::vector<int>);
+		pcl::IndicesPtr ground, obstacles;
+		pcl::PointCloud<pcl::PointXYZ>::Ptr cloud = util3d::cloudFromSensorData(data, 1, 0, 0, indices.get());
+		cloud = util3d::voxelize(cloud, indices, 0.01);
+		bool success = false;
+		if(cloud->size())
+		{
+			util3d::segmentObstaclesFromGround<pcl::PointXYZ>(cloud, ground, obstacles, 20, M_PI/4.0f, 0.02, 200, true);
+			if(ground->size())
+			{
+				pcl::ModelCoefficients coefficients;
+				util3d::extractPlane(cloud, ground, 0.02, 100, &coefficients);
+				if(coefficients.values.at(3) >= 0)
+				{
+					UWARN("Ground detected! coefficients=(%f, %f, %f, %f) time=%fs",
+							coefficients.values.at(0),
+							coefficients.values.at(1),
+							coefficients.values.at(2),
+							coefficients.values.at(3),
+							alignTimer.ticks());
+				}
+				else
+				{
+					UWARN("Ceiling detected! coefficients=(%f, %f, %f, %f) time=%fs",
+							coefficients.values.at(0),
+							coefficients.values.at(1),
+							coefficients.values.at(2),
+							coefficients.values.at(3),
+							alignTimer.ticks());
+				}
+				Eigen::Vector3f n(coefficients.values.at(0), coefficients.values.at(1), coefficients.values.at(2));
+				Eigen::Vector3f z(0,0,1);
+				//get rotation from z to n;
+				Eigen::Matrix3f R;
+				R = Eigen::Quaternionf().setFromTwoVectors(n,z);
+				Transform rotation(
+						R(0,0), R(0,1), R(0,2), 0,
+						R(1,0), R(1,1), R(1,2), 0,
+						R(2,0), R(2,1), R(2,2), coefficients.values.at(3));
+				_pose *= rotation;
+				success = true;
+			}
+		}
+		if(!success)
+		{
+			UERROR("Odometry failed to detect the ground. You have this "
+					"error because parameter \"Odom/AlignWithGround\" is true. "
+					"Make sure the camera is seeing the ground (e.g., tilt ~30 "
+					"degrees toward the ground).");
+		}
+	}
+
 	if(!data.stereoCameraModel().isValidForProjection() &&
 	   (data.cameraModels().size() == 0 || !data.cameraModels()[0].isValidForProjection()))
 	{
@@ -201,8 +266,8 @@ Transform Odometry::process(SensorData & data, OdometryInfo * info)
 	}
 
 	double dt = previousStamp_>0.0f?data.stamp() - previousStamp_:0.0;
-	Transform guess = dt && guessFromMotion_?Transform::getIdentity():Transform();
+	Transform guess = dt && guessFromMotion_ && !previousVelocityTransform_.isNull()?Transform::getIdentity():Transform();
-	UASSERT(dt>0.0 || (dt == 0.0 && previousVelocityTransform_.isNull()));
+	UASSERT_MSG(dt>0.0 || (dt == 0.0 && previousVelocityTransform_.isNull()), uFormat("dt=%f previous transform=%s", dt, previousVelocityTransform_.prettyPrint().c_str()).c_str());
 	if(!previousVelocityTransform_.isNull())
 	{
 		if(guessFromMotion_)
@@ -227,6 +292,11 @@ Transform Odometry::process(SensorData & data, OdometryInfo * info)
 		}
 	}
 
+	if(!guessIn.isNull())
+	{
+		guess = guessIn;
+	}
+
 	UTimer time;
 	Transform t;
 	if(_imageDecimation > 1)
@@ -438,7 +508,7 @@ Transform Odometry::process(SensorData & data, OdometryInfo * info)
 			info->distanceTravelled = distanceTravelled_;
 		}
 
-		return _pose *= t; // updated
+		return _pose *= t; // update
 	}
 	else if(_resetCurrentCount > 0)
 	{
@@ -491,7 +561,7 @@ void Odometry::initKalmanFilter(const Transform & initialPose, float vx, float v
 	    0, 0, 0, 0, 0, 0.09 } };
 	 */
 
-
+
 	kalmanFilter_.init(nStates, nMeasurements, nInputs);                 // init Kalman Filter
 	cv::setIdentity(kalmanFilter_.processNoiseCov, cv::Scalar::all(_kalmanProcessNoise));  // set process noise
 	cv::setIdentity(kalmanFilter_.measurementNoiseCov, cv::Scalar::all(_kalmanMeasurementNoise));   // set measurement noise

corelib/src/OdometryF2F.cpp

@@ -1,5 +1,5 @@
 /*
-Copyright (c) 2010-2014, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
+Copyright (c) 2010-2016, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
 All rights reserved.
 
 Redistribution and use in source and binary forms, with or without
@@ -176,6 +176,12 @@ Transform OdometryF2F::computeTransform(
 			}
 			else
 			{
+				if (!refFrame_.sensorData().isValid())
+				{
+					// Don't send odometry if we don't have a keyframe yet
+					output.setNull();
+				}
+
 				if(features < registrationPipeline_->getMinVisualCorrespondences())
 				{
 					UWARN("Too low 2D features (%d), keeping last key frame...", features);

corelib/src/OdometryF2M.cpp

@@ -1,5 +1,5 @@
 /*
-Copyright (c) 2010-2014, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
+Copyright (c) 2010-2016, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
 All rights reserved.
 
 Redistribution and use in source and binary forms, with or without
@@ -44,6 +44,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #include "rtabmap/utilite/UConversion.h"
 #include <opencv2/calib3d/calib3d.hpp>
 #include <rtabmap/core/OdometryF2M.h>
+#include <pcl/common/io.h>
 
 #if _MSC_VER
 	#define ISFINITE(value) _finite(value)
@@ -60,7 +61,7 @@ OdometryF2M::OdometryF2M(const ParametersMap & parameters) :
 	maxNewFeatures_(Parameters::defaultOdomF2MMaxNewFeatures()),
 	scanKeyFrameThr_(Parameters::defaultOdomScanKeyFrameThr()),
 	scanMaximumMapSize_(Parameters::defaultOdomF2MScanMaxSize()),
-	scanSubstractRadius_(Parameters::defaultOdomF2MScanSubstractRadius()),
+	scanSubtractRadius_(Parameters::defaultOdomF2MScanSubtractRadius()),
 	fixedMapPath_(Parameters::defaultOdomF2MFixedMapPath()),
 	regPipeline_(Registration::create(parameters)),
 	map_(new Signature(-1)),
@@ -72,7 +73,7 @@ OdometryF2M::OdometryF2M(const ParametersMap & parameters) :
 	Parameters::parse(parameters, Parameters::kOdomF2MMaxNewFeatures(), maxNewFeatures_);
 	Parameters::parse(parameters, Parameters::kOdomScanKeyFrameThr(), scanKeyFrameThr_);
 	Parameters::parse(parameters, Parameters::kOdomF2MScanMaxSize(), scanMaximumMapSize_);
-	Parameters::parse(parameters, Parameters::kOdomF2MScanSubstractRadius(), scanSubstractRadius_);
+	Parameters::parse(parameters, Parameters::kOdomF2MScanSubtractRadius(), scanSubtractRadius_);
 	Parameters::parse(parameters, Parameters::kOdomF2MFixedMapPath(), fixedMapPath_);
 	UASSERT(maximumMapSize_ >= 0);
 	UASSERT(keyFrameThr_ >= 0.0f && keyFrameThr_<=1.0f);
@@ -327,55 +328,115 @@ Transform OdometryF2M::computeTransform(
 						(scanKeyFrameThr_==0 || regInfo.icpInliersRatio <= scanKeyFrameThr_))
 					{
 						UINFO("Update local scan map %d (ratio=%f < %f)", lastFrame_->id(), regInfo.icpInliersRatio, scanKeyFrameThr_);
-						pcl::PointCloud<pcl::PointNormal>::Ptr mapCloudNormals = util3d::laserScanToPointCloudNormal(mapScan);
-						pcl::PointCloud<pcl::PointNormal>::Ptr frameCloudNormals = util3d::laserScanToPointCloudNormal(lastFrame_->sensorData().laserScanRaw(), newFramePose);
 
-						if(mapCloudNormals->size() && scanSubstractRadius_ > 0.0f)
+						UTimer tmpTimer;
+						if(lastFrame_->sensorData().laserScanRaw().cols)
 						{
-							frameCloudNormals = util3d::subtractFiltering(frameCloudNormals, mapCloudNormals, scanSubstractRadius_, 0.0f);
+							pcl::PointCloud<pcl::PointNormal>::Ptr mapCloudNormals = util3d::laserScanToPointCloudNormal(mapScan);
-						}
+							pcl::PointCloud<pcl::PointNormal>::Ptr frameCloudNormals = util3d::laserScanToPointCloudNormal(lastFrame_->sensorData().laserScanRaw(), newFramePose);
-						if(frameCloudNormals->size())
-						{
-							scansBuffer_.insert(std::make_pair(lastFrame_->id(), frameCloudNormals));
 
-							//remove points if too big
+							pcl::IndicesPtr frameCloudNormalsIndices(new std::vector<int>);
-							UDEBUG("scansBuffer=%d, mapSize=%d maxPoints=%d", (int)scansBuffer_.size(), int(mapCloudNormals->size() + frameCloudNormals->size()), scanMaximumMapSize_);
+							int newPoints;
-							if(scansBuffer_.size() > 1 && int(mapCloudNormals->size() + frameCloudNormals->size()) > scanMaximumMapSize_)
+							if(mapCloudNormals->size() && scanSubtractRadius_ > 0.0f)
 							{
-								//asssemble
+								frameCloudNormalsIndices = util3d::subtractFiltering(
-								mapCloudNormals->clear();
+										frameCloudNormals,
-								std::list<int> toRemove;
+										pcl::IndicesPtr(new std::vector<int>),
-								for(std::map<int, pcl::PointCloud<pcl::PointNormal>::Ptr>::reverse_iterator iter=scansBuffer_.rbegin();
+										mapCloudNormals,
-									iter!=scansBuffer_.rend();
+										pcl::IndicesPtr(new std::vector<int>),
-									++iter)
+										scanSubtractRadius_,
-								{
+										0.0f);
-									if(mapCloudNormals->empty())
+								newPoints = frameCloudNormalsIndices->size();
-									{
-										*mapCloudNormals = *iter->second;
-									}
-									else if((int)mapCloudNormals->size() < scanMaximumMapSize_)
-									{
-										*mapCloudNormals += *iter->second;
-									}
-									else
-									{
-										toRemove.push_back(iter->first);
-									}
-								}
-								for(std::list<int>::iterator iter=toRemove.begin(); iter!=toRemove.end(); ++iter)
-								{
-									scansBuffer_.erase(*iter);
-								}
 							}
 							else
 							{
-								//assemble
+								newPoints = mapCloudNormals->size();
-								*mapCloudNormals += *frameCloudNormals;
 							}
 
-							mapScan = util3d::laserScanFromPointCloud(*mapCloudNormals);
+							if(newPoints)
-							modified=true;
+							{
+								scansBuffer_.push_back(std::make_pair(frameCloudNormals, frameCloudNormalsIndices));
+
+								//remove points if too big
+								UDEBUG("scansBuffer=%d, mapSize=%d newPoints=%d maxPoints=%d",
+										(int)scansBuffer_.size(),
+										int(mapCloudNormals->size()),
+										newPoints,
+										scanMaximumMapSize_);
+
+								if(newPoints < 20)
+								{
+									UWARN("The number of new scan points added to local odometry "
+										  "map is low (%d), you may want to decrease the parameter \"%s\" "
+										  "(current value=%f and ICP inliers ratio is %f)",
+											newPoints,
+											Parameters::kOdomScanKeyFrameThr().c_str(),
+											scanKeyFrameThr_,
+											regInfo.icpInliersRatio);
+								}
+
+								if(scansBuffer_.size() > 1 &&
+									int(mapCloudNormals->size() + newPoints) > scanMaximumMapSize_)
+								{
+									//regenerate the local map
+									mapCloudNormals->clear();
+									std::list<int> toRemove;
+									int i = int(scansBuffer_.size())-1;
+									for(; i>=0; --i)
+									{
+										int pointsToAdd = scansBuffer_[i].second->size()?scansBuffer_[i].second->size():scansBuffer_[i].first->size();
+										if((int)mapCloudNormals->size() + pointsToAdd > scanMaximumMapSize_ ||
+											i == 0)
+										{
+											*mapCloudNormals += *scansBuffer_[i].first;
+											break;
+										}
+										else
+										{
+											if(scansBuffer_[i].second->size())
+											{
+												pcl::PointCloud<pcl::PointNormal> tmp;
+												pcl::copyPointCloud(*scansBuffer_[i].first, *scansBuffer_[i].second, tmp);
+												*mapCloudNormals += tmp;
+											}
+											else
+											{
+												*mapCloudNormals += *scansBuffer_[i].first;
+											}
+										}
+									}
+									// remove old clouds
+									if(i > 0)
+									{
+										std::vector<std::pair<pcl::PointCloud<pcl::PointNormal>::Ptr, pcl::IndicesPtr> > scansTmp(scansBuffer_.size()-i);
+										int oi = 0;
+										for(; i<(int)scansBuffer_.size(); ++i)
+										{
+											UASSERT(oi < (int)scansTmp.size());
+											scansTmp[oi++] = scansBuffer_[i];
+										}
+										scansBuffer_ = scansTmp;
+									}
+								}
+								else
+								{
+									// just append the last cloud
+									if(scansBuffer_.back().second->size())
+									{
+										pcl::PointCloud<pcl::PointNormal> tmp;
+										pcl::copyPointCloud(*scansBuffer_.back().first, *scansBuffer_.back().second, tmp);
+										*mapCloudNormals += tmp;
+									}
+									else
+									{
+										*mapCloudNormals += *scansBuffer_.back().first;
+									}
+								}
+								mapScan = util3d::laserScanFromPointCloud(*mapCloudNormals);
+								modified=true;
+							}
 						}
+						UDEBUG("Update local map = %fs", tmpTimer.ticks());
 					}
 
 					if(modified)
@@ -419,51 +480,75 @@ Transform OdometryF2M::computeTransform(
 
 			data.setFeatures(lastFrame_->sensorData().keypoints(), lastFrame_->sensorData().descriptors());
 
-			if(fixedMapPath_.empty())
+			// a very high variance tells that the new pose is not linked with the previous one
+			regInfo.variance = 9999;
+
+			bool frameValid = false;
+			Transform newFramePose = this->getPose(); // initial pose may be not identity...
+			if(regPipeline_->isImageRequired())
 			{
-				output.setIdentity();
+				if ((int)lastFrame_->getWords3().size() >= regPipeline_->getMinVisualCorrespondences())
-				// a very high variance tells that the new pose is not linked with the previous one
-				regInfo.variance = 9999;
-
-				Transform newFramePose = this->getPose(); // initial pose may be not identity...
-				if(regPipeline_->isImageRequired() &&
-				   (int)lastFrame_->getWords3().size() >= regPipeline_->getMinVisualCorrespondences())
 				{
-					// update local map
+					frameValid = true;
-					UASSERT_MSG(lastFrame_->getWordsDescriptors().size() == lastFrame_->getWords3().size(), uFormat("%d vs %d", lastFrame_->getWordsDescriptors().size(), lastFrame_->getWords3().size()).c_str());
+					if (fixedMapPath_.empty())
-					UASSERT(lastFrame_->getWords3().size() == lastFrame_->getWords().size());
-
-					std::multimap<int, cv::KeyPoint> words;
-					std::multimap<int, cv::Point3f> transformedPoints;
-					std::multimap<int, cv::Mat> descriptors;
-					UASSERT(lastFrame_->getWords3().size() == lastFrame_->getWordsDescriptors().size());
-					std::multimap<int, cv::KeyPoint>::const_iterator wordsIter = lastFrame_->getWords().begin();
-					std::multimap<int, cv::Mat>::const_iterator descIter = lastFrame_->getWordsDescriptors().begin();
-					for(std::multimap<int, cv::Point3f>::const_iterator iter = lastFrame_->getWords3().begin();
-							iter!=lastFrame_->getWords3().end();
-							++iter,++descIter,++wordsIter)
 					{
-						if(util3d::isFinite(iter->second))
+						// update local map
-						{
+						UASSERT_MSG(lastFrame_->getWordsDescriptors().size() == lastFrame_->getWords3().size(), uFormat("%d vs %d", lastFrame_->getWordsDescriptors().size(), lastFrame_->getWords3().size()).c_str());
-							words.insert(*wordsIter);
+						UASSERT(lastFrame_->getWords3().size() == lastFrame_->getWords().size());
-							transformedPoints.insert(std::make_pair(iter->first, util3d::transformPoint(iter->second, newFramePose)));
-							descriptors.insert(*descIter);
-						}
-					}
-					map_->setWords(words);
-					map_->setWords3(transformedPoints);
-					map_->setWordsDescriptors(descriptors);
 
-					map_->sensorData().setCameraModels(lastFrame_->sensorData().cameraModels());
+						std::multimap<int, cv::KeyPoint> words;
-					map_->sensorData().setStereoCameraModel(lastFrame_->sensorData().stereoCameraModel());
+						std::multimap<int, cv::Point3f> transformedPoints;
+						std::multimap<int, cv::Mat> descriptors;
+						UASSERT(lastFrame_->getWords3().size() == lastFrame_->getWordsDescriptors().size());
+						std::multimap<int, cv::KeyPoint>::const_iterator wordsIter = lastFrame_->getWords().begin();
+						std::multimap<int, cv::Mat>::const_iterator descIter = lastFrame_->getWordsDescriptors().begin();
+						for (std::multimap<int, cv::Point3f>::const_iterator iter = lastFrame_->getWords3().begin();
+							iter != lastFrame_->getWords3().end();
+							++iter, ++descIter, ++wordsIter)
+						{
+							if (util3d::isFinite(iter->second))
+							{
+								words.insert(*wordsIter);
+								transformedPoints.insert(std::make_pair(iter->first, util3d::transformPoint(iter->second, newFramePose)));
+								descriptors.insert(*descIter);
+							}
+						}
+						map_->setWords(words);
+						map_->setWords3(transformedPoints);
+						map_->setWordsDescriptors(descriptors);
+
+						map_->sensorData().setCameraModels(lastFrame_->sensorData().cameraModels());
+						map_->sensorData().setStereoCameraModel(lastFrame_->sensorData().stereoCameraModel());
+					}
 				}
-				if(regPipeline_->isScanRequired())
+				else
 				{
-					pcl::PointCloud<pcl::PointNormal>::Ptr mapCloudNormals = util3d::laserScanToPointCloudNormal(lastFrame_->sensorData().laserScanRaw(), newFramePose);
+					UWARN("%d visual features required to initialize the odometry (only %d extracted).", regPipeline_->getMinVisualCorrespondences(), (int)lastFrame_->getWords3().size());
-					scansBuffer_.insert(std::make_pair(lastFrame_->id(), mapCloudNormals));
-					map_->sensorData().setLaserScanRaw(util3d::laserScanFromPointCloud(*mapCloudNormals), 0,0);
 				}
 			}
+			if(regPipeline_->isScanRequired())
+			{
+				if (lastFrame_->sensorData().laserScanRaw().cols)
+				{
+					frameValid = true;
+					if (fixedMapPath_.empty())
+					{
+						pcl::PointCloud<pcl::PointNormal>::Ptr mapCloudNormals = util3d::laserScanToPointCloudNormal(lastFrame_->sensorData().laserScanRaw(), newFramePose);
+						scansBuffer_.push_back(std::make_pair(mapCloudNormals, pcl::IndicesPtr(new std::vector<int>)));
+						map_->sensorData().setLaserScanRaw(util3d::laserScanFromPointCloud(*mapCloudNormals), 0,0);
+					}
+				}
+				else
+				{
+					UWARN("Missing scan to initialize odometry.");
+				}
+			}
+
+			if (frameValid)
+			{
+				// We initialized the local map
+				output.setIdentity();
+			}
 
 			if(info)
 			{

corelib/src/OdometryMono.cpp

@@ -1,5 +1,5 @@
 /*
-Copyright (c) 2010-2014, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
+Copyright (c) 2010-2016, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
 All rights reserved.
 
 Redistribution and use in source and binary forms, with or without

corelib/src/OdometryThread.cpp

@@ -1,5 +1,5 @@
 /*
-Copyright (c) 2010-2014, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
+Copyright (c) 2010-2016, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
 All rights reserved.
 
 Redistribution and use in source and binary forms, with or without

corelib/src/Optimizer.cpp

@@ -1,5 +1,5 @@
 /*
-Copyright (c) 2010-2014, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
+Copyright (c) 2010-2016, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
 All rights reserved.
 
 Redistribution and use in source and binary forms, with or without

corelib/src/OptimizerCVSBA.cpp

@@ -1,5 +1,5 @@
 /*
-Copyright (c) 2010-2014, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
+Copyright (c) 2010-2016, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
 All rights reserved.
 
 Redistribution and use in source and binary forms, with or without

corelib/src/OptimizerG2O.cpp

@@ -1,5 +1,5 @@
 /*
-Copyright (c) 2010-2014, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
+Copyright (c) 2010-2016, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
 All rights reserved.
 
 Redistribution and use in source and binary forms, with or without

corelib/src/OptimizerGTSAM.cpp

@@ -1,5 +1,5 @@
 /*
-Copyright (c) 2010-2014, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
+Copyright (c) 2010-2016, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
 All rights reserved.
 
 Redistribution and use in source and binary forms, with or without