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/include/rtabmap/core/Odometry.h

@@ -89,6 +89,7 @@ private:
 	float _linearUpdate;
 	float _angularUpdate;
 	int _resetCountdown;
+	bool _force2D;
 	Transform _pose;
 	int _resetCurrentCount;
 

corelib/include/rtabmap/core/Parameters.h

@@ -246,7 +246,7 @@ class RTABMAP_EXP Parameters
 
 	// RGB-D SLAM
 	RTABMAP_PARAM(RGBD, Enabled,           bool, true, 	"");
-	RTABMAP_PARAM(RGBD, ScanMatchingSize,  int, 0, 		"Laser scan matching history for odometry correction (laser scans are required). Set to 0 to disable odometry correction.");
+	RTABMAP_PARAM(RGBD, PoseScanMatching,  bool, false, "Laser scan matching for odometry pose correction (laser scans are required).");
 	RTABMAP_PARAM(RGBD, LinearUpdate,      float, 0.0, 	"Min linear displacement to update the map. Rehearsal is done prior to this, so weights are still updated.");
 	RTABMAP_PARAM(RGBD, AngularUpdate,     float, 0.0, 	"Min angular displacement to update the map. Rehearsal is done prior to this, so weights are still updated.");
 	RTABMAP_PARAM(RGBD, NewMapOdomChangeDistance, float, 0, "A new map is created if a change of odometry translation greater than X m is detected (0 m = disabled).");
@@ -274,6 +274,7 @@ class RTABMAP_EXP Parameters
 	RTABMAP_PARAM(Odom, ResetCountdown,         int, 0,         "Automatically reset odometry after X consecutive images on which odometry cannot be computed (value=0 disables auto-reset).");
 	RTABMAP_PARAM_STR(Odom, RoiRatios,          "0.0 0.0 0.0 0.0", "Region of interest ratios [left, right, top, bottom].");
 	RTABMAP_PARAM(Odom, FeaturesRatio,          float, 0.0, 	"Minimum ratio of keypoints between the current image and the last image to compute odometry.");
+	RTABMAP_PARAM(Odom, Force2D, 		        bool, false,     "Force 2D transform (3Dof: x,y and yaw).");
 
 	// Odometry Bag-of-words
 	RTABMAP_PARAM(OdomBow, LocalHistorySize,       int, 1000,      "Local history size: If > 0 (example 5000), the odometry will maintain a local map of X maximum words.");
@@ -298,7 +299,7 @@ class RTABMAP_EXP Parameters
 	RTABMAP_PARAM(LccBow, InlierDistance,  float, 0.02, 	"Maximum distance for visual word correspondences.");
 	RTABMAP_PARAM(LccBow, Iterations,      int, 100, 		"Maximum iterations to compute the transform from visual words.");
 	RTABMAP_PARAM(LccBow, MaxDepth,        float, 4.0, 		"Max depth of the words (0 means no limit).");
-	RTABMAP_PARAM(LccBow, Force2D, 		   bool, false,     "Force 2D transform (3Dof: x,y and yaw).")
+	RTABMAP_PARAM(LccBow, Force2D, 		   bool, false,     "Force 2D transform (3Dof: x,y and yaw).");
 	RTABMAP_PARAM(LccReextract, Activated, bool, false, 	"Activate re-extracting features on global loop closure.");
 	RTABMAP_PARAM(LccReextract, NNType, 	int, 3, 		"kNNFlannNaive=0, kNNFlannKdTree=1, kNNFlannLSH=2, kNNBruteForce=3, kNNBruteForceGPU=4.");
 	RTABMAP_PARAM(LccReextract, NNDR, 		float, 0.7, 	"NNDR: nearest neighbor distance ratio.");

corelib/include/rtabmap/core/Rtabmap.h

@@ -150,7 +150,7 @@ private:
 	float _newMapOdomChangeDistance;
 	int _globalLoopClosureIcpType;
 	float _globalLoopClosureIcpMaxDistance;
