Added OdomBow/FixedLocalMapPath parameter

corelib/include/rtabmap/core/Memory.h

@@ -83,7 +83,7 @@ public:
 	std::list<int> forget(const std::set<int> & ignoredIds = std::set<int>());
 	std::set<int> reactivateSignatures(const std::list<int> & ids, unsigned int maxLoaded, double & timeDbAccess);
 
-	std::list<int> cleanup(const std::list<int> & ignoredIds = std::list<int>());
+	int cleanup();
 	void emptyTrash();
 	void joinTrashThread();
 	bool addLink(const Link & link);

corelib/include/rtabmap/core/Odometry.h

@@ -118,6 +118,7 @@ private:
 private:
 	//Parameters
 	int _localHistoryMaxSize;
+	std::string _fixedLocalMapPath;
 
 	Memory * _memory;
 	std::multimap<int, pcl::PointXYZ> localMap_;

corelib/include/rtabmap/core/Parameters.h

@@ -339,6 +339,7 @@ class RTABMAP_EXP Parameters
 	RTABMAP_PARAM(OdomBow, LocalHistorySize,       int, 1000,      "Local history size: If > 0 (example 5000), the odometry will maintain a local map of X maximum words.");
 	RTABMAP_PARAM(OdomBow, NNType,                 int, 3, 	    "kNNFlannNaive=0, kNNFlannKdTree=1, kNNFlannLSH=2, kNNBruteForce=3, kNNBruteForceGPU=4");
 	RTABMAP_PARAM(OdomBow, NNDR,                   float, 0.8,  "NNDR: nearest neighbor distance ratio.");
+	RTABMAP_PARAM_STR(OdomBow, FixedLocalMapPath,   "",         "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
 	RTABMAP_PARAM(OdomMono, InitMinFlow,            float, 100,  "Minimum optical flow required for the initialization step.");

corelib/src/Memory.cpp

@@ -1487,10 +1487,10 @@ std::list<int> Memory::forget(const std::set<int> & ignoredIds)
 }
 
 
-std::list<int> Memory::cleanup(const std::list<int> & ignoredIds)
+int Memory::cleanup()
 {
 	UDEBUG("");
-	std::list<int> signaturesRemoved;
+	int signatureRemoved = 0;
 
 	// bad signature
 	if(_lastSignature && ((_lastSignature->isBadSignature() && _badSignaturesIgnored) || !_incrementalMemory))
@@ -1499,11 +1499,11 @@ std::list<int> Memory::cleanup(const std::list<int> & ignoredIds)
 		{
 			UDEBUG("Bad signature! %d", _lastSignature->id());
 		}
-		signaturesRemoved.push_back(_lastSignature->id());
+		signatureRemoved = _lastSignature->id();
 		moveToTrash(_lastSignature, _incrementalMemory);
 	}
 
-	return signaturesRemoved;
+	return signatureRemoved;
 }
 
 void Memory::emptyTrash()

corelib/src/Odometry.cpp

@@ -162,9 +162,10 @@ Transform Odometry::process(const SensorData & data, OdometryInfo * info)
 	}
 
 	UASSERT(!data.imageRaw().empty());
