0.17.1: Fixed issue #282 and https://github.com/introlab/rtabmap_ros/issues/243

CMakeLists.txt

@@ -21,7 +21,7 @@ SET(CMAKE_MODULE_PATH "${PROJECT_SOURCE_DIR}/cmake_modules")
 #######################
 SET(RTABMAP_MAJOR_VERSION 0)
 SET(RTABMAP_MINOR_VERSION 17)
-SET(RTABMAP_PATCH_VERSION 0)
+SET(RTABMAP_PATCH_VERSION 1)
 SET(RTABMAP_VERSION
   ${RTABMAP_MAJOR_VERSION}.${RTABMAP_MINOR_VERSION}.${RTABMAP_PATCH_VERSION})
   

corelib/include/rtabmap/core/CameraRGBD.h

@@ -39,9 +39,14 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #include <pcl/pcl_config.h>
 
 #ifdef HAVE_OPENNI
+#if __linux__ && __i386__ && __cplusplus >= 201103L
+#warning "Openni driver is not available on i386 when building with c++11 support"
+#else
+#define RTABMAP_OPENNI
 #include <pcl/io/openni_camera/openni_depth_image.h>
 #include <pcl/io/openni_camera/openni_image.h>
 #endif
+#endif
 
 #include <boost/signals2/connection.hpp>
 
@@ -102,7 +107,7 @@ public:
 			float imageRate = 0,
 			const Transform & localTransform = Transform::getIdentity());
 	virtual ~CameraOpenni();
-#ifdef HAVE_OPENNI
+#ifdef RTABMAP_OPENNI
     void image_cb (
     		const boost::shared_ptr<openni_wrapper::Image>& rgb,
 			const boost::shared_ptr<openni_wrapper::DepthImage>& depth,

corelib/include/rtabmap/core/DBDriver.h

@@ -155,7 +155,7 @@ public:
 	void executeNoResult(const std::string & sql) const;
 
 	// Load objects
-	void load(VWDictionary * dictionary) const;
+	void load(VWDictionary * dictionary, bool lastStateOnly = true) const;
 	void loadLastNodes(std::list<Signature *> & signatures) const;
 	void loadSignatures(const std::list<int> & ids, std::list<Signature *> & signatures, std::set<int> * loadedFromTrash = 0);
 	void loadWords(const std::set<int> & wordIds, std::list<VisualWord *> & vws);
@@ -255,7 +255,7 @@ private:
 				cv::Mat * textures) const = 0;
 
 	// Load objects
-	virtual void loadQuery(VWDictionary * dictionary) const = 0;
+	virtual void loadQuery(VWDictionary * dictionary, bool lastStateOnly = true) const = 0;
 	virtual void loadLastNodesQuery(std::list<Signature *> & signatures) const = 0;
 	virtual void loadSignaturesQuery(const std::list<int> & ids, std::list<Signature *> & signatures) const = 0;
 	virtual void loadWordsQuery(const std::set<int> & wordIds, std::list<VisualWord *> & vws) const = 0;

corelib/src/CameraRGBD.cpp

@@ -39,7 +39,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #include <rtabmap/utilite/UTimer.h>
 #include <rtabmap/utilite/UMath.h>
 
-#ifdef HAVE_OPENNI
+#ifdef RTABMAP_OPENNI
 #include <pcl/io/openni_grabber.h>
 #include <pcl/io/oni_grabber.h>
 #include <pcl/io/openni_camera/openni_depth_image.h>