-	int _scanMatchingSize;
+	bool _poseScanMatching;
 	bool _localLoopClosureDetectionTime;
 	bool _localLoopClosureDetectionSpace;
 	float _localDetectRadius;

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);
+			UINFO("Odometry correction by scan matching");
-			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;
-					}
-				}
-			}
-
 			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);
+				scanMatchingSuccess = true;
-				UASSERT(oldS != 0);
+				UINFO("Scan matching: update neighbor link (%d->%d) from %s to %s",
-				std::string rejectedMsg;
+						signature->id(),
-				Transform t = _memory->computeScanMatchingTransform(signature->id(), oldId, poses, &rejectedMsg);
+						oldId,
-				if(!t.isNull())
+						signature->getNeighbors().at(oldId).prettyPrint().c_str(),
-				{
+						t.prettyPrint().c_str());
-					scanMatchingSuccess = true;
+				_memory->updateNeighborLink(signature->id(), oldId, t);
-					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());
-				}
 			}
 			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())
 	{
-		//============================================================
+		if(_toroIterations == 0)
-		// 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())
 		{
-			std::string rejectedMsg;
+			UWARN("Cannot do local loop closure detection in space if graph optimization is disabled!");
-			Transform t = _memory->computeScanMatchingTransform(signature->id(), localSpaceNearestId, poses, &rejectedMsg);
+		}
-			if(!t.isNull())
+		else
-			{
+		{
-				localSpaceClosureId = localSpaceNearestId;
+			//============================================================
-				UINFO("Add local loop closure in SPACE (%d->%d) %s",
+			// Scan matching LOCAL LOOP CLOSURE SPACE
-						signature->id(),
+			//============================================================
-						localSpaceNearestId,
+			// get all nodes in radius of the current node
-						t.prettyPrint().c_str());
+			std::map<int, Transform> poses;
-				_memory->addLoopClosureLink(localSpaceNearestId, signature->id(), t, false);
+			localSpaceNearestId = 0;
+			poses = this->getOptimizedWMPosesInRadius(signature->id(), _localDetectMaxNeighbors, _localDetectRadius, _localDetectMaxDiffID, localSpaceNearestId);
 
-				// Old map -> new map, used for localization correction on loop closure
+			// add current node to poses
-				const Signature * oldS = _memory->getSignature(localSpaceNearestId);
+			UASSERT(_optimizedPoses.find(signature->id()) != _optimizedPoses.end());
-				UASSERT(oldS != 0);
+			poses.insert(std::make_pair(signature->id(), _optimizedPoses.at(signature->id())));
-				_mapTransform = oldS->getPose() * t.inverse() * signature->getPose().inverse();
+
-			}
+			localSpaceDetectionPosesCount = (int)poses.size()-1;
-			else
+			//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());
+				}
 			}
 		}
 	}

guilib/src/PreferencesDialog.cpp

@@ -416,7 +416,7 @@ PreferencesDialog::PreferencesDialog(QWidget * parent) :
 	_ui->rgdb_linearUpdate->setObjectName(Parameters::kRGBDLinearUpdate().c_str());
 	_ui->rgdb_angularUpdate->setObjectName(Parameters::kRGBDAngularUpdate().c_str());
 	_ui->rgdb_newMapOdomChange->setObjectName(Parameters::kRGBDNewMapOdomChangeDistance().c_str());
-	_ui->odomScanHistory->setObjectName(Parameters::kRGBDScanMatchingSize().c_str());
+	_ui->odomScanHistory->setObjectName(Parameters::kRGBDPoseScanMatching().c_str());
 	_ui->globalDetection_toroIterations->setObjectName(Parameters::kRGBDToroIterations().c_str());
 	_ui->globalDetection_optimizeFromGraphEnd->setObjectName(Parameters::kRGBDOptimizeFromGraphEnd().c_str());
 
@@ -470,6 +470,7 @@ PreferencesDialog::PreferencesDialog(QWidget * parent) :
 	_ui->odom_maxDepth->setObjectName(Parameters::kOdomMaxDepth().c_str());
 	_ui->odom_minInliers->setObjectName(Parameters::kOdomMinInliers().c_str());
 	_ui->odom_refine_iterations->setObjectName(Parameters::kOdomRefineIterations().c_str());
