Increased database version to 0.8.11 (new Depth.data2d_max_pts column). Updated how local loop closure detection in space is done. Update GraphViewer with local radius ellipse and current goal node color. MainWindow saving/loading figures automatically accordingly to the previous session saved.

2026-09-02 01:20:25 +08:00 · 2015-05-03 18:16:55 -04:00
parent abb7eb15ac
commit d7030e0e38
28 changed files with 927 additions and 366 deletions
--- a/corelib/src/DBReader.cpp
+++ b/corelib/src/DBReader.cpp
@@ -46,14 +46,26 @@ DBReader::DBReader(const std::string & databasePath,
 				   float frameRate,
 				   bool odometryIgnored,
 				   bool ignoreGoalDelay) :
-	_path(databasePath),
+	_paths(uSplit(databasePath, ';')),
 	_frameRate(frameRate),
 	_odometryIgnored(odometryIgnored),
 	_ignoreGoalDelay(ignoreGoalDelay),
 	_dbDriver(0),
 	_currentId(_ids.end())
 {
+}

+DBReader::DBReader(const std::list<std::string> & databasePaths,
+				   float frameRate,
+				   bool odometryIgnored,
+				   bool ignoreGoalDelay) :
+   _paths(databasePaths),
+   _frameRate(frameRate),
+	_odometryIgnored(odometryIgnored),
+	_ignoreGoalDelay(ignoreGoalDelay),
+	_dbDriver(0),
+	_currentId(_ids.end())
+{
 }

 DBReader::~DBReader()
@@ -77,9 +89,16 @@ bool DBReader::init(int startIndex)
 	_currentId=_ids.end();
 	_previousStamp = 0;

-	if(!UFile::exists(_path))
+	if(_paths.size() == 0)
 	{
-		UERROR("Database path does not exist (%s)", _path.c_str());
+		UERROR("No database path set...");
+		return false;
+	}
+
+	std::string path = _paths.front();
+	if(!UFile::exists(path))
+	{
+		UERROR("Database path does not exist (%s)", path.c_str());
 		return false;
 	}

@@ -91,9 +110,9 @@ bool DBReader::init(int startIndex)
 		UERROR("Driver doesn't exist.");
 		return false;
 	}
-	if(!_dbDriver->openConnection(_path))
+	if(!_dbDriver->openConnection(path))
 	{
-		UERROR("Can't open database %s", _path.c_str());
+		UERROR("Can't open database %s", path.c_str());
 		delete _dbDriver;
 		_dbDriver = 0;
 		return false;
@@ -197,8 +216,23 @@ void DBReader::mainLoop()
 	else if(!this->isKilled())
 	{
 		UINFO("no more images...");
-		this->kill();
-		this->post(new CameraEvent());
+		if(_paths.size() > 1)
+		{
+			_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();
+				this->post(new CameraEvent());
+			}
+		}
+		else
+		{
+			this->kill();
+			this->post(new CameraEvent());
+		}
+
 	}

 }
--- a/corelib/src/Graph.cpp
+++ b/corelib/src/Graph.cpp
@@ -1122,7 +1122,7 @@ public:
 	rtabmap::Transform pose() const {return pose_;}
 	float distFrom(const rtabmap::Transform & pose) const
 	{
-		return pose_.getDistanceSquared(pose); // use sqrt distance
+		return pose_.getDistance(pose); // use sqrt distance
 	}

 	void setClosed(bool closed) {closed_ = closed;}