-	if(dynamic_cast<OdometryMono*>(this) == 0)
+	if(dynamic_cast<OdometryMono*>(this) == 0 && dynamic_cast<OdometryBOW*>(this) == 0)
 	{
-		UASSERT(!data.depthOrRightRaw().empty());
+		UERROR("Depth or stereo images required with the odometry selected!");
+		return Transform();
 	}
 
 	if(!data.stereoCameraModel().isValid() &&

corelib/src/OdometryBOW.cpp

@@ -32,6 +32,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #include "rtabmap/core/util3d_transforms.h"
 #include "rtabmap/core/util3d_registration.h"
 #include "rtabmap/core/util3d_correspondences.h"
+#include "rtabmap/core/Graph.h"
 #include "rtabmap/core/VWDictionary.h"
 #include "rtabmap/utilite/ULogger.h"
 #include "rtabmap/utilite/UTimer.h"
@@ -49,9 +50,12 @@ namespace rtabmap {
 OdometryBOW::OdometryBOW(const ParametersMap & parameters) :
 	Odometry(parameters),
 	_localHistoryMaxSize(Parameters::defaultOdomBowLocalHistorySize()),
+	_fixedLocalMapPath(Parameters::defaultOdomBowFixedLocalMapPath()),
 	_memory(0)
 {
+	UDEBUG("");
 	Parameters::parse(parameters, Parameters::kOdomBowLocalHistorySize(), _localHistoryMaxSize);
+	Parameters::parse(parameters, Parameters::kOdomBowFixedLocalMapPath(), _fixedLocalMapPath);
 
 	ParametersMap customParameters;
 	customParameters.insert(ParametersPair(Parameters::kKpMaxDepth(), uNumber2Str(this->getMaxDepth())));
@@ -101,10 +105,71 @@ OdometryBOW::OdometryBOW(const ParametersMap & parameters) :
 		}
 	}
 
-	_memory = new Memory(customParameters);
-	if(!_memory->init("", false, ParametersMap()))
+	if(_fixedLocalMapPath.empty())
 	{
-		UERROR("Error initializing the memory for BOW Odometry.");
+		_memory = new Memory(customParameters);
+		if(!_memory->init("", false, ParametersMap()))
+		{
+			UERROR("Error initializing the memory for BOW Odometry.");
+		}
+	}
+	else
+	{
+		UINFO("Init odometry from a fixed database: \"%s\"", _fixedLocalMapPath.c_str());
+		// init the local map with a all 3D features contained in the database
+		customParameters.insert(ParametersPair(Parameters::kMemIncrementalMemory(), "false"));
+		customParameters.insert(ParametersPair(Parameters::kMemInitWMWithAllNodes(), "true"));
+		_memory = new Memory(customParameters);
+		if(!_memory->init(_fixedLocalMapPath, false, ParametersMap()))
+		{
+			UERROR("Error initializing the memory for BOW Odometry.");
+		}
+		else
+		{
+			// get the graph
+			std::map<int, int> ids = _memory->getNeighborsId(_memory->getLastSignatureId(), 0, -1);
+			std::map<int, Transform> poses;
+			std::multimap<int, Link> links;
+			_memory->getMetricConstraints(uKeysSet(ids), poses, links, true);
+
+			if(poses.size())
+			{
+				//optimize the graph
+				graph::TOROOptimizer optimizer;
+				std::map<int, Transform> optimizedPoses = optimizer.optimize(poses.begin()->first, poses, links);
+
+				// fill the local map
+				for(std::map<int, Transform>::iterator posesIter=optimizedPoses.begin();
+					posesIter!=optimizedPoses.end();
+					++posesIter)
+				{
+					const Signature * s = _memory->getSignature(posesIter->first);
+					if(s)
+					{
+						// Transform 3D points accordingly to pose and add them to local map
+						const std::multimap<int, pcl::PointXYZ> & words3D = s->getWords3();
+						for(std::multimap<int, pcl::PointXYZ>::const_iterator pointsIter=words3D.begin();
+							pointsIter!=words3D.end();
+							++pointsIter)
+						{
+							if(!uContains(localMap_, pointsIter->first))
+							{
+								localMap_.insert(std::make_pair(pointsIter->first, util3d::transformPoint(pointsIter->second, posesIter->second)));
+							}
+						}
+					}
+				}
+			}
+			else
+			{
+				UERROR("No pose loaded from database \"%s\"", _fixedLocalMapPath.c_str());
+			}
+		}
+		if((int)localMap_.size() < this->getMinInliers() || localMap_.size() == 0)
+		{
+			UERROR("The loaded fixed map from \"%s\" is too small! Only %d unique features loaded. Odometry won't be computed!",
+					_fixedLocalMapPath.c_str(), (int)localMap_.size());
+		}
 	}
 }
 
@@ -117,9 +182,16 @@ OdometryBOW::~OdometryBOW()
 
 void OdometryBOW::reset(const Transform & initialPose)
 {
-	Odometry::reset(initialPose);
-	_memory->init("", false, ParametersMap());
-	localMap_.clear();
+	if(_fixedLocalMapPath.empty())
+	{
+		Odometry::reset(initialPose);
+		_memory->init("", false, ParametersMap());
+		localMap_.clear();
+	}
+	else
+	{
+		UWARN("Odometry cannot be reset when a fixed local map is set.");
+	}
 }
 
 // return not null transform if odometry is correctly computed
