0.20.14: added globalBundleAdjustment CLI, added Rtabmap/Memory::cleanupLocalGrids function, init with optimizedPoses from db even in mapping mode, reprocess: added -db option to save optimized 2d grid in database.

CMakeLists.txt

@@ -20,7 +20,7 @@ SET(CMAKE_MODULE_PATH "${PROJECT_SOURCE_DIR}/cmake_modules")
 #######################
 SET(RTABMAP_MAJOR_VERSION 0)
 SET(RTABMAP_MINOR_VERSION 20)
-SET(RTABMAP_PATCH_VERSION 13)
+SET(RTABMAP_PATCH_VERSION 14)
 SET(RTABMAP_VERSION
   ${RTABMAP_MAJOR_VERSION}.${RTABMAP_MINOR_VERSION}.${RTABMAP_PATCH_VERSION})
   

corelib/include/rtabmap/core/Memory.h

@@ -228,6 +228,14 @@ public:
 	unsigned long getMemoryUsed() const; //Bytes
 
 	void generateGraph(const std::string & fileName, const std::set<int> & ids = std::set<int>());
+	int cleanupLocalGrids(
+			const std::map<int, Transform> & poses,
+			const cv::Mat & map,
+			float xMin,
+			float yMin,
+			float cellSize,
+			int cropRadius = 1,
+			bool filterScans = false);
 
 	//keypoint stuff
 	const VWDictionary * getVWDictionary() const;

corelib/include/rtabmap/core/Rtabmap.h

@@ -206,6 +206,19 @@ public:
 			bool interSession = true,
 			const ProgressState * state = 0,
 			float clusterRadiusMin = 0.0f);
+	bool globalBundleAdjustment(
+			int optimizerType = 1 /*g2o*/,
+			bool rematchFeatures = true,
+			int iterations = 0,
+			float pixelVariance = 0.0f);
+	int cleanupLocalGrids(
+			const std::map<int, Transform> & mapPoses,
+			const cv::Mat & map,
+			float xMin,
+			float yMin,
+			float cellSize,
+			int cropRadius = 1,
+			bool filterScans = false);
 	int refineLinks();
 	bool addLink(const Link & link);
 	cv::Mat getInformation(const cv::Mat & covariance) const;

corelib/src/Memory.cpp

@@ -2066,10 +2066,11 @@ std::map<int, Transform> Memory::loadOptimizedPoses(Transform * lastlocalization
 		bool ok = true;
 		std::map<int, Transform> poses = _dbDriver->loadOptimizedPoses(lastlocalizationPose);
 		// Make sure optimized poses match the working directory! Otherwise return nothing.
-		for(std::map<int, Transform>::iterator iter=poses.begin(); iter!=poses.end() && ok; ++iter)
+		for(std::map<int, Transform>::iterator iter=poses.lower_bound(1); iter!=poses.end() && ok; ++iter)
 		{
 			if(_workingMem.find(iter->first)==_workingMem.end())
 			{
+				UWARN("Node %d not found in working memory", iter->first);
 				ok = false;
 			}
 		}
@@ -2080,7 +2081,7 @@ std::map<int, Transform> Memory::loadOptimizedPoses(Transform * lastlocalization
 				  "poses to force re-update. If you want to use the "
 				  "saved optimized poses, set %s to true",
 				  (int)poses.size(),
-				  (int)_workingMem.size(),
+				  (int)_workingMem.size()-1, // less virtual place
 				  Parameters::kMemInitWMWithAllNodes().c_str());
 			return std::map<int, Transform>();
 		}
@@ -4050,6 +4051,221 @@ void Memory::generateGraph(const std::string & fileName, const std::set<int> & i
 	_dbDriver->generateGraph(fileName, ids, _signatures);
 }
 