+	_ui->odom_force2D->setObjectName(Parameters::kOdomForce2D().c_str());
 	_ui->lineEdit_odom_roi->setObjectName(Parameters::kOdomRoiRatios().c_str());
 
 	//Odometry BOW

guilib/src/ui/preferencesDialog.ui

@@ -65,7 +65,7 @@
             <x>0</x>
             <y>0</y>
             <width>744</width>
-            <height>932</height>
+            <height>1161</height>
            </rect>
           </property>
           <layout class="QVBoxLayout" name="verticalLayout_16">
@@ -86,7 +86,7 @@
                 <enum>QFrame::Raised</enum>
                </property>
                <property name="currentIndex">
-                <number>23</number>
+                <number>19</number>
                </property>
                <widget class="QWidget" name="page_22">
                 <layout class="QVBoxLayout" name="verticalLayout_29">
@@ -4923,7 +4923,7 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag
                        <item row="3" column="1">
                         <widget class="QLabel" name="label_scanMatching">
                          <property name="text">
-                          <string>Laser scan matching history size for odometry correction. Set to 0 to disable odometry correction. ICP 2D only is used here.</string>
+                          <string>Laser scan matching for odometry pose correction. ICP 2D only is used here.</string>
                          </property>
                          <property name="wordWrap">
                           <bool>true</bool>
@@ -4931,7 +4931,11 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag
                         </widget>
                        </item>
                        <item row="3" column="0">
-                        <widget class="QSpinBox" name="odomScanHistory"/>
+                        <widget class="QCheckBox" name="odomScanHistory">
+                         <property name="text">
+                          <string/>
+                         </property>
+                        </widget>
                        </item>
                       </layout>
                      </widget>
@@ -5067,7 +5071,7 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag
                        <item row="0" column="0">
                         <widget class="QSpinBox" name="globalDetection_toroIterations">
                          <property name="minimum">
-                          <number>1</number>
+                          <number>0</number>
                          </property>
                          <property name="maximum">
                           <number>10000</number>
@@ -6323,6 +6327,23 @@ Lower the ratio -&gt; higher the precision. 0 means disabled, matching the neare
                         </property>
                        </widget>
                       </item>
+                      <item row="5" column="0">
+                       <widget class="QCheckBox" name="odom_force2D">
+                        <property name="text">
+                         <string/>
+                        </property>
+                       </widget>
+                      </item>
+                      <item row="5" column="1">
+                       <widget class="QLabel" name="label_196">
+                        <property name="text">
+                         <string>Force 2D transform (3DoF: x,y and yaw).</string>
+                        </property>
+                        <property name="wordWrap">
+                         <bool>true</bool>
+                        </property>
+                       </widget>
+                      </item>
                      </layout>
                     </item>
                     <item>

tools/CameraRGBD/main.cpp

@@ -120,6 +120,7 @@ int main(int argc, char * argv[])
 	cv::namedWindow("Video", CV_WINDOW_AUTOSIZE); // create window
 	cv::namedWindow("Depth", CV_WINDOW_AUTOSIZE); // create window
 	pcl::visualization::CloudViewer viewer("cloud");
+	rtabmap::Transform opticalTransform(0,0,1,0, -1,0,0,0, 0,-1,0,0);
 	while(!rgb.empty() && !viewer.wasStopped())
 	{
 		cv::Mat tmp;
@@ -128,7 +129,9 @@ int main(int argc, char * argv[])
 		cv::imshow("Video", rgb); // show frame
 		cv::imshow("Depth",tmp);
 
-		viewer.showCloud(rtabmap::util3d::cloudFromDepthRGB(rgb, depth, cx, cy, fx, fy), "cloud");
+		pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud = rtabmap::util3d::cloudFromDepthRGB(rgb, depth, cx, cy, fx, fy);
+		cloud = rtabmap::util3d::transformPointCloud<pcl::PointXYZRGB>(cloud, opticalTransform);
+		viewer.showCloud(cloud, "cloud");
 
 		int c = cv::waitKey(10); // wait 10 ms or for key stroke
 		if(c == 27)