@@ -140,7 +212,6 @@ Transform OdometryBOW::computeTransform(
 	int correspondences = 0;
 	int nFeatures = 0;
 
-	const Signature * previousSignature = _memory->getLastWorkingSignature();
 	if(_memory->update(data))
 	{
 		const Signature * newSignature = _memory->getLastWorkingSignature();
@@ -153,7 +224,7 @@ Transform OdometryBOW::computeTransform(
 			}
 		}
 
-		if(previousSignature && newSignature)
+		if(localMap_.size() && newSignature)
 		{
 			Transform transform;
 			if((int)localMap_.size() >= this->getMinInliers())
@@ -257,6 +328,10 @@ Transform OdometryBOW::computeTransform(
 									double median_error_sqr = (double)errorSqrdDists[errorSqrdDists.size () >> 1];
 									variance = 2.1981 * median_error_sqr;
 								}
+								else
+								{
+									variance = 1;
+								}
 							}
 							else
 							{
@@ -364,9 +439,10 @@ Transform OdometryBOW::computeTransform(
 			{
 				_memory->deleteLocation(newSignature->id());
 			}
-			else
+			else if(_fixedLocalMapPath.empty())
 			{
 				output = transform;
+
 				// remove words if history max size is reached
 				while(localMap_.size() && (int)localMap_.size() > _localHistoryMaxSize && _memory->getStMem().size()>1)
 				{
@@ -410,14 +486,18 @@ Transform OdometryBOW::computeTransform(
 					}
 				}
 			}
+			else
+			{
+				// fixed local map, just delete the new signature
+				output = transform;
+				_memory->deleteLocation(newSignature->id());
+			}
 		}
-		else if(!previousSignature && newSignature)
+		else if(newSignature)
 		{
-			localMap_.clear();
-
 			int count = 0;
 			std::list<int> uniques = uUniqueKeys(newSignature->getWords3());
-			if((int)uniques.size() >= this->getMinInliers())
+			if(_fixedLocalMapPath.empty() && (int)uniques.size() >= this->getMinInliers())
 			{
 				output.setIdentity();
 

corelib/src/OdometryThread.cpp

@@ -105,7 +105,7 @@ void OdometryThread::mainLoop()
 
 void OdometryThread::addData(const SensorData & data)
 {
-	if(dynamic_cast<OdometryMono*>(_odometry) == 0)
+	if(dynamic_cast<OdometryMono*>(_odometry) == 0 && dynamic_cast<OdometryBOW*>(_odometry) == 0)
 	{
 		if(data.imageRaw().empty() || data.depthOrRightRaw().empty() || (data.cameraModels().size()==0 && !data.stereoCameraModel().isValid()))
 		{
@@ -115,6 +115,7 @@ void OdometryThread::addData(const SensorData & data)
 	}
 	else
 	{
+		// Mono and BOW can accept RGB only
 		if(data.imageRaw().empty() || (data.cameraModels().size()==0 && !data.stereoCameraModel().isValid()))
 		{
 			ULOGGER_ERROR("Missing some information (image empty or missing calibration)!?");

corelib/src/Rtabmap.cpp

@@ -2141,32 +2141,39 @@ bool Rtabmap::process(
 		lastSignatureData = *signature;
 	}
 
-	//By default, remove all signatures with a loop closure link if they are not in reactivateIds
-	//This will also remove rehearsed signatures
-	std::list<int> signaturesRemoved = _memory->cleanup();
+	// remove last signature if the memory is not incremental or is a bad signature (if bad signatures are ignored)
+	std::list<int> signaturesRemoved;
+	int signatureRemoved = _memory->cleanup();
+	if(signatureRemoved)
+	{
+		signaturesRemoved.push_back(signatureRemoved);
+	}
 
 	// If this option activated, add new nodes only if there are linked with a previous map.
 	// Used when rtabmap is first started, it will wait a
 	// global loop closure detection before starting the new map,
 	// otherwise it deletes the current node.
-	if(_startNewMapOnLoopClosure &&
-		_memory->isIncremental() &&              // only in mapping mode
-		signature->getLinks().size() == 0 &&     // alone in the current map
-		_memory->getWorkingMem().size()>1)       // The working memory should not be empty
+	if(signatureRemoved != lastSignatureData.id())
 	{
-		UWARN("Ignoring location %d because a global loop closure is required before starting a new map!",
-				signature->id());
-		signaturesRemoved.push_back(signature->id());
-		_memory->deleteLocation(signature->id());
-	}
-	else if(smallDisplacement && _loopClosureHypothesis.first == 0 && lastLocalSpaceClosureId == 0)
-	{
-		// Don't delete the location if a loop closure is detected
-		UINFO("Ignoring location %d because the displacement is too small! (d=%f a=%f)",
-			  signature->id(), _rgbdLinearUpdate, _rgbdAngularUpdate);
-		// If there is a too small displacement, remove the node
-		signaturesRemoved.push_back(signature->id());
-		_memory->deleteLocation(signature->id());
+		if(_startNewMapOnLoopClosure &&
+			_memory->isIncremental() &&              // only in mapping mode
+			signature->getLinks().size() == 0 &&     // alone in the current map
+			_memory->getWorkingMem().size()>1)       // The working memory should not be empty
+		{
+			UWARN("Ignoring location %d because a global loop closure is required before starting a new map!",
+					signature->id());
+			signaturesRemoved.push_back(signature->id());
+			_memory->deleteLocation(signature->id());
+		}
+		else if(smallDisplacement && _loopClosureHypothesis.first == 0 && lastLocalSpaceClosureId == 0)
+		{
+			// Don't delete the location if a loop closure is detected
+			UINFO("Ignoring location %d because the displacement is too small! (d=%f a=%f)",
+				  signature->id(), _rgbdLinearUpdate, _rgbdAngularUpdate);
+			// If there is a too small displacement, remove the node
+			signaturesRemoved.push_back(signature->id());
+			_memory->deleteLocation(signature->id());
+		}
 	}
 
 	// Pass this point signature should not be used, since it could have been transferred...