@@ -97,7 +97,7 @@ CameraOpenni::CameraOpenni(const std::string & deviceId, float imageRate, const
 
 bool CameraOpenni::available() 
 {
-#ifdef HAVE_OPENNI
+#ifdef RTABMAP_OPENNI
 	return true;
 #else
 	return false;
@@ -106,7 +106,7 @@ bool CameraOpenni::available()
 
 CameraOpenni::~CameraOpenni()
 {
-#ifdef HAVE_OPENNI
+#ifdef RTABMAP_OPENNI
 	UDEBUG("");
 	if(connection_.connected())
 	{
@@ -122,7 +122,7 @@ CameraOpenni::~CameraOpenni()
 	}
 #endif
 }
-#ifdef HAVE_OPENNI
+#ifdef RTABMAP_OPENNI
 void CameraOpenni::image_cb (
 		const boost::shared_ptr<openni_wrapper::Image>& rgb,
 		const boost::shared_ptr<openni_wrapper::DepthImage>& depth,
@@ -150,7 +150,7 @@ void CameraOpenni::image_cb (
 
 bool CameraOpenni::init(const std::string & calibrationFolder, const std::string & cameraName)
 {
-#ifdef HAVE_OPENNI
+#ifdef RTABMAP_OPENNI
 	if(interface_)
 	{
 		interface_->stop();
@@ -197,7 +197,7 @@ bool CameraOpenni::init(const std::string & calibrationFolder, const std::string
 
 bool CameraOpenni::isCalibrated() const
 {
-#ifdef HAVE_OPENNI
+#ifdef RTABMAP_OPENNI
 	return true;
 #else
 	return false;
@@ -206,7 +206,7 @@ bool CameraOpenni::isCalibrated() const
 
 std::string CameraOpenni::getSerial() const
 {
-#ifdef HAVE_OPENNI
+#ifdef RTABMAP_OPENNI
 	if(interface_)
 	{
 		return interface_->getName();
@@ -218,7 +218,7 @@ std::string CameraOpenni::getSerial() const
 SensorData CameraOpenni::captureImage(CameraInfo * info)
 {
 	SensorData data;
-#ifdef HAVE_OPENNI
+#ifdef RTABMAP_OPENNI
 	if(interface_ && interface_->isRunning())
 	{
 		if(!dataReady_.acquire(1, 5000))

corelib/src/DBDriver.cpp

@@ -436,7 +436,7 @@ void DBDriver::saveOrUpdate(const std::vector<Signature *> & signatures)
 
 void DBDriver::saveOrUpdate(const std::vector<VisualWord *> & words) const
 {
-	ULOGGER_DEBUG("");
+	ULOGGER_DEBUG("words.size=%d", (int)words.size());
 	std::list<VisualWord *> toSave;
 	std::list<VisualWord *> toUpdate;
 	if(this->isConnected() && words.size())
@@ -511,10 +511,10 @@ void DBDriver::updateDepthImage(int nodeId, const cv::Mat & image)
 	_dbSafeAccessMutex.unlock();
 }
 
-void DBDriver::load(VWDictionary * dictionary) const
+void DBDriver::load(VWDictionary * dictionary, bool lastStateOnly) const
 {
 	_dbSafeAccessMutex.lock();
-	this->loadQuery(dictionary);
+	this->loadQuery(dictionary, lastStateOnly);
 	_dbSafeAccessMutex.unlock();
 }
 

corelib/src/DBDriverSqlite3.cpp

@@ -3077,7 +3077,7 @@ void DBDriverSqlite3::loadLastNodesQuery(std::list<Signature *> & nodes) const
 	}
 }
 
-void DBDriverSqlite3::loadQuery(VWDictionary * dictionary) const
+void DBDriverSqlite3::loadQuery(VWDictionary * dictionary, bool lastStateOnly) const
 {
 	ULOGGER_DEBUG("");
 	if(_ppDb && dictionary)
@@ -3087,26 +3087,25 @@ void DBDriverSqlite3::loadQuery(VWDictionary * dictionary) const
 		timer.start();
 		int rc = SQLITE_OK;
 		sqlite3_stmt * ppStmt = 0;
-		std::string query;
+		std::stringstream query;
 		std::list<VisualWord *> visualWords;
 
 		// Get the visual words
+		query << "SELECT id, descriptor_size, descriptor FROM Word ";
+		if(lastStateOnly)
+		{
 			if(uStrNumCmp(_version, "0.11.11") >= 0)
 			{
-			query = "SELECT id, descriptor_size, descriptor "
+				query << "WHERE time_enter >= (SELECT MAX(time_enter) FROM Info) ";
-					"FROM Word "
-					"WHERE time_enter >= (SELECT MAX(time_enter) FROM Info) "
-					"ORDER BY id;";
 			}
 			else
 			{
-			query = "SELECT id, descriptor_size, descriptor "
+				query << "WHERE time_enter >= (SELECT MAX(time_enter) FROM Statistics) ";
-					"FROM Word "
-					"WHERE time_enter >= (SELECT MAX(time_enter) FROM Statistics) "
-					"ORDER BY id;";
 			}
+		}
+		query << "ORDER BY id;";
 
-		rc = sqlite3_prepare_v2(_ppDb, query.c_str(), -1, &ppStmt, 0);
+		rc = sqlite3_prepare_v2(_ppDb, query.str().c_str(), -1, &ppStmt, 0);
 		UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
 
 		// Process the result if one

corelib/src/DBDriverSqlite3.h

@@ -124,7 +124,7 @@ private:
 			cv::Mat * textures) const;
 
 	// Load objects
-	virtual void loadQuery(VWDictionary * dictionary) const;
+	virtual void loadQuery(VWDictionary * dictionary, bool lastStateOnly = true) const;
 	virtual void loadLastNodesQuery(std::list<Signature *> & signatures) const;
 	virtual void loadSignaturesQuery(const std::list<int> & ids, std::list<Signature *> & signatures) const;
 	virtual void loadWordsQuery(const std::set<int> & wordIds, std::list<VisualWord *> & vws) const;

corelib/src/Memory.cpp

@@ -279,6 +279,8 @@ void Memory::loadDataFromDb(bool postInitClosingEvents)
 			}
 
 			UDEBUG("load words %d", (int)wordIds.size());
+			if(_vwd->isIncremental())
+			{
 				if(wordIds.size())
 				{
 					std::list<VisualWord*> words;
@@ -294,10 +296,15 @@ void Memory::loadDataFromDb(bool postInitClosingEvents)
 				}
 			}
 			else
+			{
+				_dbDriver->load(_vwd, false);
+			}
+		}
+		else
 		{
 			UDEBUG("load words");
 			// load the last dictionary
-			_dbDriver->load(_vwd);
+			_dbDriver->load(_vwd, _vwd->isIncremental());
 		}
 		UDEBUG("%d words loaded!", _vwd->getUnusedWordsSize());
 		_vwd->update();
@@ -649,7 +656,10 @@ void Memory::parseParameters(const ParametersMap & parameters)
 void Memory::preUpdate()
 {
 	_signaturesAdded = 0;
+	if(_vwd->isIncremental())
+	{
 		this->cleanUnusedWords();
+	}
 	if(_vwd && !_parallelized)
 	{
 		//When parallelized, it is done in CreateSignature
@@ -1584,7 +1594,7 @@ std::map<int, float> Memory::computeLikelihood(const Signature * signature, cons
 				{
 					// "Inverted index" - Pour chaque endroit contenu dans chaque mot
 					vw = _vwd->getWord(*i);
-					UASSERT(vw!=0);
+					UASSERT_MSG(vw!=0, uFormat("Word %d not found in dictionary!?", *i).c_str());
 
 					const std::map<int, int> & refs = vw->getReferences();
 					nw = refs.size();
@@ -2055,7 +2065,7 @@ void Memory::moveToTrash(Signature * s, bool keepLinkedToGraph, std::list<int> *
 		}
 
 		this->disableWordsRef(s->id());
-		if(!keepLinkedToGraph)
+		if(!keepLinkedToGraph && _vwd->isIncremental())
 		{
 			std::list<int> keys = uUniqueKeys(s->getWords());
 			for(std::list<int>::const_iterator i=keys.begin(); i!=keys.end(); ++i)
@@ -4216,8 +4226,6 @@ void Memory::disableWordsRef(int signatureId)
 }
 
 void Memory::cleanUnusedWords()
-{
-	if(_vwd->isIncremental())
 {
 	std::vector<VisualWord*> removedWords = _vwd->getUnusedWords();
 	UDEBUG("Removing %d words (dictionary size=%d)...", removedWords.size(), _vwd->getVisualWords().size());
@@ -4239,7 +4247,6 @@ void Memory::cleanUnusedWords()
 		}
 	}
 }
-}
 
 void Memory::enableWordsRef(const std::list<int> & signatureIds)
 {
@@ -4270,6 +4277,11 @@ void Memory::enableWordsRef(const std::list<int> & signatureIds)
 		}
 	}
 
+	if(!_vwd->isIncremental() && oldWordIds.size())
+	{
+		UWARN("Dictionary is fixed, but some words retrieved have not been found!?");
+	}
+
 	UDEBUG("oldWordIds.size()=%d, getOldIds time=%fs", oldWordIds.size(), timer.ticks());
 
 	// the words were deleted, so try to math it with an active word

corelib/src/Parameters.cpp

@@ -136,7 +136,8 @@ ParametersMap Parameters::deserialize(const std::string & parameters)
 
 			}
 
-			if(Parameters::getDefaultParameters().find(key) == Parameters::getDefaultParameters().end())
+			if(oldIter==Parameters::getRemovedParameters().end() &&
+			   Parameters::getDefaultParameters().find(key) == Parameters::getDefaultParameters().end())
 			{
 				UWARN("Unknown parameter \"%s\"=\"%s\"! The parameter is still added to output map.", key.c_str(), value.c_str());
 			}

corelib/src/VWDictionary.cpp

@@ -142,6 +142,32 @@ void VWDictionary::setFixedDictionary(const std::string & dictionaryPath)
 		if((!_incrementalDictionary && _dictionaryPath.compare(dictionaryPath) != 0) ||
 		   _visualWords.size() == 0)
 		{
+			UDEBUG("incremental=%d, oldPath=%s newPath=%s, visual words=%d",
+					_incrementalDictionary?1:0, _dictionaryPath.c_str(), dictionaryPath.c_str(), (int)_visualWords.size());
+
+			if(UFile::getExtension(dictionaryPath).compare("db") == 0)
+			{
+				UDEBUG("Loading fixed vocabulary \"%s\", this may take a while...", dictionaryPath.c_str());
+				DBDriver * driver = DBDriver::create();
+				if(driver->openConnection(dictionaryPath, false))
+				{
+					driver->load(this, false);
+					for(std::map<int, VisualWord*>::iterator iter=_visualWords.begin(); iter!=_visualWords.end(); ++iter)
+					{
+						iter->second->setSaved(false);
+					}
+					_incrementalDictionary = _visualWords.size()==0;
+					driver->closeConnection(false);
+				}
+				else
+				{
+					UERROR("Could not load dictionary from database %s", dictionaryPath.c_str());
+				}
+				delete driver;
+			}
+			else
+			{
+				UWARN("Loading fixed vocabulary \"%s\", this may take a while...", dictionaryPath.c_str());
 				std::ifstream file;
 				file.open(dictionaryPath.c_str(), std::ifstream::in);
 				if(file.good())
@@ -154,7 +180,7 @@ void VWDictionary::setFixedDictionary(const std::string & dictionaryPath)
 					std::list<std::string> strList;
 					std::getline(file, str);
 					strList = uSplitNumChar(str);
-				unsigned int dimension  = 0;
+					int dimension  = 0;
 					for(std::list<std::string>::iterator iter = strList.begin(); iter != strList.end(); ++iter)
 					{
 						if(uIsDigit(iter->at(0)))
@@ -163,8 +189,9 @@ void VWDictionary::setFixedDictionary(const std::string & dictionaryPath)
 							break;
 						}
 					}
+					UDEBUG("descriptor dimension = %d", dimension);
 
-				if(dimension == 0 || dimension > 1000)
+					if(dimension <= 0 || dimension > 1000)
 					{
 						UERROR("Invalid dictionary file, visual word dimension (%d) is not valid, \"%s\"", dimension, dictionaryPath.c_str());
 					}
@@ -175,14 +202,14 @@ void VWDictionary::setFixedDictionary(const std::string & dictionaryPath)
 						{
 							std::getline(file, str);
 							strList = uSplit(str);
-						if(strList.size() == dimension+1)
+							if((int)strList.size() == dimension+1)
 							{
 								//first one is the visual word id
 								std::list<std::string>::iterator iter = strList.begin();
 								int id = std::atoi(iter->c_str());
 								cv::Mat descriptor(1, dimension, CV_32F);
 								++iter;
-							unsigned int i=0;
+								int i=0;
 
 								//get descriptor
 								for(;i<dimension && iter != strList.end(); ++i, ++iter)
@@ -191,24 +218,24 @@ void VWDictionary::setFixedDictionary(const std::string & dictionaryPath)
 								}
 								if(i != dimension)
 								{
-								UERROR("");
+									UERROR("Loaded word has not the same size (%d) than descriptor size previously detected (%d).", i, dimension);
 								}
 
 								VisualWord * vw = new VisualWord(id, descriptor, 0);
 								_visualWords.insert(_visualWords.end(), std::pair<int, VisualWord*>(id, vw));
 								_notIndexedWords.insert(_notIndexedWords.end(), id);
+								_unusedWords.insert(_unusedWords.end(), std::pair<int, VisualWord*>(id, vw));
 							}
-						else
+							else if(!str.empty())
 							{
 								UWARN("Cannot parse line \"%s\"", str.c_str());
 							}
 						}
-					this->update();
+						if(_visualWords.size())
-					_incrementalDictionary = false;
+						{
+							UWARN("Loaded %d words!", (int)_visualWords.size());
+						}
 					}
-
-
-				UDEBUG("Time changing dictionary = %fs", timer.ticks());
 				}
 				else
 				{
@@ -216,6 +243,19 @@ void VWDictionary::setFixedDictionary(const std::string & dictionaryPath)
 				}
 				file.close();
 			}
+
+			if(_visualWords.size() == 0)
+			{
+				_incrementalDictionary = _visualWords.size()==0;
+				UWARN("No words loaded, cannot set a fixed dictionary.", (int)_visualWords.size());
+			}
+			else
+			{
+				this->update();
+				_incrementalDictionary = false;
+				UDEBUG("Loaded %d words!", (int)_visualWords.size());
+			}
+		}
 		else if(!_incrementalDictionary)
 		{
 			UDEBUG("Dictionary \"%s\" already loaded...", dictionaryPath.c_str());
@@ -225,14 +265,14 @@ void VWDictionary::setFixedDictionary(const std::string & dictionaryPath)
 			UERROR("Cannot change to a fixed dictionary if there are already words (%d) in the incremental one.", _visualWords.size());
 		}
 	}
-	else if(_visualWords.size() == 0)
+	else if(_incrementalDictionary && _visualWords.size())
-	{
-		_incrementalDictionary = false;
-	}
-	else if(_incrementalDictionary)
 	{
 		UWARN("Cannot change to fixed dictionary, %d words already loaded as incremental", (int)_visualWords.size());
 	}
+	else
+	{
+		_incrementalDictionary = false;
+	}
 	_dictionaryPath = dictionaryPath;
 }
 
@@ -568,7 +608,7 @@ void VWDictionary::addWordRef(int wordId, int signatureId)
 		}
 		else
 		{
-			UERROR("Not found word %d", wordId);
+			UERROR("Not found word %d (dict size=%d)", wordId, (int)_visualWords.size());
 		}
 	}
 }