@@ -1287,7 +1287,6 @@ int findNearestNode(
 std::map<int, float> getNodesInRadius(
 		int nodeId,
 		const std::map<int, Transform> & nodes,
-		int maxNearestNeighbors,
 		float radius)
 {
 	UASSERT(uContains(nodes, nodeId));
@@ -1323,7 +1322,7 @@ std::map<int, float> getNodesInRadius(
 		std::vector<int> ind;
 		std::vector<float> dist;
 		pcl::PointXYZ pt(fromT.x(), fromT.y(), fromT.z());
-		kdTree->radiusSearch(pt, radius, ind, dist, maxNearestNeighbors);
+		kdTree->radiusSearch(pt, radius, ind, dist, 0);
 		for(unsigned int i=0; i<ind.size(); ++i)
 		{
 			if(ind[i] >=0)
@@ -1337,6 +1336,59 @@ std::map<int, float> getNodesInRadius(
 	return foundNodes;
 }

+// return <id, Transform>, excluding query
+std::map<int, Transform> getPosesInRadius(
+		int nodeId,
+		const std::map<int, Transform> & nodes,
+		float radius)
+{
+	UASSERT(uContains(nodes, nodeId));
+	std::map<int, Transform> foundNodes;
+	if(nodes.size() <= 1)
+	{
+		return foundNodes;
+	}
+
+	pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>);
+	cloud->resize(nodes.size());
+	std::vector<int> ids(nodes.size());
+	int oi = 0;
+	for(std::map<int, Transform>::const_iterator iter = nodes.begin(); iter!=nodes.end(); ++iter)
+	{
+		if(iter->first != nodeId)
+		{
+			(*cloud)[oi] = pcl::PointXYZ(iter->second.x(), iter->second.y(), iter->second.z());
+			UASSERT_MSG(pcl::isFinite((*cloud)[oi]), uFormat("Invalid pose (%d) %s", iter->first, iter->second.prettyPrint().c_str()).c_str());
+			ids[oi] = iter->first;
+			++oi;
+		}
+	}
+	cloud->resize(oi);
+	ids.resize(oi);
+
+	Transform fromT = nodes.at(nodeId);
+
+	if(cloud->size())
+	{
+		pcl::search::KdTree<pcl::PointXYZ>::Ptr kdTree(new pcl::search::KdTree<pcl::PointXYZ>);
+		kdTree->setInputCloud(cloud);
+		std::vector<int> ind;
+		std::vector<float> dist;
+		pcl::PointXYZ pt(fromT.x(), fromT.y(), fromT.z());
+		kdTree->radiusSearch(pt, radius, ind, dist, 0);
+		for(unsigned int i=0; i<ind.size(); ++i)
+		{
+			if(ind[i] >=0)
+			{
+				UDEBUG("Inlier %d: %f", ids[ind[i]], sqrt(dist[i]));
+				foundNodes.insert(std::make_pair(ids[ind[i]], nodes.at(ids[ind[i]])));
+			}
+		}
+	}
+	UDEBUG("found nodes=%d", (int)foundNodes.size());
+	return foundNodes;
+}
+
 float computePathLength(
 		const std::vector<std::pair<int, Transform> > & path,
 		unsigned int fromIndex,
--- a/corelib/src/Memory.cpp
+++ b/corelib/src/Memory.cpp
@@ -73,6 +73,7 @@ Memory::Memory(const ParametersMap & parameters) :
 	_localSpaceLinksKeptInWM(Parameters::defaultMemLocalSpaceLinksKeptInWM()),
 	_rehearsalMaxDistance(Parameters::defaultRGBDLinearUpdate()),
 	_rehearsalMaxAngle(Parameters::defaultRGBDAngularUpdate()),
+	_rehearsalWeightIgnoredWhileMoving(Parameters::defaultMemRehearsalWeightIgnoredWhileMoving()),
 	_idCount(kIdStart),
 	_idMapCount(kIdStart),
 	_lastSignature(0),
@@ -399,6 +400,7 @@ void Memory::parseParameters(const ParametersMap & parameters)
 	Parameters::parse(parameters, Parameters::kMemLocalSpaceLinksKeptInWM(), _localSpaceLinksKeptInWM);
 	Parameters::parse(parameters, Parameters::kRGBDLinearUpdate(), _rehearsalMaxDistance);
 	Parameters::parse(parameters, Parameters::kRGBDAngularUpdate(), _rehearsalMaxAngle);
+	Parameters::parse(parameters, Parameters::kMemRehearsalWeightIgnoredWhileMoving(), _rehearsalWeightIgnoredWhileMoving);

 	UASSERT_MSG(_maxStMemSize >= 0, uFormat("value=%d", _maxStMemSize).c_str());
 	UASSERT_MSG(_similarityThreshold >= 0.0f && _similarityThreshold <= 1.0f, uFormat("value=%f", _similarityThreshold).c_str());
@@ -837,14 +839,14 @@ std::map<int, Link> Memory::getLoopClosureLinks(
 // maxCheckedInDatabase = -1 means no limit to check in database (default)
 // maxCheckedInDatabase = 0 means don't check in database
 std::map<int, int> Memory::getNeighborsId(int signatureId,
-		int margin, // 0 means infinite margin
+		int maxGraphDepth, // 0 means infinite margin
 		int maxCheckedInDatabase, // default -1 (no limit)
 		bool incrementMarginOnLoop, // default false
 		bool ignoreLoopIds, // default false
 		double * dbAccessTime
 		) const
 {
-	UASSERT(margin >= 0);
+	UASSERT(maxGraphDepth >= 0);
 	//UDEBUG("signatureId=%d, neighborsMargin=%d", signatureId, margin);
 	if(dbAccessTime)
 	{
@@ -861,9 +863,10 @@ std::map<int, int> Memory::getNeighborsId(int signatureId,
 	std::set<int> nextMargin;
 	nextMargin.insert(signatureId);
 	int m = 0;
-	while((margin == 0 || m < margin) && nextMargin.size())
+	while((maxGraphDepth == 0 || m < maxGraphDepth) && nextMargin.size())
 	{
-		curentMarginList = std::list<int>(nextMargin.begin(), nextMargin.end());
+		// insert more recent first (priority to be loaded first from the database below if set)
+		curentMarginList = std::list<int>(nextMargin.rbegin(), nextMargin.rend());
 		nextMargin.clear();

 		for(std::list<int>::iterator jter = curentMarginList.begin(); jter!=curentMarginList.end(); ++jter)
@@ -927,6 +930,73 @@ std::map<int, int> Memory::getNeighborsId(int signatureId,
 	return ids;
 }

+// return map<Id,sqrdDistance>, including signatureId
+std::map<int, float> Memory::getNeighborsIdRadius(
+		int signatureId,
+		float radius, // 0 means ignore radius
+		const std::map<int, Transform> & optimizedPoses,
+		int maxGraphDepth // 0 means infinite margin
+		) const
+{
+	UASSERT(maxGraphDepth >= 0);
+	UASSERT(uContains(optimizedPoses, signatureId));
+	UASSERT(signatureId > 0);
+	std::map<int, float> ids;
+	std::list<int> curentMarginList;
+	std::set<int> currentMargin;
+	std::set<int> nextMargin;
+	nextMargin.insert(signatureId);
+	int m = 0;
+	Transform referential = optimizedPoses.at(signatureId);
+	UASSERT(!referential.isNull());
+	float radiusSqrd = radius*radius;
+	std::map<int, float> savedRadius;
+	savedRadius.insert(std::make_pair(signatureId, 0));
+	while((maxGraphDepth == 0 || m < maxGraphDepth) && nextMargin.size())
+	{
+		curentMarginList = std::list<int>(nextMargin.begin(), nextMargin.end());
+		nextMargin.clear();
+
+		for(std::list<int>::iterator jter = curentMarginList.begin(); jter!=curentMarginList.end(); ++jter)
+		{
+			if(ids.find(*jter) == ids.end())
+			{
+				//UDEBUG("Added %d with margin %d", *jter, m);
+				// Look up in STM/WM if all ids are here, if not... load them from the database
+				const Signature * s = this->getSignature(*jter);
+				std::map<int, Link> tmpLinks;
+				const std::map<int, Link> * links = &tmpLinks;
+				if(s)
+				{
+					ids.insert(std::pair<int, float>(*jter, savedRadius.at(*jter)));
+
+					links = &s->getLinks();
+				}
+
+				// links
+				for(std::map<int, Link>::const_iterator iter=links->begin(); iter!=links->end(); ++iter)
+				{
+					if(!uContains(ids, iter->first) &&
+						uContains(optimizedPoses, iter->first))
+					{
+						const Transform & t = optimizedPoses.at(iter->first);
+						UASSERT(!t.isNull());
+						float distanceSqrd = referential.getDistanceSquared(t);
+						if(radiusSqrd == 0 || distanceSqrd<radiusSqrd)
+						{
+							savedRadius.insert(std::make_pair(iter->first, distanceSqrd));
+							nextMargin.insert(iter->first);
+						}
+
+					}
+				}
+			}
+		}
+		++m;
+	}
+	return ids;
+}
+
 int Memory::getNextId()
 {
 	return ++_idCount;
@@ -1256,7 +1326,7 @@ std::list<int> Memory::forget(const std::set<int> & ignoredIds)
 {
 	UDEBUG("");
 	std::list<int> signaturesRemoved;
-	if(_vwd->isIncremental())
+	if(_vwd->isIncremental() && _vwd->getVisualWords().size())
 	{
 		int newWords = 0;
 		int wordsRemoved = 0;
@@ -1890,6 +1960,13 @@ Transform Memory::computeVisualTransform(
 					if(variance <= _bowEpipolarGeometryVar)
 					{
 						transform = cameraTransform.inverse();
+						if(_bowForce2D)
+						{
+							UDEBUG("Forcing 2D...");
+							float x,y,z,r,p,yaw;
+							transform.getTranslationAndEulerAngles(x,y,z, r,p,yaw);
+							transform = Transform::fromEigen3f(pcl::getTransformation(x,y,0, 0, 0, yaw));
+						}
 					}
 					else
 					{
@@ -1990,6 +2067,22 @@ Transform Memory::computeVisualTransform(
 		}
 	}

+	if(!transform.isNull())
+	{
+		// verify if it is a 180 degree transform, well verify > 90
+		float roll,pitch,yaw;
+		transform.getEulerAngles(roll, pitch, yaw);
+		if(fabs(roll) > CV_PI/2 ||
+		   fabs(pitch) > CV_PI/2 ||
+		   fabs(yaw) > CV_PI/2)
+		{
+			transform.setNull();
+			msg = uFormat("Too large rotation detected! (roll=%f, pitch=%f, yaw=%f)",
+					roll, pitch, yaw);
+			UWARN(msg.c_str());
+		}
+	}
+
 	if(rejectedMsg)
 	{
 		*rejectedMsg = msg;
@@ -2295,7 +2388,8 @@ Transform Memory::computeIcpTransform(
 				}
 				else
 				{
-					correspondencesRatio = float(correspondences)/float(oldCloud->size()>newCloud->size()?oldCloud->size():newCloud->size());
+					UWARN("Maximum laser scans points not set for signature %d, correspondences ratio set to 0!",
+							newS.id());
 				}

 				UDEBUG("%d->%d hasConverged=%s, variance=%f, correspondences=%d/%d (%f%%)",
@@ -2466,9 +2560,18 @@ Transform Memory::computeScanMatchingTransform(
 		UDEBUG("icpT=%s", icpT.prettyPrint().c_str());

 		// verify if there enough correspondences
-		float correspondencesRatio = float(correspondences)/float(newCloud->size());
+		float correspondencesRatio = 0.0f;
+		if(newS->getLaserScanMaxPts())
+		{
+			correspondencesRatio = float(correspondences)/float(newS->getLaserScanMaxPts());
+		}
+		else
+		{
+			UWARN("Maximum laser scans points not set for signature %d, correspondences ratio set to 0!",
+					newS->id());
+		}

-		UDEBUG("variance=%f, correspondences=%d/%d (%f%%)",
+		UDEBUG("variance=%f, correspondences=%d/%d (%f%%) %f",
 				variance?*variance:-1,
 				correspondences,
 				(int)newCloud->size(),
@@ -2479,16 +2582,20 @@ Transform Memory::computeScanMatchingTransform(
 			*inliers = correspondences;
 		}

+		//pcl::io::savePCDFile("old.pcd", *assembledOldClouds, true);
+		//pcl::io::savePCDFile("new.pcd", *newCloud, true);
+		//UWARN("local scan matching old.pcd, new.pcd saved!");
+		//if(!icpT.isNull())
+		//{
+		//	newCloud = util3d::transformPointCloud<pcl::PointXYZ>(newCloud, icpT);
+		//	pcl::io::savePCDFile("newFinal.pcd", *newCloud, true);
+		//	UWARN("local scan matching newFinal.pcd saved!");
+		//}
+
 		if(!icpT.isNull() && hasConverged &&
 		   correspondencesRatio >= _icp2CorrespondenceRatio)
 		{
 			transform = poses.at(newId).inverse()*icpT.inverse() * poses.at(oldId);
-
-			//pcl::io::savePCDFile("old.pcd", *assembledOldClouds, true);
-			//pcl::io::savePCDFile("new.pcd", *newCloud, true);
-			//newCloud = util3d::transformPointCloud<pcl::PointXYZ>(newCloud, icpT);
-			//pcl::io::savePCDFile("newFinal.pcd", *newCloud, true);
-			//UWARN("local scan matching old.pcd, new.pcd and newFinal.pcd saved!");
 		}
 		else
 		{
@@ -2788,15 +2895,23 @@ void Memory::rehearsal(Signature * signature, Statistics * stats)
 						fabs(y) > _rehearsalMaxDistance ||
 						fabs(z) > _rehearsalMaxDistance)) ||
 					(_rehearsalMaxAngle>0.0f && (
-					fabs(roll) > _rehearsalMaxAngle ||
-					fabs(pitch) > _rehearsalMaxAngle ||
-					fabs(yaw) > _rehearsalMaxAngle)))
+						fabs(roll) > _rehearsalMaxAngle ||
+						fabs(pitch) > _rehearsalMaxAngle ||
+						fabs(yaw) > _rehearsalMaxAngle)))
 				{
-					// if the robot has moved, transfer only weight
-					signature->setWeight(signature->getWeight() + 1 + sB->getWeight());
-					sB->setWeight(0);
-					UINFO("Only updated weight to %d of %d (old=%d) because the robot has moved. (d=%f a=%f)",
-							signature->getWeight(), signature->id(), id, _rehearsalMaxDistance, _rehearsalMaxAngle);
+					if(_rehearsalWeightIgnoredWhileMoving)
+					{
+						UINFO("Rehearsal ignored because the robot has moved more than %f m or %f rad",
+								_rehearsalMaxDistance, _rehearsalMaxAngle);
+					}
+					else
+					{
+						// if the robot has moved, increase only weight of the previous
+						// signature because they are not merged
+						sB->setWeight(sB->getWeight()+1);
+						UINFO("Only updated weight to %d of %d (new=%d) because the robot has moved. (d=%f a=%f)",
+								sB->getWeight(), sB->id(), signature->id(), _rehearsalMaxDistance, _rehearsalMaxAngle);
+					}
 				}
 				else if(this->rehearsalMerge(id, signature->id()))
 				{
@@ -3017,7 +3132,7 @@ Signature Memory::getSignatureData(int locationId, bool uncompressedData)
 			s->uncompressData();
 			r.setImageRaw(s->getImageRaw());
 			r.setDepthRaw(s->getDepthRaw());
-			r.setLaserScanRaw(s->getLaserScanRaw());
+			r.setLaserScanRaw(s->getLaserScanRaw(), s->getLaserScanMaxPts());
 		}
 		else
 		{
@@ -3368,7 +3483,7 @@ void Memory::copyData(const Signature * from, Signature * to)
 		{
 			to->setImageCompressed(from->getImageCompressed());
 			to->setDepthCompressed(from->getDepthCompressed(), from->getFx(), from->getFy(), from->getCx(), from->getCy());
-			to->setLaserScanCompressed(from->getLaserScanCompressed());
+			to->setLaserScanCompressed(from->getLaserScanCompressed(), from->getLaserScanMaxPts());
 			to->setLocalTransform(from->getLocalTransform());
 		}

@@ -3933,7 +4048,7 @@ Signature * Memory::createSignature(const SensorData & data, Statistics * stats)
 	{
 		s->setImageRaw(image);
 		s->setDepthRaw(depthOrRightImage);
-		s->setLaserScanRaw(laserScan);
+		s->setLaserScanRaw(laserScan, data.laserScanMaxPts());
 	}


--- a/corelib/src/Rtabmap.cpp
+++ b/corelib/src/Rtabmap.cpp
@@ -96,7 +96,7 @@ Rtabmap::Rtabmap() :
 	_localLoopClosureDetectionTime(Parameters::defaultRGBDLocalLoopDetectionTime()),
 	_localLoopClosureDetectionSpace(Parameters::defaultRGBDLocalLoopDetectionSpace()),
 	_localRadius(Parameters::defaultRGBDLocalRadius()),
-	_localDetectMaxDiffID(Parameters::defaultRGBDLocalLoopDetectionMaxDiffID()),
+	_localDetectMaxGraphDepth(Parameters::defaultRGBDLocalLoopDetectionMaxGraphDepth()),
 	_localPathFilteringRadius(Parameters::defaultRGBDLocalLoopDetectionPathFilteringRadius()),
 	_localPathOdomPosesUsed(Parameters::defaultRGBDLocalLoopDetectionPathOdomPosesUsed()),
 	_databasePath(""),
@@ -385,7 +385,7 @@ void Rtabmap::parseParameters(const ParametersMap & parameters)
 	Parameters::parse(parameters, Parameters::kRGBDLocalLoopDetectionTime(), _localLoopClosureDetectionTime);
 	Parameters::parse(parameters, Parameters::kRGBDLocalLoopDetectionSpace(), _localLoopClosureDetectionSpace);
 	Parameters::parse(parameters, Parameters::kRGBDLocalRadius(), _localRadius);
-	Parameters::parse(parameters, Parameters::kRGBDLocalLoopDetectionMaxDiffID(), _localDetectMaxDiffID);
+	Parameters::parse(parameters, Parameters::kRGBDLocalLoopDetectionMaxGraphDepth(), _localDetectMaxGraphDepth);
 	Parameters::parse(parameters, Parameters::kRGBDLocalLoopDetectionPathFilteringRadius(), _localPathFilteringRadius);
 	Parameters::parse(parameters, Parameters::kRGBDLocalLoopDetectionPathOdomPosesUsed(), _localPathOdomPosesUsed);
 	Parameters::parse(parameters, Parameters::kRGBDOptimizeFromGraphEnd(), _optimizeFromGraphEnd);
@@ -880,8 +880,9 @@ bool Rtabmap::process(const SensorData & data)
 	}
 	else
 	{
-		SensorData dataImageOnly(data.image(), data.id(), data.stamp(), data.userData());
-		if(!_memory->update(dataImageOnly, &statistics_))
+		SensorData dataWithoutOdom = data;
+		dataWithoutOdom.setPose(Transform(), 1, 1);
+		if(!_memory->update(dataWithoutOdom, &statistics_))
 		{
 			return false;
 		}
@@ -952,7 +953,7 @@ bool Rtabmap::process(const SensorData & data)
 			UASSERT(oldS != 0);
 			std::string rejectedMsg;
 			Transform guess = signature->getLinks().begin()->second.transform();
-			double variance = -1.0;
+			double variance = 1.0;
 			int inliers = 0;
 			float inliersRatio = 0;
 			Transform t = _memory->computeIcpTransform(oldId, signature->id(), guess, false, &rejectedMsg, &inliers, &variance, &inliersRatio);
@@ -964,7 +965,7 @@ bool Rtabmap::process(const SensorData & data)
 						oldId,
 						signature->getLinks().at(oldId).transform().prettyPrint().c_str(),
 						t.prettyPrint().c_str());
-				_memory->updateLink(signature->id(), oldId, t, 1, 1); // set Identify covariance
+				_memory->updateLink(signature->id(), oldId, t, variance, variance);
 			}
 			else
 			{
@@ -1304,53 +1305,50 @@ bool Rtabmap::process(const SensorData & data)
 	//============================================================
 	// RETRIEVAL 2/3 : Update planned path and get next nodes to retrieve
 	//============================================================
-	std::set<int> retrievalLocalIds;
-	if(_rgbdSlamMode && _maxLocalRetrieved > 0)
+	std::list<int> retrievalLocalIds;
+	if(_rgbdSlamMode)
 	{
 		// Priority on locations on the planned path
 		if(_path.size())
 		{
 			updateGoalIndex();

-			if(_path.size())
+			float distanceSoFar = 0.0f;
+			// immunize all nodes after current node and
+			// retrieve nodes after current node in the maximum radius from the current node
+			for(unsigned int i=_pathCurrentIndex; i<_path.size(); ++i)
 			{
-				float distanceSoFar = 0.0f;
-				// immunize all nodes after current node and
-				// retrieve nodes after current node in the maximum radius from the current node
-				for(unsigned int i=_pathCurrentIndex; i<_path.size(); ++i)
+				if(_localRadius > 0.0f && i != _pathCurrentIndex)
 				{
-					if(_localRadius > 0.0f && i != _pathCurrentIndex)
-					{
-						distanceSoFar += _path[i-1].second.getDistance(_path[i].second);
-					}
+					distanceSoFar += _path[i-1].second.getDistance(_path[i].second);
+				}

-					if(distanceSoFar <= _localRadius)
+				if(distanceSoFar <= _localRadius)
+				{
+					if(_memory->getSignature(_path[i].first) != 0)
 					{
-						if(_memory->getSignature(_path[i].first) != 0)
-						{
-							immunizedLocations.insert(_path[i].first);
-							UDEBUG("Path immunization: node %d (dist=%fm)", _path[i].first, distanceSoFar);
-						}
-						else if(retrievalLocalIds.size() < _maxLocalRetrieved)
-						{
-							UINFO("retrieval of node %d on path (dist=%fm)", _path[i].first, distanceSoFar);
-							retrievalLocalIds.insert(_path[i].first);
-							// retrieved locations are automatically immunized
-						}
+						immunizedLocations.insert(_path[i].first);
+						UDEBUG("Path immunization: node %d (dist=%fm)", _path[i].first, distanceSoFar);
 					}
-					else
+					else if(retrievalLocalIds.size() < _maxLocalRetrieved)
 					{
-						UDEBUG("Stop on node %d (dist=%fm > %fm)",
-								_path[i].first, distanceSoFar, _localRadius);
-						break;
+						UINFO("retrieval of node %d on path (dist=%fm)", _path[i].first, distanceSoFar);
+						retrievalLocalIds.push_back(_path[i].first);
+						// retrieved locations are automatically immunized
 					}
 				}
+				else
+				{
+					UDEBUG("Stop on node %d (dist=%fm > %fm)",
+							_path[i].first, distanceSoFar, _localRadius);
+					break;
+				}
 			}
 		}
-		else if(retrievalLocalIds.size() < _maxLocalRetrieved)
+		if(retrievalLocalIds.size() < _maxLocalRetrieved)
 		{
 			// retrieval based on the nodes near the current pose
-			std::map<int, float> nearNodes = graph::getNodesInRadius(signature->id(), _optimizedPoses, 0, _localRadius);
+			std::map<int, float> nearNodes = graph::getNodesInRadius(signature->id(), _optimizedPoses, _localRadius);
 			// sort by distance
 			std::multimap<float, int> nearNodesByDist;
 			for(std::map<int, float>::iterator iter=nearNodes.begin(); iter!=nearNodes.end(); ++iter)
@@ -1366,17 +1364,41 @@ bool Rtabmap::process(const SensorData & data)
 				// If there is a change of direction, better to be retrieving
 				// ALL nearest signatures than only newest neighbors
 				const std::map<int, Link> & links = s->getLinks();
-				for(std::map<int, Link>::const_iterator jter=links.begin();
-					jter!=links.end() && retrievalLocalIds.size() < _maxLocalRetrieved;
+				for(std::map<int, Link>::const_reverse_iterator jter=links.rbegin();
+					jter!=links.rend() && retrievalLocalIds.size() < _maxLocalRetrieved;
 					++jter)
 				{
 					if(_memory->getSignature(jter->first) == 0)
 					{
 						UINFO("retrieval of node %d on local map", jter->first);
-						retrievalLocalIds.insert(jter->first);
+						retrievalLocalIds.push_back(jter->first);
 					}
 				}
 			}
+			// well, if the maximum retrieved is not reached, look for neighbors in database
+			if(retrievalLocalIds.size() < _maxLocalRetrieved)
+			{
+				std::set<int> retrievalLocalIdsSet(retrievalLocalIds.begin(), retrievalLocalIds.end());
+				for(std::list<int>::iterator iter=retrievalLocalIds.begin();
+					iter!=retrievalLocalIds.end() && retrievalLocalIds.size() < _maxLocalRetrieved;
+					++iter)
+				{
+					std::map<int, int> ids = _memory->getNeighborsId(*iter, 2, _maxLocalRetrieved - retrievalLocalIds.size() + 1, true, false);
+					for(std::map<int, int>::reverse_iterator jter=ids.rbegin();
+						jter!=ids.rend() && retrievalLocalIds.size() < _maxLocalRetrieved;
+						++jter)
+					{
+						if(_memory->getSignature(jter->first) == 0 &&
+						   retrievalLocalIdsSet.find(jter->first) == retrievalLocalIdsSet.end())
+						{
+							UINFO("retrieval of node %d on local map", jter->first);
+							retrievalLocalIds.push_back(jter->first);
+							retrievalLocalIdsSet.insert(jter->first);
+						}
+					}
+				}
+			}
+
 			// update Age of the close signatures (oldest the farthest)
 			for(std::multimap<float, int>::reverse_iterator iter=nearNodesByDist.rbegin(); iter!=nearNodesByDist.rend(); ++iter)
 			{
@@ -1488,7 +1510,6 @@ bool Rtabmap::process(const SensorData & data)
 			if(!transform.isNull() && _globalLoopClosureIcpType > 0)
 			{
 				transform = _memory->computeIcpTransform(_loopClosureHypothesis.first, signature->id(), transform, _globalLoopClosureIcpType == 1, &rejectedMsg, 0, &variance);
-				variance = 1.0f; // ICP, set variance to 1
 			}
 			rejectedHypothesis = transform.isNull();
 			if(rejectedHypothesis)
@@ -1519,12 +1540,12 @@ bool Rtabmap::process(const SensorData & data)
 	timeAddLoopClosureLink = timer.ticks();
 	ULOGGER_INFO("timeAddLoopClosureLink=%fs", timeAddLoopClosureLink);

-	int localSpaceClosuresAdded = 0;
+	int localSpaceClosuresAddedVisually = 0;
 	int localSpaceClosuresAddedByICPOnly = 0;
 	int lastLocalSpaceClosureId = 0;
 	int localSpacePaths = 0;
 	if(_localLoopClosureDetectionSpace &&
-	   !signature->getLaserScanCompressed().empty())
+	   _localRadius > 0)
 	{
 		if(_graphOptimizer->iterations() == 0)
 		{
@@ -1533,108 +1554,204 @@ bool Rtabmap::process(const SensorData & data)
 		else
 		{
 			//============================================================
-			// Scan matching LOCAL LOOP CLOSURE SPACE
+			// LOCAL LOOP CLOSURE SPACE
 			//============================================================
-			std::map<int, Transform> forwardPoses;
-			forwardPoses = this->getForwardWMPoses(
-					signature->id(),
-					0,
-					_localRadius,
-					_localDetectMaxDiffID);

-			std::list<std::map<int, Transform> > forwardPaths = getPaths(forwardPoses);
-			localSpacePaths = (int)forwardPaths.size();
-
-			for(std::list<std::map<int, Transform> >::iterator iter=forwardPaths.begin(); iter!=forwardPaths.end(); ++iter)
+			//
+			// 1) compare visually with nearest locations
+			//
+			float r = _localRadius;
+			if(_localPathFilteringRadius > 0 && _localPathFilteringRadius<_localRadius)
+			{
+				r = _localPathFilteringRadius;
+			}
+			std::map<int, float> nearestIds = _memory->getNeighborsIdRadius(signature->id(), r, _optimizedPoses, _localDetectMaxGraphDepth);
+			std::map<int, Transform> nearestPoses;
+			for(std::map<int, float>::iterator iter=nearestIds.begin(); iter!=nearestIds.end(); ++iter)
+			{
+				nearestPoses.insert(std::make_pair(iter->first, _optimizedPoses.at(iter->first)));
+			}
+			// segment poses by paths, only one detection per path
+			std::list<std::map<int, Transform> > nearestPaths = getPaths(nearestPoses);
+			for(std::list<std::map<int, Transform> >::iterator iter=nearestPaths.begin(); iter!=nearestPaths.end(); ++iter)
 			{
 				std::map<int, Transform> & path = *iter;
 				UASSERT(path.size());
+				//find the nearest pose on the path
+				int nearestId = rtabmap::graph::findNearestNode(path, _optimizedPoses.at(signature->id()));
+				UASSERT(nearestId > 0);

-				// only do local loop closure detection if there is no
-				// global loop closure already detected on this path
-				if(_loopClosureHypothesis.first == 0 || path.find(_loopClosureHypothesis.first) == path.end())
+				// nearest pose must not be linked to current location, and not in STM
+				if(!signature->hasLink(nearestId) &&
+				   _memory->getStMem().find(nearestId) == _memory->getStMem().end())
 				{
+					double variance = 1.0;
+					Transform transform;
+					if(_reextractLoopClosureFeatures)
+					{
+						ParametersMap customParameters = _modifiedParameters; // get BOW LCC parameters
+						// override some parameters
+						uInsert(customParameters, ParametersPair(Parameters::kMemIncrementalMemory(), "true")); // make sure it is incremental
+						uInsert(customParameters, ParametersPair(Parameters::kMemRehearsalSimilarity(), "1.0")); // desactivate rehearsal
+						uInsert(customParameters, ParametersPair(Parameters::kMemBinDataKept(), "false"));
+						uInsert(customParameters, ParametersPair(Parameters::kMemSTMSize(), "0"));
+						uInsert(customParameters, ParametersPair(Parameters::kKpIncrementalDictionary(), "true")); // make sure it is incremental
+						uInsert(customParameters, ParametersPair(Parameters::kKpNewWordsComparedTogether(), "false"));
+						uInsert(customParameters, ParametersPair(Parameters::kKpNNStrategy(), uNumber2Str(_reextractNNType))); // bruteforce
+						uInsert(customParameters, ParametersPair(Parameters::kKpNndrRatio(), uNumber2Str(_reextractNNDR)));
+						uInsert(customParameters, ParametersPair(Parameters::kKpDetectorStrategy(), uNumber2Str(_reextractFeatureType))); // FAST/BRIEF
+						uInsert(customParameters, ParametersPair(Parameters::kKpWordsPerImage(), uNumber2Str(_reextractMaxWords)));
+						uInsert(customParameters, ParametersPair(Parameters::kKpBadSignRatio(), "0"));
+						uInsert(customParameters, ParametersPair(Parameters::kKpRoiRatios(), "0.0 0.0 0.0 0.0"));
+						uInsert(customParameters, ParametersPair(Parameters::kMemGenerateIds(), "false"));
+
+						//for(ParametersMap::iterator iter = customParameters.begin(); iter!=customParameters.end(); ++iter)
+						//{
+						//	UDEBUG("%s=%s", iter->first.c_str(), iter->second.c_str());
+						//}
+
+						Memory memory(customParameters);
+
+						UTimer timeT;
+
+						// Add signatures
+						SensorData dataFrom = data;
+						dataFrom.setId(signature->id());
+						Signature tmpTo = _memory->getSignatureData(nearestId, true);
+						SensorData dataTo = tmpTo.toSensorData();
+						UDEBUG("timeTo = %fs", timeT.ticks());
+
+						if(dataFrom.isValid() &&
+						   dataFrom.isMetric() &&
+						   dataTo.isValid() &&
+						   dataTo.isMetric() &&
+						   dataFrom.id() != Memory::kIdInvalid &&
+						   tmpTo.id() != Memory::kIdInvalid)
+						{
+							memory.update(dataTo);
+							UDEBUG("timeUpTo = %fs", timeT.ticks());
+							memory.update(dataFrom);
+							UDEBUG("timeUpFrom = %fs", timeT.ticks());
+
+							transform = memory.computeVisualTransform(dataTo.id(), dataFrom.id(), 0, 0, &variance);
+							UDEBUG("timeTransform = %fs", timeT.ticks());
+						}
+						else
+						{
+							// Fallback to normal way (raw data not kept in database...)
+							UWARN("Loop closure: Some images not found in memory for re-extracting "
+								  "features, is Mem/RawDataKept=false? Falling back with already extracted 3D features.");
+							transform = _memory->computeVisualTransform(nearestId, signature->id(), 0, 0, &variance);
+						}
+					}
+					else
+					{
+						transform = _memory->computeVisualTransform(nearestId, signature->id(), 0, 0, &variance);
+					}
+					if(!transform.isNull() && _globalLoopClosureIcpType > 0)
+					{
+						transform  = _memory->computeIcpTransform(nearestId, signature->id(), transform, _globalLoopClosureIcpType == 1, 0, 0, &variance);
+					}
+					if(!transform.isNull())
+					{
+						UINFO("[Visual] Add local loop closure in SPACE (%d->%d) %s",
+								signature->id(),
+								nearestId,
+								transform.prettyPrint().c_str());
+						_memory->addLink(nearestId, signature->id(), transform, Link::kLocalSpaceClosure, variance, variance);
+
+						// Old map -> new map, used for localization correction on loop closure
+						const Signature * oldS = _memory->getSignature(nearestId);
+						UASSERT(oldS != 0);
+						_mapTransform = oldS->getPose() * transform.inverse() * signature->getPose().inverse();
+						++localSpaceClosuresAddedVisually;
+						lastLocalSpaceClosureId = nearestId;
+					}
+				}
+			}
+
+			//
+			// 2) compare locally with nearest locations by scan matching
+			//
+			if( !signature->getLaserScanCompressed().empty())
+			{
+				std::map<int, Transform> forwardPoses;
+				forwardPoses = this->getForwardWMPoses(
+						signature->id(),
+						0,
+						_localRadius,
+						_localDetectMaxGraphDepth);
+
+				std::list<std::map<int, Transform> > forwardPaths = getPaths(forwardPoses);
+				localSpacePaths = (int)forwardPaths.size();
+
+				for(std::list<std::map<int, Transform> >::iterator iter=forwardPaths.begin(); iter!=forwardPaths.end(); ++iter)
+				{
+					std::map<int, Transform> & path = *iter;
+					UASSERT(path.size());
+
 					//find the nearest pose on the path
 					int nearestId = rtabmap::graph::findNearestNode(path, _optimizedPoses.at(signature->id()));
 					UASSERT(nearestId > 0);

-					// nearest pose must be close
-					if(_localPathFilteringRadius <= 0.0f ||
-					   _optimizedPoses.at(signature->id()).getDistance(_optimizedPoses.at(nearestId)) < _localPathFilteringRadius)
+					// nearest pose must be close and not linked to current location
+					if(!signature->hasLink(nearestId) &&
+					   (_localPathFilteringRadius <= 0.0f ||
+						_optimizedPoses.at(signature->id()).getDistanceSquared(_optimizedPoses.at(nearestId)) < _localPathFilteringRadius*_localPathFilteringRadius))
 					{
-						// 1) look for loop closures based on visual correspondences
-						double variance = 1.0;
-						Transform transform = _memory->computeVisualTransform(nearestId, signature->id(), 0, 0, &variance);
-						bool foundByVisual = false;
-						if(!transform.isNull() && _globalLoopClosureIcpType > 0)
+						// Assemble scans in the path and do ICP only
+						if(_localPathOdomPosesUsed)
 						{
-							transform  = _memory->computeIcpTransform(nearestId, signature->id(), transform, _globalLoopClosureIcpType == 1, 0, 0, &variance);
-							variance = 1.0f; // ICP, set variance to 1
-						}
-						if(transform.isNull())
-						{
-							// 2) Assemble scans in the path and do ICP only
-							if(_localPathOdomPosesUsed)
+							//optimize the path's poses locally
+							path = optimizeGraph(nearestId, uKeys(path), false);
+							// transform local poses in optimized graph referential
+							Transform t = _optimizedPoses.at(nearestId) * path.at(nearestId).inverse();
+							for(std::map<int, Transform>::iterator jter=path.begin(); jter!=path.end(); ++jter)
 							{
-								//optimize the path's poses locally
-								path = optimizeGraph(nearestId, uKeys(path), false);
-								// transform local poses in optimized graph referential
-								Transform t = _optimizedPoses.at(nearestId) * path.at(nearestId).inverse();
-								for(std::map<int, Transform>::iterator jter=path.begin(); jter!=path.end(); ++jter)
+								jter->second = t * jter->second;
+							}
+						}
+						if(_localPathFilteringRadius > 0.0f)
+						{
+							// path filtering
+							std::map<int, Transform> filteredPath = graph::radiusPosesFiltering(path, _localPathFilteringRadius, CV_PI, true);
+							// make sure the nearest and farthest poses are still here
+							filteredPath.insert(*path.find(nearestId));
+							filteredPath.insert(*path.begin());
+							filteredPath.insert(*path.rbegin());
+							path = filteredPath;
+						}
+
+						if(path.size() > 2) // more than current+nearest
+						{
+							// add current node to poses
+							path.insert(std::make_pair(signature->id(), _optimizedPoses.at(signature->id())));
+							//The nearest will be the reference for a loop closure transform
+							if(signature->getLinks().find(nearestId) == signature->getLinks().end())
+							{
+								Transform transform = _memory->computeScanMatchingTransform(signature->id(), nearestId, path, 0, 0, 0);
+								if(!transform.isNull())
 								{
-									jter->second = t * jter->second;
+									UINFO("[Scan matching] Add local loop closure in SPACE (%d->%d) %s",
+											signature->id(),
+											nearestId,
+											transform.prettyPrint().c_str());
+									// set Identify covariance for laser scan matching only
+									_memory->addLink(nearestId, signature->id(), transform, Link::kLocalSpaceClosure, 1, 1);
+
+									++localSpaceClosuresAddedByICPOnly;
+
+									// no local loop closure added visually
+									if(localSpaceClosuresAddedVisually == 0)
+									{
+										// Old map -> new map, used for localization correction on loop closure
+										const Signature * oldS = _memory->getSignature(nearestId);
+										UASSERT(oldS != 0);
+										_mapTransform = oldS->getPose() * transform.inverse() * signature->getPose().inverse();
+										lastLocalSpaceClosureId = nearestId;
+									}
 								}
 							}
-							if(_localPathFilteringRadius > 0.0f)
-							{
-								// path filtering
-								std::map<int, Transform> filteredPath = graph::radiusPosesFiltering(path, _localPathFilteringRadius, CV_PI, true);
-								// make sure the nearest and farthest poses are still here
-								filteredPath.insert(*path.find(nearestId));
-								filteredPath.insert(*path.begin());
-								filteredPath.insert(*path.rbegin());
-								path = filteredPath;
-							}
-
-							if(path.size() > 2) // more than current+nearest
-							{
-								// add current node to poses
-								path.insert(std::make_pair(signature->id(), _optimizedPoses.at(signature->id())));
-								//The nearest will be the reference for a loop closure transform
-								if(signature->getLinks().find(nearestId) == signature->getLinks().end())
-								{
-									transform = _memory->computeScanMatchingTransform(signature->id(), nearestId, path, 0, 0, &variance);
-								}
-							}
-						}
-						else
-						{
-							foundByVisual = true;
-						}
-
-						if(!transform.isNull())
-						{
-							UINFO("Add local loop closure in SPACE (%d->%d) %s",
-									signature->id(),
-									nearestId,
-									transform.prettyPrint().c_str());
-							// set Identify covariance if laser scan matching only
-							_memory->addLink(nearestId, signature->id(), transform, Link::kLocalSpaceClosure, foundByVisual?variance:1, foundByVisual?variance:1);
-
-							// Old map -> new map, used for localization correction on loop closure
-							const Signature * oldS = _memory->getSignature(nearestId);
-							UASSERT(oldS != 0);
-							_mapTransform = oldS->getPose() * transform.inverse() * signature->getPose().inverse();
-							++localSpaceClosuresAdded;
-							if(!foundByVisual)
-							{
-								++localSpaceClosuresAddedByICPOnly;
-							}
-							lastLocalSpaceClosureId = nearestId;
-						}
-						else
-						{
-							UINFO("Local loop closure %d (space) rejected", nearestId);
 						}
 					}
 				}
@@ -1762,7 +1879,7 @@ bool Rtabmap::process(const SensorData & data)
 			statistics_.addStatistic(Statistics::kLoopLast_id(), _memory->getLastGlobalLoopClosureId());

 			statistics_.addStatistic(Statistics::kLocalLoopTime_closures(), localLoopClosuresInTimeFound);
-			statistics_.addStatistic(Statistics::kLocalLoopSpace_closures_added(), localSpaceClosuresAdded);
+			statistics_.addStatistic(Statistics::kLocalLoopSpace_closures_added_visually(), localSpaceClosuresAddedVisually);
 			statistics_.addStatistic(Statistics::kLocalLoopSpace_closures_added_icp_only(), localSpaceClosuresAddedByICPOnly);
 			statistics_.addStatistic(Statistics::kLocalLoopSpace_paths(), localSpacePaths);
 			statistics_.addStatistic(Statistics::kLocalLoopSpace_last_closure_id(), lastLocalSpaceClosureId);
@@ -1861,6 +1978,7 @@ bool Rtabmap::process(const SensorData & data)
 			if(_path.size())
 			{
 				statistics_.setLocalPath(this->getPathNextNodes());
+				statistics_.setCurrentGoalId(this->getPathCurrentGoalId());
 			}
 		}

@@ -1977,6 +2095,7 @@ bool Rtabmap::process(const SensorData & data)
 		// place after transfer because the memory/local graph may have changed
 		statistics_.addStatistic(Statistics::kMemoryWorking_memory_size(), _memory->getWorkingMem().size());
 		statistics_.addStatistic(Statistics::kMemoryShort_time_memory_size(), _memory->getStMem().size());
+		statistics_.addStatistic(Statistics::kMemoryLocal_graph_size(), _optimizedPoses.size());

 		if(_rgbdSlamMode)
 		{
@@ -2166,25 +2285,27 @@ std::map<int, Transform> Rtabmap::getForwardWMPoses(
 		UDEBUG("");
 		const Signature * fromS = _memory->getSignature(fromId);
 		UASSERT(fromS != 0);
+		UASSERT(_optimizedPoses.find(fromId) != _optimizedPoses.end());

 		pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>);
 		cloud->resize(_optimizedPoses.size());
 		std::vector<int> ids(_optimizedPoses.size());
 		int oi = 0;
 		const std::set<int> & stm = _memory->getStMem();
-		//get margins
-		std::map<int, int> margins;
+		//get distances
+		std::map<int, float> foundIds;
 		if(maxDiffID > 0)
 		{
-			margins = _memory->getNeighborsId(fromId, maxDiffID, 0, true, false);
+			foundIds = _memory->getNeighborsIdRadius(fromId, radius, _optimizedPoses, maxDiffID);
 		}
+		float radiusSqrd = radius * radius;
 		for(std::map<int, Transform>::const_iterator iter = _optimizedPoses.begin(); iter!=_optimizedPoses.end(); ++iter)
 		{
 			if(iter->first != fromId)
 			{
-				// Only locations in Working Memory not too far from the current node (so inside the margin)
-				bool diffIdOk = maxDiffID == 0 || uContains(margins, iter->first);
-				if(stm.find(iter->first) == stm.end() && diffIdOk)
+				if(stm.find(iter->first) == stm.end() &&
+				   uContains(foundIds, iter->first) &&
+				   (radiusSqrd==0 || foundIds.at(iter->first) <= radiusSqrd))
 				{
 					(*cloud)[oi] = pcl::PointXYZ(iter->second.x(), iter->second.y(), iter->second.z());
 					ids[oi++] = iter->first;
@@ -2195,7 +2316,6 @@ std::map<int, Transform> Rtabmap::getForwardWMPoses(
 		cloud->resize(oi);
 		ids.resize(oi);

-		UASSERT(_optimizedPoses.find(fromId) != _optimizedPoses.end());
 		Transform fromT = _optimizedPoses.at(fromId);

 		if(cloud->size())
@@ -2904,24 +3024,17 @@ void Rtabmap::updateGoalIndex()
 		{
 			//Always check if the farthest node is accessible in local map (max to local space radius if set)
 			int goalIndex = _pathCurrentIndex;
-			float distanceSoFar = 0.0f;
+			float distanceFromCurrentNode = 0.0f;
 			for(unsigned int i=_pathCurrentIndex; i<_path.size(); ++i)
 			{
 				if(uContains(_optimizedPoses, _path[i].first))
 				{
 					if(_localRadius > 0.0f)
 					{
-						if(i == _pathCurrentIndex)
-						{
-							distanceSoFar += _optimizedPoses.at(_memory->getLastWorkingSignature()->id()).getDistance(_optimizedPoses.at(_path[i].first));
-						}
-						else
-						{
-							distanceSoFar += _optimizedPoses.at(_path[i-1].first).getDistance(_optimizedPoses.at(_path[i].first));
-						}
+						distanceFromCurrentNode = _optimizedPoses.at(_memory->getLastWorkingSignature()->id()).getDistance(_optimizedPoses.at(_path[i].first));
 					}

-					if(distanceSoFar <= _localRadius)
+					if(distanceFromCurrentNode <= _localRadius)
 					{
 						goalIndex = i;
 					}
--- a/corelib/src/Statistics.cpp
+++ b/corelib/src/Statistics.cpp
@@ -42,7 +42,8 @@ Statistics::Statistics() :
 	_extended(0),
 	_refImageId(0),
 	_loopClosureId(0),
-	_localLoopClosureId(0)
+	_localLoopClosureId(0),
+	_currentGoalId(0)
 {
 	_defaultDataInitialized = true;
 }