Odometry: added force2D option

Changed parameter RGBD/ScanMatchingSize to RGBD/PoseScanMatching

git-svn-id: http://rtabmap.googlecode.com/svn/trunk/rtabmap@2050 f169173b-cf89-36c8-b27e-44dbe73f0c83

corelib/src/Memory.cpp

@@ -2036,6 +2036,7 @@ Transform Memory::computeIcpTransform(const Signature & oldS, const Signature &
 					   hasConverged,
 					   fitness);
 
+				//UWARN("saving ICP2D clouds!");
 				//pcl::io::savePCDFile("lccold.pcd", *oldCloud);
 				//pcl::io::savePCDFile("lccnewguess.pcd", *newCloud);
 				newCloud = util3d::transformPointCloud<pcl::PointXYZ>(newCloud, icpT);
@@ -2147,6 +2148,7 @@ Transform Memory::computeScanMatchingTransform(
 		newCloud = util3d::voxelize<pcl::PointXYZ>(newCloud, _icp2VoxelSize);
 	}
 
+	//UWARN("local scan matching pcd saved!");
 	//pcl::io::savePCDFile("old.pcd", *assembledOldClouds);
 	//pcl::io::savePCDFile("new.pcd", *newCloud);
 
@@ -2183,7 +2185,7 @@ Transform Memory::computeScanMatchingTransform(
 		{
 			transform = poses.at(newId).inverse()*icpT.inverse() * poses.at(oldId);
 
-			//newCloud = util3d::cvMat2Cloud(util3d::uncompressData(newS->getDepth2D()), poses.at(oldId)*transform.inverse());
+			//newCloud = util3d::cvMat2Cloud(util3d::uncompressData(newS->getDepth2DCompressed()), poses.at(oldId)*transform.inverse());
 			//pcl::io::savePCDFile("newFinal.pcd", *newCloud);
 		}
 		else

corelib/src/Odometry.cpp

@@ -68,6 +68,7 @@ Odometry::Odometry(const rtabmap::ParametersMap & parameters) :
 		_linearUpdate(Parameters::defaultOdomLinearUpdate()),
 		_angularUpdate(Parameters::defaultOdomAngularUpdate()),
 		_resetCountdown(Parameters::defaultOdomResetCountdown()),
+		_force2D(Parameters::defaultOdomForce2D()),
 		_pose(Transform::getIdentity()),
 		_resetCurrentCount(0)
 {
@@ -82,12 +83,27 @@ Odometry::Odometry(const rtabmap::ParametersMap & parameters) :
 	Parameters::parse(parameters, Parameters::kOdomMaxDepth(), _maxDepth);
 	Parameters::parse(parameters, Parameters::kOdomMaxFeatures(), _maxFeatures);
 	Parameters::parse(parameters, Parameters::kOdomRoiRatios(), _roiRatios);
+	Parameters::parse(parameters, Parameters::kOdomForce2D(), _force2D);
 }
 
 void Odometry::reset(const Transform & initialPose)
 {
 	_resetCurrentCount = 0;
-	_pose = initialPose;
+	if(_force2D)
+	{
+		float x,y,z, roll,pitch,yaw;
+		initialPose.getTranslationAndEulerAngles(x, y, z, roll, pitch, yaw);
+		if(z != 0.0f || roll != 0.0f || yaw != 0.0f)
+		{
+			UWARN("Force2D=true and the initial pose contains z, roll or pitch values (%s). They are set to null.", initialPose.prettyPrint().c_str());
+		}
+		Transform pose(x, y, 0, 0, 0, yaw);
+		_pose = pose;
+	}
+	else
+	{
+		_pose = initialPose;
+	}
 }
 
 bool Odometry::isLargeEnoughTransform(const Transform & transform)
@@ -110,7 +126,17 @@ Transform Odometry::process(SensorData & data, int * quality, int * features, in
 	{
 		_resetCurrentCount = _resetCountdown;
 
-		_pose *= t;
+		if(_force2D)
+		{
+			float x,y,z, roll,pitch,yaw;
+			t.getTranslationAndEulerAngles(x, y, z, roll, pitch, yaw);
+			_pose *= Transform(x,y,0,0,0,yaw);
+		}
+		else
+		{
+			_pose *= t;
+		}
+
 		return _pose;
 	}
 	else if(_resetCurrentCount > 0)

corelib/src/Rtabmap.cpp

@@ -91,7 +91,7 @@ Rtabmap::Rtabmap() :
 	_newMapOdomChangeDistance(Parameters::defaultRGBDNewMapOdomChangeDistance()),
 	_globalLoopClosureIcpType(Parameters::defaultLccIcpType()),
 	_globalLoopClosureIcpMaxDistance(Parameters::defaultLccIcpMaxDistance()),
-	_scanMatchingSize(Parameters::defaultRGBDScanMatchingSize()),
+	_poseScanMatching(Parameters::defaultRGBDPoseScanMatching()),
 	_localLoopClosureDetectionTime(Parameters::defaultRGBDLocalLoopDetectionTime()),
 	_localLoopClosureDetectionSpace(Parameters::defaultRGBDLocalLoopDetectionSpace()),
 	_localDetectRadius(Parameters::defaultRGBDLocalLoopDetectionRadius()),
@@ -350,7 +350,7 @@ void Rtabmap::parseParameters(const ParametersMap & parameters)
 	Parameters::parse(parameters, Parameters::kRGBDLinearUpdate(), _rgbdLinearUpdate);
 	Parameters::parse(parameters, Parameters::kRGBDAngularUpdate(), _rgbdAngularUpdate);
 	Parameters::parse(parameters, Parameters::kRGBDNewMapOdomChangeDistance(), _newMapOdomChangeDistance);
-	Parameters::parse(parameters, Parameters::kRGBDScanMatchingSize(), _scanMatchingSize);
+	Parameters::parse(parameters, Parameters::kRGBDPoseScanMatching(), _poseScanMatching);
 	Parameters::parse(parameters, Parameters::kLccIcpMaxDistance(), _globalLoopClosureIcpMaxDistance);
 	Parameters::parse(parameters, Parameters::kRGBDLocalLoopDetectionTime(), _localLoopClosureDetectionTime);
 	Parameters::parse(parameters, Parameters::kRGBDLocalLoopDetectionSpace(), _localLoopClosureDetectionSpace);
@@ -856,54 +856,31 @@ bool Rtabmap::process(const SensorData & data)
 		//============================================================
 		// Scan matching
 		//============================================================
-		if(_scanMatchingSize>0 &&
+		if(_poseScanMatching &&
 			signature->getNeighbors().size() == 1 &&
 			!signature->getDepth2DCompressed().empty() &&
 			rehearsedId == 0) // don't do it if rehearsal happened
 		{
-			UINFO("Odometry correction by scan matching (size=%d)...", _scanMatchingSize);
-			const std::set<int> & stm = _memory->getStMem();
-			std::map<int, Transform> poses;
-			int count = _scanMatchingSize;
-			for(std::set<int>::const_reverse_iterator iter = stm.rbegin(); iter!=stm.rend() && count>0; ++iter)
-			{
-				if(_memory->getSignature(*iter)->mapId() == signature->mapId())
-				{
-					std::map<int, Transform>::iterator jter = _optimizedPoses.find(*iter);
-					UASSERT_MSG(jter != _optimizedPoses.end(), uFormat("%d not found in optimized poses!", *iter).c_str());
-					poses.insert(*jter);
-					if(*iter != signature->id())
-					{
-						--count;
-					}
-				}
-			}
-
+			UINFO("Odometry correction by scan matching");
 			int oldId = signature->getNeighbors().begin()->first;
-			if(poses.size())
+			const Signature * oldS = _memory->getSignature(oldId);
+			UASSERT(oldS != 0);
+			std::string rejectedMsg;
+			Transform guess = signature->getNeighbors().begin()->second;
+			Transform t = _memory->computeIcpTransform(oldId, signature->id(), guess, false, &rejectedMsg);
+			if(!t.isNull())
 			{
-				const Signature * oldS = _memory->getSignature(oldId);
-				UASSERT(oldS != 0);
-				std::string rejectedMsg;
-				Transform t = _memory->computeScanMatchingTransform(signature->id(), oldId, poses, &rejectedMsg);
-				if(!t.isNull())
-				{
-					scanMatchingSuccess = true;
-					UINFO("Scan matching: update neighbor link (%d->%d) from %s to %s",
-							signature->id(),
-							oldId,
-							signature->getNeighbors().at(oldId).prettyPrint().c_str(),
-							t.prettyPrint().c_str());
-					_memory->updateNeighborLink(signature->id(), oldId, t);
-				}
-				else
-				{
-					UWARN("Scan matching rejected: %s", rejectedMsg.c_str());
-				}
+				scanMatchingSuccess = true;
+				UINFO("Scan matching: update neighbor link (%d->%d) from %s to %s",
+						signature->id(),
+						oldId,
+						signature->getNeighbors().at(oldId).prettyPrint().c_str(),
+						t.prettyPrint().c_str());
+				_memory->updateNeighborLink(signature->id(), oldId, t);
 			}
 			else
 			{
-				UERROR("Poses are empty?!?");
+				UWARN("Scan matching rejected: %s", rejectedMsg.c_str());
 			}
 		}
 		timeScanMatching = timer.ticks();
@@ -1386,43 +1363,50 @@ bool Rtabmap::process(const SensorData & data)
 	   _localLoopClosureDetectionSpace &&
 	   !signature->getDepth2DCompressed().empty())
 	{
-		//============================================================
-		// Scan matching LOCAL LOOP CLOSURE SPACE
-		//============================================================
-		// get all nodes in radius of the current node
-		std::map<int, Transform> poses;
-		localSpaceNearestId = 0;
-		poses = this->getOptimizedWMPosesInRadius(signature->id(), _localDetectMaxNeighbors, _localDetectRadius, _localDetectMaxDiffID, localSpaceNearestId);
-
-		// add current node to poses
-		UASSERT(_optimizedPoses.find(signature->id()) != _optimizedPoses.end());
-		poses.insert(std::make_pair(signature->id(), _optimizedPoses.at(signature->id())));
-
-		localSpaceDetectionPosesCount = (int)poses.size()-1;
-		//The nearest will be the reference for a loop closure transform
-		if(poses.size() &&
-			localSpaceNearestId &&
-			signature->getChildLoopClosureIds().find(localSpaceNearestId) == signature->getChildLoopClosureIds().end())
+		if(_toroIterations == 0)
 		{
-			std::string rejectedMsg;
-			Transform t = _memory->computeScanMatchingTransform(signature->id(), localSpaceNearestId, poses, &rejectedMsg);
-			if(!t.isNull())
-			{
-				localSpaceClosureId = localSpaceNearestId;
-				UINFO("Add local loop closure in SPACE (%d->%d) %s",
-						signature->id(),
-						localSpaceNearestId,
-						t.prettyPrint().c_str());
-				_memory->addLoopClosureLink(localSpaceNearestId, signature->id(), t, false);
+			UWARN("Cannot do local loop closure detection in space if graph optimization is disabled!");
+		}
+		else
+		{
+			//============================================================
+			// Scan matching LOCAL LOOP CLOSURE SPACE
+			//============================================================
+			// get all nodes in radius of the current node
+			std::map<int, Transform> poses;
+			localSpaceNearestId = 0;
+			poses = this->getOptimizedWMPosesInRadius(signature->id(), _localDetectMaxNeighbors, _localDetectRadius, _localDetectMaxDiffID, localSpaceNearestId);
 
-				// Old map -> new map, used for localization correction on loop closure
-				const Signature * oldS = _memory->getSignature(localSpaceNearestId);
-				UASSERT(oldS != 0);
-				_mapTransform = oldS->getPose() * t.inverse() * signature->getPose().inverse();
-			}
-			else
+			// add current node to poses
+			UASSERT(_optimizedPoses.find(signature->id()) != _optimizedPoses.end());
+			poses.insert(std::make_pair(signature->id(), _optimizedPoses.at(signature->id())));
+
+			localSpaceDetectionPosesCount = (int)poses.size()-1;
+			//The nearest will be the reference for a loop closure transform
+			if(poses.size() &&
+				localSpaceNearestId &&
+				signature->getChildLoopClosureIds().find(localSpaceNearestId) == signature->getChildLoopClosureIds().end())
 			{
-				UINFO("Local loop closure (space) rejected: %s", rejectedMsg.c_str());
+				std::string rejectedMsg;
+				Transform t = _memory->computeScanMatchingTransform(signature->id(), localSpaceNearestId, poses, &rejectedMsg);
+				if(!t.isNull())
+				{
+					localSpaceClosureId = localSpaceNearestId;
+					UINFO("Add local loop closure in SPACE (%d->%d) %s",
+							signature->id(),
+							localSpaceNearestId,
+							t.prettyPrint().c_str());
+					_memory->addLoopClosureLink(localSpaceNearestId, signature->id(), t, false);
+
+					// Old map -> new map, used for localization correction on loop closure
+					//const Signature * oldS = _memory->getSignature(localSpaceNearestId);
+					//UASSERT(oldS != 0);
+					_mapTransform = poses.at(localSpaceNearestId) * t.inverse() * poses.at(signature->id()).inverse();
+				}
+				else
+				{
+					UINFO("Local loop closure (space) rejected: %s", rejectedMsg.c_str());
+				}
 			}
 		}
 	}