@@ -1220,26 +1260,17 @@ VisualWord * VWDictionary::getUnusedWord(int id) const
 
 std::vector<VisualWord*> VWDictionary::getUnusedWords() const
 {
-	if(!_incrementalDictionary)
-	{
-		ULOGGER_WARN("This method does nothing on a fixed dictionary");
-		return std::vector<VisualWord*>();
-	}
 	return uValues(_unusedWords);
 }
 
 std::vector<int> VWDictionary::getUnusedWordIds() const
 {
-	if(!_incrementalDictionary)
-	{
-		ULOGGER_WARN("This method does nothing on a fixed dictionary");
-		return std::vector<int>();
-	}
 	return uKeys(_unusedWords);
 }
 
 void VWDictionary::removeWords(const std::vector<VisualWord*> & words)
 {
+	UDEBUG("Removing %d words from dictionary (current size=%d)", (int)words.size(), (int)_visualWords.size());
 	for(unsigned int i=0; i<words.size(); ++i)
 	{
 		_visualWords.erase(words[i]->id());

docker/artful/Dockerfile

@@ -18,7 +18,57 @@ RUN apt-get update && apt-get install -y \
 
 WORKDIR /root/
 
+# GTSAM
+RUN git clone https://bitbucket.org/gtborg/gtsam.git
+RUN cd gtsam && \
+    git checkout 4.0.0-alpha2 && \
+    mkdir build && \
+    cd build && \
+    cmake -DMETIS_SHARED=ON -DGTSAM_BUILD_STATIC_LIBRARY=OFF -DGTSAM_BUILD_TESTS=OFF -DGTSAM_BUILD_EXAMPLES_ALWAYS=OFF -DCMAKE_BUILD_TYPE=Release .. && \
+    make -j$(nproc) && \
+    make install && \
+    cd && \
+    rm -r gtsam
+
+# g2o
+RUN git clone https://github.com/RainerKuemmerle/g2o.git
+RUN cd g2o && \
+    git checkout 20170730_git && \
+    mkdir build && \
+    cd build && \
+    cmake -DBUILD_LGPL_SHARED_LIBS=ON -DG2O_BUILD_APPS=OFF -DG2O_BUILD_EXAMPLES=OFF -DBUILD_SHARED_LIBS=ON -DCMAKE_BUILD_TYPE=Release .. && \
+    make -j$(nproc) && \
+    make install && \
+    cd && \
+    rm -r g2o
+
+# libpointmatcher 
+RUN git clone https://github.com/ethz-asl/libnabo.git
+#commit Apr 25 2018
+RUN cd libnabo && \
+    git checkout 7e378f6765393462357b8b74d8dc8c5554542ae6 && \
+    mkdir build && \
+    cd build && \
+    cmake -DCMAKE_BUILD_TYPE=Release .. && \
+    make -j$(nproc) && \
+    make install && \
+    cd && \
+    rm -r libnabo
+RUN git clone https://github.com/ethz-asl/libpointmatcher.git
+#commit Jan 19 2018
+RUN cd libpointmatcher && \
+    git checkout 00004bd41e44a1cf8de24ad87e4914760717cbcc && \
+    mkdir build && \
+    cd build && \
+    cmake -DCMAKE_BUILD_TYPE=Release .. && \
+    make -j$(nproc) && \
+    make install && \
+    cd && \
+    rm -r libpointmatcher
+
+
 # Clone source code
+ARG CACHE_DATE=2016-01-01
 RUN git clone https://github.com/introlab/rtabmap.git
 
 # Build RTAB-Map project

docker/artful/hooks/build

@@ -0,0 +1,2 @@
+#!/bin/bash 
+docker build --build-arg CACHE_DATE="$(date)" --cache-from $IMAGE_NAME -f $DOCKERFILE_PATH -t $IMAGE_NAME -t $DOCKER_REPO:17.10 .

docker/artful/hooks/post_push

@@ -0,0 +1,2 @@
+#!/bin/bash 
+docker push $DOCKER_REPO:17.10

docker/bionic/Dockerfile

@@ -18,7 +18,57 @@ RUN apt-get update && DEBIAN_FRONTEND=noninteractive apt-get install -y \
 
 WORKDIR /root/
 
+# GTSAM
+RUN git clone https://bitbucket.org/gtborg/gtsam.git
+RUN cd gtsam && \
+    git checkout 4.0.0-alpha2 && \
+    mkdir build && \
+    cd build && \
+    cmake -DMETIS_SHARED=ON -DGTSAM_BUILD_STATIC_LIBRARY=OFF -DGTSAM_BUILD_TESTS=OFF -DGTSAM_BUILD_EXAMPLES_ALWAYS=OFF -DCMAKE_BUILD_TYPE=Release .. && \
+    make -j$(nproc) && \
+    make install && \
+    cd && \
+    rm -r gtsam
+
+# g2o
+RUN git clone https://github.com/RainerKuemmerle/g2o.git
+RUN cd g2o && \
+    git checkout 20170730_git && \
+    mkdir build && \
+    cd build && \
+    cmake -DBUILD_LGPL_SHARED_LIBS=ON -DG2O_BUILD_APPS=OFF -DG2O_BUILD_EXAMPLES=OFF -DBUILD_SHARED_LIBS=ON -DCMAKE_BUILD_TYPE=Release .. && \
+    make -j$(nproc) && \
+    make install && \
+    cd && \
+    rm -r g2o
+
+# libpointmatcher 
+RUN git clone https://github.com/ethz-asl/libnabo.git
+#commit Apr 25 2018
+RUN cd libnabo && \
+    git checkout 7e378f6765393462357b8b74d8dc8c5554542ae6 && \
+    mkdir build && \
+    cd build && \
+    cmake -DCMAKE_BUILD_TYPE=Release .. && \
+    make -j$(nproc) && \
+    make install && \
+    cd && \
+    rm -r libnabo
+RUN git clone https://github.com/ethz-asl/libpointmatcher.git
+#commit Jan 19 2018
+RUN cd libpointmatcher && \
+    git checkout 00004bd41e44a1cf8de24ad87e4914760717cbcc && \
+    mkdir build && \
+    cd build && \
+    cmake -DCMAKE_BUILD_TYPE=Release .. && \
+    make -j$(nproc) && \
+    make install && \
+    cd && \
+    rm -r libpointmatcher
+
+
 # Clone source code
+ARG CACHE_DATE=2016-01-01
 RUN git clone https://github.com/introlab/rtabmap.git
 
 # Build RTAB-Map project

docker/bionic/hooks/build

@@ -0,0 +1,2 @@
+#!/bin/bash 
+docker build --build-arg CACHE_DATE="$(date)" --cache-from $IMAGE_NAME -f $DOCKERFILE_PATH -t $IMAGE_NAME -t $DOCKER_REPO:18.04 .

docker/bionic/hooks/post_push

@@ -0,0 +1,2 @@
+#!/bin/bash 
+docker push $DOCKER_REPO:18.04

docker/xenial/Dockerfile

@@ -15,8 +15,58 @@ RUN apt-get update && apt-get install -y \
       libopenni2-dev \
       software-properties-common
 
-# Clone source code
 WORKDIR /root/
+
+# GTSAM
+RUN git clone https://bitbucket.org/gtborg/gtsam.git
+RUN cd gtsam && \
+    git checkout 4.0.0-alpha2 && \
+    mkdir build && \
+    cd build && \
+    cmake -DMETIS_SHARED=ON -DGTSAM_BUILD_STATIC_LIBRARY=OFF -DGTSAM_BUILD_TESTS=OFF -DGTSAM_BUILD_EXAMPLES_ALWAYS=OFF -DCMAKE_BUILD_TYPE=Release .. && \
+    make -j$(nproc) && \
+    make install && \
+    cd && \
+    rm -r gtsam
+
+# g2o
+RUN git clone https://github.com/RainerKuemmerle/g2o.git
+RUN cd g2o && \
+    git checkout 20170730_git && \
+    mkdir build && \
+    cd build && \
+    cmake -DBUILD_LGPL_SHARED_LIBS=ON -DG2O_BUILD_APPS=OFF -DG2O_BUILD_EXAMPLES=OFF -DBUILD_SHARED_LIBS=ON -DCMAKE_BUILD_TYPE=Release .. && \
+    make -j$(nproc) && \
+    make install && \
+    cd && \
+    rm -r g2o
+
+# libpointmatcher 
+RUN git clone https://github.com/ethz-asl/libnabo.git
+#commit Apr 25 2018
+RUN cd libnabo && \
+    git checkout 7e378f6765393462357b8b74d8dc8c5554542ae6 && \
+    mkdir build && \
+    cd build && \
+    cmake -DCMAKE_BUILD_TYPE=Release .. && \
+    make -j$(nproc) && \
+    make install && \
+    cd && \
+    rm -r libnabo
+RUN git clone https://github.com/ethz-asl/libpointmatcher.git
+#commit Jan 19 2018
+RUN cd libpointmatcher && \
+    git checkout 00004bd41e44a1cf8de24ad87e4914760717cbcc && \
+    mkdir build && \
+    cd build && \
+    cmake -DCMAKE_BUILD_TYPE=Release .. && \
+    make -j$(nproc) && \
+    make install && \
+    cd && \
+    rm -r libpointmatcher
+
+# Clone source code
+ARG CACHE_DATE=2016-01-01
 RUN git clone https://github.com/introlab/rtabmap.git
 
 # Build RTAB-Map project

docker/xenial/android/install_deps.bash

@@ -29,7 +29,7 @@ rm -r *
 cmake -DCMAKE_TOOLCHAIN_FILE=$pwd/android.toolchain.cmake -DANDROID_ABI=arm64-v8a -DBUILD_SHARED_LIBS=OFF -DCMAKE_BUILD_TYPE=Release -DCMAKE_INSTALL_PREFIX=$prefix/arm64-v8a ..
 make
 make install
-cd
+cd $pwd
 rm -r boost_1_59_0.tar.gz boost_1_59_0
 
 # eigen
@@ -46,7 +46,7 @@ rm -r *
 cmake -DCMAKE_TOOLCHAIN_FILE=$pwd/android.toolchain.cmake -DANDROID_ABI=arm64-v8a -DBUILD_SHARED_LIBS=OFF -DCMAKE_BUILD_TYPE=Release -DCMAKE_INSTALL_PREFIX=$prefix/arm64-v8a ..
 make
 make install
-cd
+cd $pwd
 rm -r 3.2.7.tar.gz eigen-eigen-b30b87236a1b
 
 # FLANN
@@ -63,7 +63,7 @@ rm -r *
 cmake -DCMAKE_TOOLCHAIN_FILE=$pwd/android.toolchain.cmake -DANDROID_ABI=arm64-v8a -DBUILD_SHARED_LIBS=OFF -DCMAKE_BUILD_TYPE=Release -DCMAKE_INSTALL_PREFIX=$prefix/arm64-v8a ..
 make
 make install
-cd
+cd $pwd
 rm -r flann-1.8.4-src.zip flann-1.8.4-src
 
 # GTSAM
@@ -82,12 +82,13 @@ rm -r *
 cmake -DCMAKE_TOOLCHAIN_FILE=$pwd/android.toolchain.cmake -DANDROID_ABI=arm64-v8a -DMETIS_SHARED=OFF -DGTSAM_BUILD_STATIC_LIBRARY=ON -DGTSAM_BUILD_TESTS=OFF -DGTSAM_BUILD_EXAMPLES_ALWAYS=OFF -DCMAKE_BUILD_TYPE=Release -DCMAKE_INSTALL_PREFIX=$prefix/arm64-v8a ..
 make
 make install
-cd
+cd $pwd
-rm -r gtsam
+rm -rf gtsam
 
 # g2o
 git clone https://github.com/RainerKuemmerle/g2o.git
 cd g2o
+git checkout a3f7706bdbb849b2808dc3e1b7aee189f63b498e
 mkdir build
 cd build
 cmake -DCMAKE_TOOLCHAIN_FILE=$pwd/android.toolchain.cmake -DANDROID_ABI=armeabi-v7a -DBUILD_LGPL_SHARED_LIBS=OFF -DG2O_BUILD_APPS=OFF -DG2O_BUILD_EXAMPLES=OFF -DBUILD_SHARED_LIBS=OFF -DCMAKE_BUILD_TYPE=Release -DCMAKE_INSTALL_PREFIX=$prefix/armeabi-v7a ..
@@ -97,8 +98,8 @@ rm -r *
 cmake -DCMAKE_TOOLCHAIN_FILE=$pwd/android.toolchain.cmake -DANDROID_ABI=arm64-v8a -DBUILD_LGPL_SHARED_LIBS=OFF -DG2O_BUILD_APPS=OFF -DG2O_BUILD_EXAMPLES=OFF -DBUILD_SHARED_LIBS=OFF -DCMAKE_BUILD_TYPE=Release -DCMAKE_INSTALL_PREFIX=$prefix/arm64-v8a ..
 make
 make install
-cd
+cd $pwd
-rm -r g2o
+rm -rf g2o
 
 
 #CMake 3.7 for VTK
@@ -113,15 +114,15 @@ cd VTK
 git checkout tags/v7.1.1
 mkdir build
 cd build
-~/cmake-3.7.2-Linux-x86_64/bin/cmake -DBUILD_EXAMPLES=OFF -DBUILD_TESTING=OFF -DVTK_ANDROID_BUILD=ON -DANDROID_ARCH_ABI=armeabi-v7a -DBUILD_SHARED_LIBS=OFF -DCMAKE_BUILD_TYPE=Release -DCMAKE_INSTALL_PREFIX=$prefix/armeabi-v7a ..
+$pwd/cmake-3.7.2-Linux-x86_64/bin/cmake -DBUILD_EXAMPLES=OFF -DBUILD_TESTING=OFF -DVTK_ANDROID_BUILD=ON -DANDROID_ARCH_ABI=armeabi-v7a -DBUILD_SHARED_LIBS=OFF -DCMAKE_BUILD_TYPE=Release -DCMAKE_INSTALL_PREFIX=$prefix/armeabi-v7a ..
 make
 cp -r CMakeExternals/Install/vtk-android/* $prefix/armeabi-v7a/.
 rm -r *
-~/cmake-3.7.2-Linux-x86_64/bin/cmake -DBUILD_EXAMPLES=OFF -DBUILD_TESTING=OFF -DVTK_ANDROID_BUILD=ON -DANDROID_ARCH_ABI=arm64-v8a -DBUILD_SHARED_LIBS=OFF -DCMAKE_BUILD_TYPE=Release -DCMAKE_INSTALL_PREFIX=$prefix/arm64-v8a ..
+$pwd/cmake-3.7.2-Linux-x86_64/bin/cmake -DBUILD_EXAMPLES=OFF -DBUILD_TESTING=OFF -DVTK_ANDROID_BUILD=ON -DANDROID_ARCH_ABI=arm64-v8a -DBUILD_SHARED_LIBS=OFF -DCMAKE_BUILD_TYPE=Release -DCMAKE_INSTALL_PREFIX=$prefix/arm64-v8a ..
 make
 cp -r CMakeExternals/Install/vtk-android/* $prefix/arm64-v8a/.
-cd
+cd $pwd
-rm -r VTK cmake-3.7.2-Linux-x86_64
+rm -rf VTK cmake-3.7.2-Linux-x86_64
 
 # PCL
 git clone https://github.com/PointCloudLibrary/pcl.git
@@ -143,25 +144,25 @@ cmake -DCMAKE_TOOLCHAIN_FILE=$pwd/android.toolchain.cmake -DANDROID_ABI=arm64-v8
 cmake -DCMAKE_TOOLCHAIN_FILE=$pwd/android.toolchain.cmake -DANDROID_ABI=arm64-v8a -DBUILD_apps=OFF -DBUILD_examples=OFF -DBUILD_tools=OFF -DBUILD_visualization=OFF -DBUILD_tracking=OFF -DBUILD_people=OFF -DBUILD_global_tests=OFF -DWITH_QT=OFF -DWITH_OPENGL=OFF -DWITH_VTK=ON -DPCL_SHARED_LIBS=OFF -DCMAKE_BUILD_TYPE=Release -DCMAKE_INSTALL_PREFIX=$prefix/arm64-v8a ..
 make
 make install
-cd
+cd $pwd
-rm -r pcl
+rm -rf pcl
 
 # OpenCV
 git clone https://github.com/opencv/opencv_contrib.git
 cd opencv_contrib
 git checkout tags/3.2.0
-cd
+cd $pwd
 git clone https://github.com/opencv/opencv.git
 cd opencv
 git checkout tags/3.2.0
 mkdir build
 cd build
-cmake -DCMAKE_TOOLCHAIN_FILE=$pwd/android.toolchain.cmake -DANDROID_ABI=armeabi-v7a -DOPENCV_EXTRA_MODULES_PATH=$pwd/opencv_contrib/modules -DCMAKE_BUILD_TYPE=Release -DBUILD_SHARED_LIBS=OFF -DBUILD_TESTS=OFF -DBUILD_PERF_TESTS=OFF -DCMAKE_INSTALL_PREFIX=$prefix/armeabi-v7a ..
+cmake -DCMAKE_TOOLCHAIN_FILE=$pwd/android.toolchain.cmake -DANDROID_ABI=armeabi-v7a -DOPENCV_EXTRA_MODULES_PATH=$pwd/opencv_contrib/modules -DCMAKE_BUILD_TYPE=Release -DBUILD_SHARED_LIBS=OFF -DBUILD_TESTS=OFF -DBUILD_PERF_TESTS=OFF -DWITH_CUDA=OFF -DCMAKE_INSTALL_PREFIX=$prefix/armeabi-v7a ..
 make
 make install
 rm -r *
-cmake -DCMAKE_TOOLCHAIN_FILE=$pwd/android.toolchain.cmake -DANDROID_ABI=arm64-v8a -DOPENCV_EXTRA_MODULES_PATH=$pwd/opencv_contrib/modules -DCMAKE_BUILD_TYPE=Release -DBUILD_SHARED_LIBS=OFF -DBUILD_TESTS=OFF -DBUILD_PERF_TESTS=OFF -DCMAKE_INSTALL_PREFIX=$prefix/arm64-v8a ..
+cmake -DCMAKE_TOOLCHAIN_FILE=$pwd/android.toolchain.cmake -DANDROID_ABI=arm64-v8a -DOPENCV_EXTRA_MODULES_PATH=$pwd/opencv_contrib/modules -DCMAKE_BUILD_TYPE=Release -DBUILD_SHARED_LIBS=OFF -DBUILD_TESTS=OFF -DBUILD_PERF_TESTS=OFF -DWITH_CUDA=OFF -DCMAKE_INSTALL_PREFIX=$prefix/arm64-v8a ..
 make
 make install
-cd
+cd $pwd
-rm -r opencv opencv_contrib
+rm -rf opencv opencv_contrib

docker/xenial/android/install_deps_host.bash

@@ -5,8 +5,8 @@ if [ $# -ne 1 ]; then
     exit 1
 fi
 
-apt-get update
+sudo apt-get update
-apt-get install -y git cmake g++ software-properties-common wget unzip zlib1g-dev qtbase5-dev libqt5x11extras5-dev libqt5svg5-dev qttools5-dev libxt-dev 
+sudo apt-get install -y git cmake g++ software-properties-common wget unzip zlib1g-dev qtbase5-dev libqt5x11extras5-dev libqt5svg5-dev qttools5-dev libxt-dev 
 
 prefix=$1
 pwd=$(pwd)
@@ -25,7 +25,7 @@ cd build
 cmake -DBUILD_SHARED_LIBS=ON -DCMAKE_BUILD_TYPE=Release -DCMAKE_INSTALL_PREFIX=$prefix ..
 make
 make install
-cd
+cd $pwd
 rm -r boost_1_59_0.tar.gz boost_1_59_0
 
 # eigen
@@ -38,7 +38,7 @@ cd build
 cmake -DBUILD_SHARED_LIBS=ON -DCMAKE_BUILD_TYPE=Release -DCMAKE_INSTALL_PREFIX=$prefix ..
 make
 make install
-cd
+cd $pwd
 rm -r 3.2.7.tar.gz eigen-eigen-b30b87236a1b
 
 # FLANN
@@ -51,7 +51,7 @@ cd build
 cmake -DBUILD_SHARED_LIBS=ON -DCMAKE_BUILD_TYPE=Release -DCMAKE_INSTALL_PREFIX=$prefix ..
 make 
 make install
-cd
+cd $pwd
 rm -r flann-1.8.4-src.zip flann-1.8.4-src
 
 # GTSAM
@@ -66,19 +66,20 @@ cd build
 cmake -DMETIS_SHARED=ON -DGTSAM_BUILD_STATIC_LIBRARY=OFF -DGTSAM_BUILD_TESTS=OFF -DGTSAM_BUILD_EXAMPLES_ALWAYS=OFF -DCMAKE_BUILD_TYPE=Release -DCMAKE_INSTALL_PREFIX=$prefix ..
 make 
 make install
-cd
+cd $pwd
-rm -r gtsam
+rm -rf gtsam
 
 # g2o
 git clone https://github.com/RainerKuemmerle/g2o.git
 cd g2o
+git checkout a3f7706bdbb849b2808dc3e1b7aee189f63b498e
 mkdir build
 cd build
 cmake -DBUILD_LGPL_SHARED_LIBS=ON -DG2O_BUILD_APPS=OFF -DG2O_BUILD_EXAMPLES=OFF -DBUILD_SHARED_LIBS=ON -DCMAKE_BUILD_TYPE=Release -DCMAKE_INSTALL_PREFIX=$prefix ..
 make
 make install
-cd
+cd $pwd
-rm -r g2o
+rm -rf g2o
 
 # VTK
 git clone https://github.com/Kitware/VTK.git
@@ -89,8 +90,8 @@ cd build
 cmake -DBUILD_EXAMPLES=OFF -DBUILD_TESTING=OFF -DBUILD_SHARED_LIBS=ON -DCMAKE_BUILD_TYPE=Release -DVTK_Group_Qt=ON -DVTK_QT_VERSION=5 -DCMAKE_INSTALL_PREFIX=$prefix ..
 make
 make install
-cd
+cd $pwd
-rm -r VTK
+rm -rf VTK
 
 # PCL
 git clone https://github.com/PointCloudLibrary/pcl.git
@@ -104,14 +105,14 @@ cd build
 cmake -DBUILD_apps=OFF -DBUILD_examples=OFF -DBUILD_tools=OFF -DBUILD_visualization=ON -DBUILD_tracking=OFF -DBUILD_people=OFF -DBUILD_global_tests=OFF -DWITH_QT=ON -DWITH_OPENGL=OFF -DWITH_VTK=ON -DPCL_SHARED_LIBS=ON -DCMAKE_BUILD_TYPE=Release -DCMAKE_INSTALL_PREFIX=$prefix ..
 make
 make install
-cd
+cd $pwd
-rm -r pcl
+rm -rf pcl
 
 # OpenCV
 git clone https://github.com/opencv/opencv_contrib.git
 cd opencv_contrib
 git checkout tags/3.2.0
-cd
+cd $pwd
 git clone https://github.com/opencv/opencv.git
 cd opencv
 git checkout tags/3.2.0
@@ -120,5 +121,5 @@ cd build
 cmake -DOPENCV_EXTRA_MODULES_PATH=$pwd/opencv_contrib/modules -DCMAKE_BUILD_TYPE=Release -DBUILD_SHARED_LIBS=ON -DBUILD_TESTS=OFF -DBUILD_PERF_TESTS=OFF -DCMAKE_INSTALL_PREFIX=$prefix ..
 make
 make install
-cd
+cd $pwd
-rm -r opencv opencv_contrib
+rm -rf opencv opencv_contrib

docker/xenial/android/tango/build.bash

@@ -35,7 +35,7 @@ mv TangoSDK_Ikariotikos_Java.jar rtabmap-tango/app/android/libs/.
 cd rtabmap-tango/build
 cmake -DANDROID_PREBUILD=ON ..
 make
-cd
+cd ../..
 
 # rtabmap
 mkdir rtabmap-tango/build/armeabi-v7a
@@ -43,7 +43,7 @@ cd rtabmap-tango/build/armeabi-v7a
 cmake -DCMAKE_TOOLCHAIN_FILE=../../cmake_modules/android.toolchain.cmake -DANDROID_ABI=armeabi-v7a -DBUILD_SHARED_LIBS=OFF -DBUILD_EXAMPLES=OFF -DBUILD_TOOLS=OFF -DCMAKE_BUILD_TYPE=Release -DOpenCV_DIR=$prefix/armeabi-v7a/sdk/native/jni -DCMAKE_INSTALL_PREFIX=$prefix/armeabi-v7a ../..
 make
 
-cd
+cd ../../..
 mkdir rtabmap-tango/build/arm64-v8a
 cd rtabmap-tango/build/arm64-v8a
 cmake -DCMAKE_TOOLCHAIN_FILE=../../cmake_modules/android.toolchain.cmake -DANDROID_ABI=arm64-v8a -DBUILD_SHARED_LIBS=OFF -DBUILD_EXAMPLES=OFF -DBUILD_TOOLS=OFF -DCMAKE_BUILD_TYPE=Release -DOpenCV_DIR=$prefix/arm64-v8a/sdk/native/jni -DCMAKE_INSTALL_PREFIX=$prefix/arm64-v8a ../..

docker/xenial/hooks/build

@@ -0,0 +1,2 @@
+#!/bin/bash 
+docker build --build-arg CACHE_DATE="$(date)" --cache-from $IMAGE_NAME -f $DOCKERFILE_PATH -t $IMAGE_NAME -t $DOCKER_REPO:16.04 -t $DOCKER_REPO:latest  .

docker/xenial/hooks/post_push

@@ -0,0 +1,3 @@
+#!/bin/bash 
+docker push $DOCKER_REPO:16.04
+docker push $DOCKER_REPO:latest

docker/zesty/Dockerfile

@@ -16,8 +16,59 @@ RUN apt-get update && apt-get install -y \
 # Issue: http://www.pcl-users.org/Build-failure-on-Ubuntu-17-04-td4044552.html
 RUN sed -i 's|/usr/lib/libmpi.so;||g' /usr/lib/x86_64-linux-gnu/cmake/pcl/PCLConfig.cmake
 
-# Clone source code
 WORKDIR /root/
+
+# GTSAM
+RUN git clone https://bitbucket.org/gtborg/gtsam.git
+RUN cd gtsam && \
+    git checkout 4.0.0-alpha2 && \
+    mkdir build && \
+    cd build && \
+    cmake -DMETIS_SHARED=ON -DGTSAM_BUILD_STATIC_LIBRARY=OFF -DGTSAM_BUILD_TESTS=OFF -DGTSAM_BUILD_EXAMPLES_ALWAYS=OFF -DCMAKE_BUILD_TYPE=Release .. && \
+    make -j$(nproc) && \
+    make install && \
+    cd && \
+    rm -r gtsam
+
+# g2o
+RUN git clone https://github.com/RainerKuemmerle/g2o.git
+RUN cd g2o && \
+    git checkout 20170730_git && \
+    mkdir build && \
+    cd build && \
+    cmake -DBUILD_LGPL_SHARED_LIBS=ON -DG2O_BUILD_APPS=OFF -DG2O_BUILD_EXAMPLES=OFF -DBUILD_SHARED_LIBS=ON -DCMAKE_BUILD_TYPE=Release .. && \
+    make -j$(nproc) && \
+    make install && \
+    cd && \
+    rm -r g2o
+
+# libpointmatcher 
+RUN git clone https://github.com/ethz-asl/libnabo.git
+#commit Apr 25 2018
+RUN cd libnabo && \
+    git checkout 7e378f6765393462357b8b74d8dc8c5554542ae6 && \
+    mkdir build && \
+    cd build && \
+    cmake -DCMAKE_BUILD_TYPE=Release .. && \
+    make -j$(nproc) && \
+    make install && \
+    cd && \
+    rm -r libnabo
+RUN git clone https://github.com/ethz-asl/libpointmatcher.git
+#commit Jan 19 2018
+RUN cd libpointmatcher && \
+    git checkout 00004bd41e44a1cf8de24ad87e4914760717cbcc && \
+    mkdir build && \
+    cd build && \
+    cmake -DCMAKE_BUILD_TYPE=Release .. && \
+    make -j$(nproc) && \
+    make install && \
+    cd && \
+    rm -r libpointmatcher
+
+
+# Clone source code
+ARG CACHE_DATE=2016-01-01
 RUN git clone https://github.com/introlab/rtabmap.git
 
 # Build RTAB-Map project

docker/zesty/hooks/build

@@ -0,0 +1,2 @@
+#!/bin/bash 
+docker build --build-arg CACHE_DATE="$(date)" --cache-from $IMAGE_NAME -f $DOCKERFILE_PATH -t $IMAGE_NAME -t $DOCKER_REPO:17.04 .

docker/zesty/hooks/post_push

@@ -0,0 +1,2 @@
+#!/bin/bash 
+docker push $DOCKER_REPO:17.04

examples/RGBDMapping/MapBuilder.h

@@ -38,6 +38,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #include "rtabmap/core/util3d_filtering.h"
 #include "rtabmap/core/util3d_transforms.h"
 #include "rtabmap/core/RtabmapEvent.h"
+#include "rtabmap/core/OccupancyGrid.h"
 #endif
 #include "rtabmap/utilite/UStl.h"
 #include "rtabmap/utilite/UConversion.h"
@@ -242,6 +243,44 @@ protected slots:
 			cloudViewer_->setCloudPointSize("graph_nodes", 5);
 		}
 
+		//============================
+		// Update/add occupancy grid (when RGBD/CreateOccupancyGrid is true)
+		//============================
+		for(std::map<int, Transform>::const_reverse_iterator iter = stats.poses().rbegin(); iter!=stats.poses().rend(); ++iter)
+		{
+			int id = iter->first;
+			if(grid_.addedNodes().find(id) == grid_.addedNodes().end())
+			{
+				std::map<int, Signature>::const_iterator jter = stats.getSignatures().find(id);
+				if(jter != stats.getSignatures().end() && jter->second.sensorData().gridCellSize() > 0.0f)
+				{
+					cv::Mat groundCells, obstacleCells, emptyCells;
+					jter->second.sensorData().uncompressDataConst(0, 0, 0, 0, &groundCells, &obstacleCells, &emptyCells);
+					grid_.addToCache(id, groundCells, obstacleCells, emptyCells);
+				}
+			}
+			else
+			{
+				// Assume that older nodes are already added to map
+				break;
+			}
+		}
+		if(grid_.addedNodes().size() || grid_.cacheSize())
+		{
+			grid_.update(stats.poses());
+		}
+		if(grid_.addedNodes().size())
+		{
+			float xMin, yMin;
+			cv::Mat map8S = grid_.getMap(xMin, yMin);
+			if(!map8S.empty())
+			{
+				//convert to gray scaled map
+				cv::Mat map8U = util3d::convertMap2Image8U(map8S);
+				cloudViewer_->addOccupancyGridMap(map8U, grid_.getCellSize(), xMin, yMin, 0.75);
+			}
+		}
+
 		odometryCorrection_ = stats.mapCorrection();
 
 		cloudViewer_->update();
@@ -283,6 +322,7 @@ protected:
 	Transform odometryCorrection_;
 	bool processingStatistics_;
 	bool lastOdometryProcessed_;
+	OccupancyGrid grid_;
 };
 
 

examples/RGBDMapping/main.cpp

@@ -163,9 +163,12 @@ int main(int argc, char * argv[])
 	OdometryThread odomThread(new OdometryF2M());
 
 
+	ParametersMap params;
+	//param.insert(ParametersPair(Parameters::kRGBDCreateOccupancyGrid(), "true")); // uncomment to create local occupancy grids
+
 	// Create RTAB-Map to process OdometryEvent
 	Rtabmap * rtabmap = new Rtabmap();
-	rtabmap->init();
+	rtabmap->init(params);
 	RtabmapThread rtabmapThread(rtabmap); // ownership is transfered
 
 	// Setup handlers

guilib/src/MainWindow.cpp

@@ -2067,7 +2067,7 @@ void MainWindow::updateMapCloud(
 		{
 			std::map<int, Transform> posesInTmp = posesIn;
 			posesInTmp.erase(-1);
-			poses = rtabmap::graph::radiusPosesFiltering(posesIn, radius, angle);
+			poses = rtabmap::graph::radiusPosesFiltering(posesInTmp, radius, angle);
 		}
 		else
 		{

guilib/src/PreferencesDialog.cpp

@@ -4171,11 +4171,11 @@ void PreferencesDialog::changeDictionaryPath()
 	QString path;
 	if(_ui->lineEdit_dictionaryPath->text().isEmpty())
 	{
-		path = QFileDialog::getOpenFileName(this, tr("Dictionary"), this->getWorkingDirectory());
+		path = QFileDialog::getOpenFileName(this, tr("Dictionary"), this->getWorkingDirectory(), tr("Dictionary (*.txt *.db)"));
 	}
 	else
 	{
-		path = QFileDialog::getOpenFileName(this, tr("Dictionary"), _ui->lineEdit_dictionaryPath->text());
+		path = QFileDialog::getOpenFileName(this, tr("Dictionary"), _ui->lineEdit_dictionaryPath->text(), tr("Dictionary (*.txt *.db)"));
 	}
 	if(!path.isEmpty())
 	{
@@ -4188,11 +4188,11 @@ void PreferencesDialog::changeOdometryORBSLAM2Vocabulary()
 	QString path;
 	if(_ui->lineEdit_OdomORBSLAM2VocPath->text().isEmpty())
 	{
-		path = QFileDialog::getOpenFileName(this, tr("ORBSLAM2 Vocabulary"), this->getWorkingDirectory());
+		path = QFileDialog::getOpenFileName(this, tr("ORBSLAM2 Vocabulary"), this->getWorkingDirectory(), tr("Vocabulary (*.txt)"));
 	}
 	else
 	{
-		path = QFileDialog::getOpenFileName(this, tr("ORBSLAM2 Vocabulary"), _ui->lineEdit_OdomORBSLAM2VocPath->text());
+		path = QFileDialog::getOpenFileName(this, tr("ORBSLAM2 Vocabulary"), _ui->lineEdit_OdomORBSLAM2VocPath->text(),  tr("Vocabulary (*.txt)"));
 	}
 	if(!path.isEmpty())
 	{

package.xml

@@ -1,7 +1,7 @@
 <?xml version="1.0"?>
 <package>
   <name>rtabmap</name>
-  <version>0.17.0</version>
+  <version>0.17.1</version>
   <description>RTAB-Map's standalone library. RTAB-Map is a RGB-D SLAM approach with real-time constraints.</description>
   <maintainer email="matlabbe@gmail.com">Mathieu Labbe</maintainer>
   <author>Mathieu Labbe</author>

tools/Report/main.cpp

@@ -43,7 +43,13 @@ void showUsage()
 {
 	printf("\nUsage:\n"
 			"rtabmap-report [\"Statistic/Id\"] [--latex] [--kitti] [--scale] [--poses] path\n"
-			"  path               Directory containing rtabmap databases or path of a database.\n\n");
+			"  path               Directory containing rtabmap databases or path of a database.\n"
+			"  --latex            Print table formatted in LaTeX with results.\n"
+			"  --kitti            Compute error based on KITTI benchmark.\n"
+			"  --scale            Find the best scale for the map against the ground truth\n"
+			"                       and compute error based on the scaled path.\n"
+			"  --poses            Export poses to [path]_poses.txt, ground truth to [path]_gt.txt\n"
+			"                       and valid ground truth indices to [path]_indices.txt \n\n");
 	exit(1);
 }
 
@@ -54,6 +60,9 @@ int main(int argc, char * argv[])
 		showUsage();
 	}
 
+	ULogger::setType(ULogger::kTypeConsole);
+	ULogger::setLevel(ULogger::kWarning);
+
 	QApplication app(argc, argv);
 
 	bool outputLatex = false;
@@ -161,7 +170,9 @@ int main(int argc, char * argv[])
 				ParametersMap params;
 				if(driver->openConnection(filePath))
 				{
+					ULogger::setLevel(ULogger::kError); // to suppress parameter warnings
 					params = driver->getLastParameters();
+					ULogger::setLevel(ULogger::kWarning);
 					std::set<int> ids;
 					driver->getAllNodeIds(ids);
 					std::map<int, std::pair<std::map<std::string, float>, double> > stats = driver->getAllStatistics();
@@ -294,7 +305,24 @@ int main(int argc, char * argv[])
 						optimizer->getConnectedGraph(firstId, odomPoses, graph::filterDuplicateLinks(links), posesOut, linksOut);
 
 						std::map<int, Transform> poses = optimizer->optimize(firstId, posesOut, linksOut);
+						if(poses.empty())
+						{
+							// try incremental optimization
+							UWARN("Optimization failed! Try incremental optimization...");
+							poses = optimizer->optimizeIncremental(firstId, posesOut, linksOut);
+							if(poses.empty())
+							{
+								UERROR("Incremental optimization also failed! Only original RMSE will be shown.");
+								bestRMSE = rmse;
+							}
+							else
+							{
+								UWARN("Incremental optimization succeeded!");
+							}
+						}
 
+						if(poses.size())
+						{
 							std::map<int, Transform> groundTruth;
 							for(std::map<int, Transform>::const_iterator iter=poses.begin(); iter!=poses.end(); ++iter)
 							{
@@ -393,15 +421,46 @@ int main(int argc, char * argv[])
 								}
 								if(groundTruth.size())
 								{
+									// For missing ground truth poses, set them to null
+									std::vector<int> validIndices(poses.size(), 1);
+									int i=0;
+									for(std::map<int, Transform>::iterator iter=poses.begin(); iter!=poses.end(); ++iter, ++i)
+									{
+										if(groundTruth.find(iter->first) == groundTruth.end())
+										{
+											groundTruth.insert(std::make_pair(iter->first, Transform()));
+											validIndices[i] = 0;
+										}
+									}
 									path = dir+UDirectory::separator()+dbName+"_gt.txt";
 									if(!graph::exportPoses(path, outputKittiError?2:0, groundTruth))
 									{
 										printf("Could not export the ground truth to \"%s\"!?!\n", path.c_str());
 									}
+									else
+									{
+										// save valid indices
+										path = dir+UDirectory::separator()+dbName+"_indices.txt";
+										FILE * file = 0;
+	#ifdef _MSC_VER
+										fopen_s(&file, path.c_str(), "w");
+	#else
+										file = fopen(path.c_str(), "w");
+	#endif
+										if(file)
+										{
+											// VERTEX3 id x y z phi theta psi
+											for(unsigned int k=0; k<validIndices.size(); ++k)
+											{
+												fprintf(file, "%d\n", validIndices[k]);
+											}
+											fclose(file);
+										}
+									}
+								}
 							}
 						}
 					}
-
 					printf("   %s (%d, s=%.3f):\terror lin=%.3fm (max=%.3fm) ang=%.1fdeg%s, slam: avg=%dms (max=%dms) loops=%d, odom: avg=%dms (max=%dms), camera: avg=%dms, %smap=%dMB\n",
 							fileName.c_str(),
 							(int)ids.size(),