+int Memory::cleanupLocalGrids(
+		const std::map<int, Transform> & poses,
+		const cv::Mat & map,
+		float xMin,
+		float yMin,
+		float cellSize,
+		int cropRadius,
+		bool filterScans)
+{
+	if(!_dbDriver)
+	{
+		UERROR("A database must be loaded first...");
+		return -1;
+	}
+
+	if(poses.empty() || poses.lower_bound(1) == poses.end())
+	{
+		UERROR("Empty poses?!");
+		return -1;
+	}
+	if(map.empty())
+	{
+		UERROR("Map is empty!");
+		return -1;
+	}
+	UASSERT(cropRadius>=0);
+	UASSERT(cellSize>0.0f);
+
+	int maxPoses = 0;
+	for(std::map<int, Transform>::const_iterator iter=poses.lower_bound(1); iter!=poses.end(); ++iter)
+	{
+		++maxPoses;
+	}
+
+	UINFO("Processing %d grids...", maxPoses);
+	int processedGrids = 1;
+	int gridsScansModified = 0;
+	for(std::map<int, Transform>::const_iterator iter=poses.lower_bound(1); iter!=poses.end(); ++iter, ++processedGrids)
+	{
+		// local grid
+		cv::Mat gridGround;
+		cv::Mat gridObstacles;
+		cv::Mat gridEmpty;
+
+		// scan
+		SensorData data = this->getNodeData(iter->first, false, true, false, true);
+		LaserScan scan;
+		data.uncompressData(0,0,&scan,0,&gridGround,&gridObstacles,&gridEmpty);
+
+		if(!gridObstacles.empty())
+		{
+			UASSERT(data.gridCellSize() == cellSize);
+			cv::Mat filtered = cv::Mat(1, gridObstacles.cols, gridObstacles.type());
+			int oi = 0;
+			for(int i=0; i<gridObstacles.cols; ++i)
+			{
+				const float * ptr = gridObstacles.ptr<float>(0, i);
+				cv::Point3f pt(ptr[0], ptr[1], gridObstacles.channels()==2?0:ptr[2]);
+				pt = util3d::transformPoint(pt, iter->second);
+
+				int x = int((pt.x - xMin) / cellSize + 0.5f);
+				int y = int((pt.y - yMin) / cellSize + 0.5f);
+
+				if(x>=0 && x<map.cols &&
+				   y>=0 && y<map.rows)
+				{
+					bool obstacleDetected = false;
+
+					for(int j=-cropRadius; j<=cropRadius && !obstacleDetected; ++j)
+					{
+						for(int k=-cropRadius; k<=cropRadius && !obstacleDetected; ++k)
+						{
+							if(x+j>=0 && x+j<map.cols &&
+							   y+k>=0 && y+k<map.rows &&
+							   map.at<unsigned char>(y+k,x+j) == 100)
+							{
+								obstacleDetected = true;
+							}
+						}
+					}
+
+					if(map.at<unsigned char>(y,x) != 0 || obstacleDetected)
+					{
+						// Verify that we don't have an obstacle on neighbor cells
+						cv::Mat(gridObstacles, cv::Range::all(), cv::Range(i,i+1)).copyTo(cv::Mat(filtered, cv::Range::all(), cv::Range(oi,oi+1)));
+						++oi;
+					}
+				}
+			}
+
+			if(oi != gridObstacles.cols)
+			{
+				UINFO("Grid id=%d (%d/%d) filtered %d -> %d", iter->first, processedGrids, maxPoses, gridObstacles.cols, oi);
+				gridsScansModified += 1;
+
+				// update
+				Signature * s = this->_getSignature(iter->first);
+				cv::Mat newObstacles = cv::Mat(filtered, cv::Range::all(), cv::Range(0, oi));
+				bool modifyDb = true;
+				if(s)
+				{
+					s->sensorData().setOccupancyGrid(gridGround, newObstacles, gridEmpty, cellSize, data.gridViewPoint());
+					if(!s->isSaved())
+					{
+						// not saved in database yet
+						modifyDb = false;
+					}
+				}
+				if(modifyDb)
+				{
+					_dbDriver->updateOccupancyGrid(iter->first,
+							gridGround,
+							newObstacles,
+							gridEmpty,
+							cellSize,
+							data.gridViewPoint());
+				}
+			}
+		}
+
+		if(filterScans && !scan.isEmpty())
+		{
+			Transform mapToScan = iter->second * scan.localTransform();
+
+			cv::Mat filtered = cv::Mat(1, scan.size(), scan.dataType());
+			int oi = 0;
+			for(int i=0; i<scan.size(); ++i)
+			{
+				const float * ptr = scan.data().ptr<float>(0, i);
+				cv::Point3f pt(ptr[0], ptr[1], scan.is2d()?0:ptr[2]);
+				pt = util3d::transformPoint(pt, mapToScan);
+
+				int x = int((pt.x - xMin) / cellSize + 0.5f);
+				int y = int((pt.y - yMin) / cellSize + 0.5f);
+
+				if(x>=0 && x<map.cols &&
+				   y>=0 && y<map.rows)
+				{
+					bool obstacleDetected = false;
+
+					for(int j=-cropRadius; j<=cropRadius && !obstacleDetected; ++j)
+					{
+						for(int k=-cropRadius; k<=cropRadius && !obstacleDetected; ++k)
+						{
+							if(x+j>=0 && x+j<map.cols &&
+							   y+k>=0 && y+k<map.rows &&
+							   map.at<unsigned char>(y+k,x+j) == 100)
+							{
+								obstacleDetected = true;
+							}
+						}
+					}
+
+					if(map.at<unsigned char>(y,x) != 0 || obstacleDetected)
+					{
+						// Verify that we don't have an obstacle on neighbor cells
+						cv::Mat(scan.data(), cv::Range::all(), cv::Range(i,i+1)).copyTo(cv::Mat(filtered, cv::Range::all(), cv::Range(oi,oi+1)));
+						++oi;
+					}
+				}
+			}
+
+			if(oi != scan.size())
+			{
+				UINFO("Scan id=%d (%d/%d) filtered %d -> %d", iter->first, processedGrids, maxPoses, (int)scan.size(), oi);
+				gridsScansModified += 1;
+
+				// update
+				if(scan.angleIncrement()!=0)
+				{
+					// copy meta data
+					scan = LaserScan(
+							cv::Mat(filtered, cv::Range::all(), cv::Range(0, oi)),
+							scan.format(),
+							scan.rangeMin(),
+							scan.rangeMax(),
+							scan.angleMin(),
+							scan.angleMax(),
+							scan.angleIncrement(),
+							scan.localTransform());
+				}
+				else
+				{
+					// copy meta data
+					scan = LaserScan(
+							cv::Mat(filtered, cv::Range::all(), cv::Range(0, oi)),
+							scan.maxPoints(),
+							scan.rangeMax(),
+							scan.format(),
+							scan.localTransform());
+				}
+
+				// update
+				Signature * s = this->_getSignature(iter->first);
+				bool modifyDb = true;
+				if(s)
+				{
+					s->sensorData().setLaserScan(scan, true);
+					if(!s->isSaved())
+					{
+						// not saved in database yet
+						modifyDb = false;
+					}
+				}
+				if(modifyDb)
+				{
+					_dbDriver->updateLaserScan(iter->first, scan);
+				}
+
+			}
+		}
+	}
+	return gridsScansModified;
+}
+
 int Memory::getNi(int signatureId) const
 {
 	int ni = 0;

corelib/src/Parameters.cpp

@@ -1053,7 +1053,7 @@ ParametersMap Parameters::parseArguments(int argc, char * argv[], bool onlyParam
 						ignore = true;
 					}
 #endif
-#ifndef RTABMAP_LOAM
+#if not defined(RTABMAP_LOAM) and not defined(RTABMAP_FLOAM)
 					if(group.compare("OdomLOAM") == 0)
 					{
 						ignore = true;

corelib/src/Rtabmap.cpp

@@ -348,9 +348,9 @@ void Rtabmap::init(const ParametersMap & parameters, const std::string & databas
 	this->parseParameters(allParameters);
 
 	Transform lastPose;
+	_optimizedPoses = _memory->loadOptimizedPoses(&lastPose);
 	if(!_memory->isIncremental())
 	{
-		_optimizedPoses = _memory->loadOptimizedPoses(&lastPose);
 		if(_optimizedPoses.empty() &&
 			_memory->getWorkingMem().size()>1 &&
 			_memory->getWorkingMem().lower_bound(1)!=_memory->getWorkingMem().end())
@@ -404,6 +404,16 @@ void Rtabmap::init(const ParametersMap & parameters, const std::string & databas
 			UINFO("Loaded optimizedPoses=0, last localization pose is ignored!");
 		}
 	}
+	else
+	{
+		_lastLocalizationPose = lastPose;
+		if(!_optimizedPoses.empty())
+		{
+			std::map<int, Transform> tmp;
+			// Get just the links
+			_memory->getMetricConstraints(uKeysSet(_optimizedPoses), tmp, _constraints, false, true);
+		}
+	}
 
 	if(_databasePath.empty())
 	{
@@ -4706,6 +4716,12 @@ void Rtabmap::getGraph(
 				poses = _optimizedPoses; // guess
 				cv::Mat covariance;
 				this->optimizeCurrentMap(_memory->getLastWorkingSignature()->id(), global, poses, covariance, &constraints);
+				if(!global && !_optimizedPoses.empty())
+				{
+					// We send directly the already optimized poses if they are set
+					UDEBUG("_optimizedPoses=%ld poses=%ld", _optimizedPoses.size(), poses.size());
+					poses = _optimizedPoses;
+				}
 			}
 			else
 			{
@@ -5080,6 +5096,88 @@ int Rtabmap::detectMoreLoopClosures(
 	return (int)loopClosuresAdded.size();
 }
 
+bool Rtabmap::globalBundleAdjustment(
+		int optimizerType,
+		bool rematchFeatures,
+		int iterations,
+		float pixelVariance)
+{
+	if(!_optimizedPoses.empty() && !_constraints.empty())
+	{
+		int iterations = Parameters::defaultOptimizerIterations();
+		float pixelVariance = Parameters::defaultg2oPixelVariance();
+		ParametersMap params = _parameters;
+		Parameters::parse(params, Parameters::kOptimizerIterations(), iterations);
+		Parameters::parse(params, Parameters::kg2oPixelVariance(), pixelVariance);
+		if(iterations > 0)
+		{
+			uInsert(params, ParametersPair(Parameters::kOptimizerIterations(), uNumber2Str(iterations)));
+		}
+		if(pixelVariance > 0.0f)
+		{
+			uInsert(params, ParametersPair(Parameters::kg2oPixelVariance(), uNumber2Str(pixelVariance)));
+		}
+
+		std::map<int, Signature> signatures;
+		for(std::map<int, Transform>::iterator iter=_optimizedPoses.lower_bound(1); iter!=_optimizedPoses.end(); ++iter)
+		{
+			if(_memory->getSignature(iter->first))
+			{
+				signatures.insert(std::make_pair(iter->first, *_memory->getSignature(iter->first)));
+			}
+		}
+
+		Optimizer * optimizer = Optimizer::create((Optimizer::Type)optimizerType, params);
+		std::map<int, Transform> poses = optimizer->optimizeBA(
+				_optimizeFromGraphEnd?_optimizedPoses.lower_bound(1)->first:_optimizedPoses.rbegin()->first,
+				_optimizedPoses,
+				_constraints,
+				signatures,
+				rematchFeatures);
+		delete optimizer;
+
+		if(poses.empty())
+		{
+			UERROR("Optimization failed!");
+		}
+		else
+		{
+			_optimizedPoses = poses;
+			// This will force rtabmap_ros to regenerate the global occupancy grid if there was one
+			_memory->save2DMap(cv::Mat(), 0, 0, 0);
+			return true;
+		}
+	}
+	else
+	{
+		UERROR("Optimized poses (%ld) or constraints (%ld) are empty!", _optimizedPoses.size(), _constraints.size());
+	}
+	return false;
+}
+
+int Rtabmap::cleanupLocalGrids(
+		const std::map<int, Transform> & poses,
+		const cv::Mat & map,
+		float xMin,
+		float yMin,
+		float cellSize,
+		int cropRadius,
+		bool filterScans)
+{
+	if(_memory)
+	{
+		return _memory->cleanupLocalGrids(
+				poses,
+				map,
+				xMin,
+				yMin,
+				cellSize,
+				cropRadius,
+				filterScans);
+	}
+	return -1;
+}
+
 int Rtabmap::refineLinks()
 {
 	if(!_rgbdSlamMode)

guilib/src/MainWindow.cpp

@@ -6931,7 +6931,7 @@ void MainWindow::downloadAllClouds()
 	items.append("Global map not optimized");
 
 	bool ok;
-	QString item = QInputDialog::getItem(this, tr("Download map"), tr("Options:"), items, 2, false, &ok);
+	QString item = QInputDialog::getItem(this, tr("Download map"), tr("Options:"), items, 0, false, &ok);
 	if(ok)
 	{
 		bool optimized=false, global=false;
@@ -6975,7 +6975,7 @@ void MainWindow::downloadPoseGraph()
 	items.append("Global map not optimized");
 
 	bool ok;
-	QString item = QInputDialog::getItem(this, tr("Download graph"), tr("Options:"), items, 2, false, &ok);
+	QString item = QInputDialog::getItem(this, tr("Download graph"), tr("Options:"), items, 0, false, &ok);
 	if(ok)
 	{
 		bool optimized=false, global=false;

tools/CMakeLists.txt

@@ -14,6 +14,7 @@ ADD_SUBDIRECTORY( Export )
 ADD_SUBDIRECTORY( Report )
 ADD_SUBDIRECTORY( Info )
 ADD_SUBDIRECTORY( CleanupLocalGrids )
+ADD_SUBDIRECTORY( GlobalBundleAdjustment )
 
 IF(OPENCV_NONFREE_FOUND)
 ADD_SUBDIRECTORY( VocabularyComparison )

tools/CleanupLocalGrids/main.cpp

@@ -25,8 +25,10 @@ ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
 SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 */
 
-#include <rtabmap/core/DBDriver.h>
+#include <rtabmap/core/Rtabmap.h>
+#include <rtabmap/core/Memory.h>
 #include <rtabmap/utilite/UFile.h>
+#include <rtabmap/utilite/UTimer.h>
 #include <rtabmap/core/util3d_transforms.h>
 
 using namespace rtabmap;
@@ -57,7 +59,7 @@ void showUsage()
 int main(int argc, char * argv[])
 {
 	ULogger::setType(ULogger::kTypeConsole);
-	ULogger::setLevel(ULogger::kError);
+	ULogger::setLevel(ULogger::kInfo);
 
 	if(argc < 2)
 	{
@@ -102,187 +104,34 @@ int main(int argc, char * argv[])
 
 	// Get parameters
 	ParametersMap parameters;
-	DBDriver * driver = DBDriver::create();
-	if(driver->openConnection(dbPath))
+	Rtabmap rtabmap;
+	rtabmap.init(ParametersMap(), dbPath, true);
+
+	float xMin, yMin, cellSize;
+	cv::Mat map = rtabmap.getMemory()->load2DMap(xMin, yMin, cellSize);
+	if(map.empty())
 	{
-		float xMin, yMin, cellSize;
-		cv::Mat map = driver->load2DMap(xMin, yMin, cellSize);
-		if(map.empty())
-		{
-			UERROR("Database %s doesn't have optimized 2d map saved in it!", dbPath.c_str());
-			return -1;
-		}
-
-		printf("Options:\n");
-		printf("  --radius:  %d cell(s) (cell size=%.3fm)\n", cropRadius, cellSize);
-		printf("  --scan: %s\n", filterScans?"true":"false");
-
-		Transform lastLocalizationPose;
-		std::map<int, Transform> poses = driver->loadOptimizedPoses(&lastLocalizationPose);
-		if(poses.empty() || poses.lower_bound(1) == poses.end())
-		{
-			UERROR("Database %s doesn't have optimized poses saved in it!", dbPath.c_str());
-			return -1;
-		}
-
-		int maxPoses = 0;
-		for(std::map<int, Transform>::iterator iter=poses.lower_bound(1); iter!=poses.end(); ++iter)
-		{
-			++maxPoses;
-		}
-
-		printf("Processing %d grids...\n", maxPoses);
-		int processedGrids = 1;
-		for(std::map<int, Transform>::iterator iter=poses.lower_bound(1); iter!=poses.end(); ++iter, ++processedGrids)
-		{
-			// local grid
-			cv::Mat gridGround;
-			cv::Mat gridObstacles;
-			cv::Mat gridEmpty;
-
-			// scan
-			SensorData data;
-			driver->getNodeData(iter->first, data);
-			LaserScan scan;
-			data.uncompressData(0,0,&scan,0,&gridGround,&gridObstacles,&gridEmpty);
-
-			if(!gridObstacles.empty())
-			{
-				cv::Mat filtered = cv::Mat(1, gridObstacles.cols, gridObstacles.type());
-				int oi = 0;
-				for(int i=0; i<gridObstacles.cols; ++i)
-				{
-					const float * ptr = gridObstacles.ptr<float>(0, i);
-					cv::Point3f pt(ptr[0], ptr[1], gridObstacles.channels()==2?0:ptr[2]);
-					pt = util3d::transformPoint(pt, iter->second);
-
-					int x = int((pt.x - xMin) / cellSize + 0.5f);
-					int y = int((pt.y - yMin) / cellSize + 0.5f);
-
-					if(x>=0 && x<map.cols &&
-					   y>=0 && y<map.rows)
-					{
-						bool obstacleDetected = false;
-
-						for(int j=-cropRadius; j<=cropRadius && !obstacleDetected; ++j)
-						{
-							for(int k=-cropRadius; k<=cropRadius && !obstacleDetected; ++k)
-							{
-								if(x+j>=0 && x+j<map.cols &&
-								   y+k>=0 && y+k<map.rows &&
-								   map.at<unsigned char>(y+k,x+j) == 100)
-								{
-									obstacleDetected = true;
-								}
-							}
-						}
-
-						if(map.at<unsigned char>(y,x) != 0 || obstacleDetected)
-						{
-							// Verify that we don't have an obstacle on neighbor cells
-							cv::Mat(gridObstacles, cv::Range::all(), cv::Range(i,i+1)).copyTo(cv::Mat(filtered, cv::Range::all(), cv::Range(oi,oi+1)));
-							++oi;
-						}
-					}
-				}
-
-				if(oi != gridObstacles.cols)
-				{
-					printf("Grid id=%d (%d/%d) filtered %d -> %d\n", iter->first, processedGrids, maxPoses, gridObstacles.cols, oi);
-
-					// update
-					driver->updateOccupancyGrid(iter->first,
-							gridGround,
-							cv::Mat(filtered, cv::Range::all(), cv::Range(0, oi)),
-							gridEmpty,
-							cellSize,
-							data.gridViewPoint());
-				}
-			}
-
-			if(filterScans && !scan.isEmpty())
-			{
-				Transform mapToScan = iter->second * scan.localTransform();
-
-				cv::Mat filtered = cv::Mat(1, scan.size(), scan.dataType());
-				int oi = 0;
-				for(int i=0; i<scan.size(); ++i)
-				{
-					const float * ptr = scan.data().ptr<float>(0, i);
-					cv::Point3f pt(ptr[0], ptr[1], scan.is2d()?0:ptr[2]);
-					pt = util3d::transformPoint(pt, mapToScan);
-
-					int x = int((pt.x - xMin) / cellSize + 0.5f);
-					int y = int((pt.y - yMin) / cellSize + 0.5f);
-
-					if(x>=0 && x<map.cols &&
-					   y>=0 && y<map.rows)
-					{
-						bool obstacleDetected = false;
-
-						for(int j=-cropRadius; j<=cropRadius && !obstacleDetected; ++j)
-						{
-							for(int k=-cropRadius; k<=cropRadius && !obstacleDetected; ++k)
-							{
-								if(x+j>=0 && x+j<map.cols &&
-								   y+k>=0 && y+k<map.rows &&
-								   map.at<unsigned char>(y+k,x+j) == 100)
-								{
-									obstacleDetected = true;
-								}
-							}
-						}
-
-						if(map.at<unsigned char>(y,x) != 0 || obstacleDetected)
-						{
-							// Verify that we don't have an obstacle on neighbor cells
-							cv::Mat(scan.data(), cv::Range::all(), cv::Range(i,i+1)).copyTo(cv::Mat(filtered, cv::Range::all(), cv::Range(oi,oi+1)));
-							++oi;
-						}
-					}
-				}
-
-				if(oi != scan.size())
-				{
-					printf("Scan id=%d (%d/%d) filtered %d -> %d\n", iter->first, processedGrids, maxPoses, (int)scan.size(), oi);
-
-					// update
-					if(scan.angleIncrement()!=0)
-					{
-						// copy meta data
-						scan = LaserScan(
-								cv::Mat(filtered, cv::Range::all(), cv::Range(0, oi)),
-								scan.format(),
-								scan.rangeMin(),
-								scan.rangeMax(),
-								scan.angleMin(),
-								scan.angleMax(),
-								scan.angleIncrement(),
-								scan.localTransform());
-					}
-					else
-					{
-						// copy meta data
-						scan = LaserScan(
-								cv::Mat(filtered, cv::Range::all(), cv::Range(0, oi)),
-								scan.maxPoints(),
-								scan.rangeMax(),
-								scan.format(),
-								scan.localTransform());
-					}
-					driver->updateLaserScan(iter->first, scan);
-				}
-			}
-		}
-	}
-	else
-	{
-		UERROR("Cannot open database %s!", dbPath.c_str());
+		UERROR("Database %s doesn't have optimized 2d map saved in it!", dbPath.c_str());
 		return -1;
 	}
-	driver->closeConnection();
-	delete driver;
-	driver = 0;
+
+	printf("Options:\n");
+	printf("  --radius:  %d cell(s) (cell size=%.3fm)\n", cropRadius, cellSize);
+	printf("  --scan: %s\n", filterScans?"true":"false");
+
+	std::map<int, Transform> poses = rtabmap.getLocalOptimizedPoses();
+	if(poses.empty() || poses.lower_bound(1) == poses.end())
+	{
+		UERROR("Database %s doesn't have optimized poses saved in it!", dbPath.c_str());
+		return -1;
+	}
+
+	UTimer timer;
+	printf("Cleaning grids...\n");
+	int modifiedCells = rtabmap.cleanupLocalGrids(poses, map, xMin, yMin, cellSize, cropRadius, filterScans);
+	printf("Cleanup %d cells! (%fs)\n", modifiedCells, timer.ticks());
+
+	rtabmap.close();
 
 	printf("Done!\n");
 	return 0;

tools/GlobalBundleAdjustment/CMakeLists.txt

@@ -0,0 +1,40 @@
+
+SET(RTABMap_INCLUDE_DIRS 
+    ${PROJECT_SOURCE_DIR}/utilite/include
+	${PROJECT_SOURCE_DIR}/corelib/include
+)
+SET(RTABMap_LIBRARIES 
+    rtabmap_core
+	rtabmap_utilite
+)  
+
+if(POLICY CMP0020)
+	cmake_policy(SET CMP0020 NEW)
+endif()
+
+SET(INCLUDE_DIRS
+	${RTABMap_INCLUDE_DIRS}
+    ${OpenCV_INCLUDE_DIRS}
+    ${PCL_INCLUDE_DIRS}
+)
+
+SET(LIBRARIES
+	${RTABMap_LIBRARIES}
+	${OpenCV_LIBRARIES}
+	${PCL_LIBRARIES}
+)
+
+INCLUDE_DIRECTORIES(${INCLUDE_DIRS})
+
+ADD_EXECUTABLE(globalBundleAdjustment main.cpp)
+  
+TARGET_LINK_LIBRARIES(globalBundleAdjustment ${LIBRARIES})
+
+SET_TARGET_PROPERTIES( globalBundleAdjustment 
+  PROPERTIES OUTPUT_NAME ${PROJECT_PREFIX}-globalBundleAdjustment)
+
+INSTALL(TARGETS globalBundleAdjustment
+		RUNTIME DESTINATION "${CMAKE_INSTALL_BINDIR}" COMPONENT runtime
+		BUNDLE DESTINATION "${CMAKE_BUNDLE_LOCATION}" COMPONENT runtime)
+
+

tools/GlobalBundleAdjustment/main.cpp

@@ -0,0 +1,138 @@
+/*
+Copyright (c) 2010-2016, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
+All rights reserved.
+
+Redistribution and use in source and binary forms, with or without
+modification, are permitted provided that the following conditions are met:
+    * Redistributions of source code must retain the above copyright
+      notice, this list of conditions and the following disclaimer.
+    * Redistributions in binary form must reproduce the above copyright
+      notice, this list of conditions and the following disclaimer in the
+      documentation and/or other materials provided with the distribution.
+    * Neither the name of the Universite de Sherbrooke nor the
+      names of its contributors may be used to endorse or promote products
+      derived from this software without specific prior written permission.
+
+THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
+ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY
+DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+*/
+
+#include <rtabmap/core/DBDriver.h>
+#include <rtabmap/core/Rtabmap.h>
+#include <rtabmap/core/Optimizer.h>
+#include <rtabmap/utilite/UTimer.h>
+#include <rtabmap/utilite/UFile.h>
+#include <rtabmap/utilite/UStl.h>
+
+using namespace rtabmap;
+
+void showUsage()
+{
+	printf("\nUsage:\n"
+			"rtabmap-globalBundleAdjustment database.db\n"
+			"\n%s", Parameters::showUsage());
+	exit(1);
+}
+
+int main(int argc, char * argv[])
+{
+	ULogger::setType(ULogger::kTypeConsole);
+	ULogger::setLevel(ULogger::kError);
+
+	if(argc < 2)
+	{
+		showUsage();
+	}
+
+	for(int i=1; i<argc-1; ++i)
+	{
+		if(std::strcmp(argv[i], "--help") == 0)
+		{
+			showUsage();
+		}
+	}
+	ParametersMap inputParams = Parameters::parseArguments(argc,  argv);
+
+	std::string dbPath = argv[argc-1];
+	if(!UFile::exists(dbPath))
+	{
+		printf("Database %s doesn't exist!\n", dbPath.c_str());
+	}
+
+	// Get parameters
+	ParametersMap parameters;
+	DBDriver * driver = DBDriver::create();
+	if(driver->openConnection(dbPath))
+	{
+		if(uStrNumCmp(driver->getDatabaseVersion(), "0.17.0")<0)
+		{
+			printf("Database is too old (%s), we cannot save back optimized poses. "
+					"Consider upgrading the database with:\n"
+					"rtabmap-reprocess --Db/TargetVersion \"\" \"%s\" \"output.db\"\n",
+					driver->getDatabaseVersion().c_str(),
+					dbPath.c_str());
+			driver->closeConnection(false);
+			delete driver;
+			return -1;
+		}
+
+		parameters = driver->getLastParameters();
+		// This will force rtabmap_ros to regenerate the global occupancy grid if there was one
+		driver->save2DMap(cv::Mat(), 0, 0, 0);
+		driver->saveOptimizedMesh(cv::Mat());
+		driver->closeConnection(false);
+	}
+	else
+	{
+		UERROR("Cannot open database %s!", dbPath.c_str());
+	}
+	delete driver;
+
+	for(ParametersMap::iterator iter=inputParams.begin(); iter!=inputParams.end(); ++iter)
+	{
+		printf("Added custom parameter %s=%s\n",iter->first.c_str(), iter->second.c_str());
+	}
+
+	UTimer timer;
+
+	printf("Loading database \"%s\"...\n", dbPath.c_str());
+	// Get the global optimized map
+	Rtabmap rtabmap;
+	uInsert(parameters, inputParams);
+	rtabmap.init(parameters, dbPath);
+	printf("Loading database \"%s\"... done (%fs).\n", dbPath.c_str(), timer.ticks());
+
+	std::map<int, Signature> nodes;
+	std::map<int, Transform> optimizedPoses;
+	std::multimap<int, Link> links;
+	printf("Optimizing the map...\n");
+	rtabmap.getGraph(optimizedPoses, links, true, true, &nodes, true, true, true, true);
+	printf("Optimizing the map... done (%fs, poses=%d).\n", timer.ticks(), (int)optimizedPoses.size());
+
+	printf("Global bundle adjustment...\n");
+	Optimizer * optimizer = Optimizer::create(Optimizer::kTypeG2O, parameters);
+	optimizedPoses = optimizer->optimizeBA(optimizedPoses.lower_bound(1)->first, optimizedPoses, links, nodes, true);
+	delete optimizer;
+	printf("Global bundle adjustment... done (%fs).\n", timer.ticks());
+
+	if(!optimizedPoses.empty())
+	{
+		rtabmap.setOptimizedPoses(optimizedPoses, links);
+	}
+	else
+	{
+		UERROR("Returned empty poses!");
+	}
+
+	rtabmap.close();
+
+	return 0;
+}

tools/Reprocess/main.cpp

@@ -68,10 +68,11 @@ void showUsage()
 			"                       arguments, they overwrite those in config file and the database.\n"
 			"     -start #    Start from this node ID.\n"
 			"     -stop #     Last node to process.\n"
-			"     -g2         Assemble 2D occupancy grid map and save it to \"[output]_map.pgm\".\n"
+			"     -g2         Assemble 2D occupancy grid map and save it to \"[output]_map.pgm\". Use with -db to save in database.\n"
 			"     -g3         Assemble 3D cloud map and save it to \"[output]_map.pcd\".\n"
-			"     -o2         Assemble OctoMap 2D projection and save it to \"[output]_octomap.pgm\".\n"
+			"     -o2         Assemble OctoMap 2D projection and save it to \"[output]_octomap.pgm\". Use with -db to save in database.\n"
 			"     -o3         Assemble OctoMap 3D cloud and save it to \"[output]_octomap.pcd\".\n"
+			"     -db         Save assembled 2D occupancy grid in database instead of a file.\n"
 			"     -p          Save odometry and localization poses (*.g2o).\n"
 			"     -scan_from_depth    Generate scans from depth images (overwrite previous\n"
 			"                         scans if they exist).\n"
@@ -211,6 +212,7 @@ int main(int argc, char * argv[])
 		showUsage();
 	}
 
+	bool save2DMap = false;
 	bool assemble2dMap = false;
 	bool assemble3dMap = false;
 	bool assemble2dOctoMap = false;
@@ -300,6 +302,11 @@ int main(int argc, char * argv[])
 			exportPoses = true;
 			printf("Odometry trajectory and localization poses will be exported in g2o format (-p option).\n");
 		}
+		else if(strcmp(argv[i], "-db") == 0 || strcmp(argv[i], "--db") == 0)
+		{
+			save2DMap = true;
+			printf("2D occupancy grid will be saved in database (-db option).\n");
+		}
 		else if(strcmp(argv[i], "-g2") == 0 || strcmp(argv[i], "--g2") == 0)
 		{
 			assemble2dMap = true;
@@ -856,36 +863,50 @@ int main(int argc, char * argv[])
 		cv::Mat map = grid.getMap(xMin, yMin);
 		if(!map.empty())
 		{
-			cv::Mat map8U(map.rows, map.cols, CV_8U);
-			//convert to gray scaled map
-			for (int i = 0; i < map.rows; ++i)
+			if(save2DMap)
 			{
-				for (int j = 0; j < map.cols; ++j)
+				DBDriver * driver = DBDriver::create();
+				if(driver->openConnection(outputDatabasePath))
 				{
-					char v = map.at<char>(i, j);
-					unsigned char gray;
-					if(v == 0)
-					{
-						gray = 178;
-					}
-					else if(v == 100)
-					{
-						gray = 0;
-					}
-					else // -1
-					{
-						gray = 89;
-					}
-					map8U.at<unsigned char>(i, j) = gray;
+					driver->save2DMap(map, xMin, yMin, grid.getCellSize());
+					printf("Saving occupancy grid to database... done!\n");
 				}
-			}
-			if(cv::imwrite(outputPath, map8U))
-			{
-				printf("Saving occupancy grid \"%s\"... done!\n", outputPath.c_str());
+				delete driver;
 			}
 			else
 			{
-				printf("Saving occupancy grid \"%s\"... failed!\n", outputPath.c_str());
+				cv::Mat map8U(map.rows, map.cols, CV_8U);
+				//convert to gray scaled map
+				for (int i = 0; i < map.rows; ++i)
+				{
+					for (int j = 0; j < map.cols; ++j)
+					{
+						char v = map.at<char>(i, j);
+						unsigned char gray;
+						if(v == 0)
+						{
+							gray = 178;
+						}
+						else if(v == 100)
+						{
+							gray = 0;
+						}
+						else // -1
+						{
+							gray = 89;
+						}
+						map8U.at<unsigned char>(i, j) = gray;
+					}
+				}
+
+				if(cv::imwrite(outputPath, map8U))
+				{
+					printf("Saving occupancy grid \"%s\"... done!\n", outputPath.c_str());
+				}
+				else
+				{
+					printf("Saving occupancy grid \"%s\"... failed!\n", outputPath.c_str());
+				}
 			}
 		}
 		else
@@ -937,36 +958,49 @@ int main(int argc, char * argv[])
 		cv::Mat map = octomap.createProjectionMap(xMin, yMin, cellSize);
 		if(!map.empty())
 		{
-			cv::Mat map8U(map.rows, map.cols, CV_8U);
-			//convert to gray scaled map
-			for (int i = 0; i < map.rows; ++i)
+			if(save2DMap)
 			{
-				for (int j = 0; j < map.cols; ++j)
+				DBDriver * driver = DBDriver::create();
+				if(driver->openConnection(outputDatabasePath))
 				{
-					char v = map.at<char>(i, j);
-					unsigned char gray;
-					if(v == 0)
-					{
-						gray = 178;
-					}
-					else if(v == 100)
-					{
-						gray = 0;
-					}
-					else // -1
-					{
-						gray = 89;
-					}
-					map8U.at<unsigned char>(i, j) = gray;
+					driver->save2DMap(map, xMin, yMin, cellSize);
+					printf("Saving occupancy grid to database... done!\n");
 				}
-			}
-			if(cv::imwrite(outputPath, map8U))
-			{
-				printf("Saving octomap 2D projection \"%s\"... done!\n", outputPath.c_str());
+				delete driver;
 			}
 			else
 			{
-				printf("Saving octomap 2D projection \"%s\"... failed!\n", outputPath.c_str());
+				cv::Mat map8U(map.rows, map.cols, CV_8U);
+				//convert to gray scaled map
+				for (int i = 0; i < map.rows; ++i)
+				{
+					for (int j = 0; j < map.cols; ++j)
+					{
+						char v = map.at<char>(i, j);
+						unsigned char gray;
+						if(v == 0)
+						{
+							gray = 178;
+						}
+						else if(v == 100)
+						{
+							gray = 0;
+						}
+						else // -1
+						{
+							gray = 89;
+						}
+						map8U.at<unsigned char>(i, j) = gray;
+					}
+				}
+				if(cv::imwrite(outputPath, map8U))
+				{
+					printf("Saving octomap 2D projection \"%s\"... done!\n", outputPath.c_str());
+				}
+				else
+				{
+					printf("Saving octomap 2D projection \"%s\"... failed!\n", outputPath.c_str());
+				}
 			}
 		}
 		else