increased version to 0.8.4: Database Link's "variance" field split into "rot_variance" and "trans_variance". Added Parameter LccIcp/HighTransitionalVariance

CMakeLists.txt

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

corelib/include/rtabmap/core/CameraEvent.h

@@ -57,7 +57,7 @@ public:
 
 	CameraEvent(const cv::Mat & rgb, const cv::Mat & depth, float fx, float fy, float cx, float cy, const Transform & localTransform, int id) :
 		UEvent(kCodeImageDepth),
-		data_(rgb, depth, fx, fy, cx, cy, localTransform, Transform(), 1.0f, id)
+		data_(rgb, depth, fx, fy, cx, cy, localTransform, Transform(), 1.0f, 1.0f, id)
 	{
 	}
 

corelib/include/rtabmap/core/Link.h

@@ -40,15 +40,17 @@ public:
 		from_(0),
 		to_(0),
 		type_(kUndef),
-		variance_(1.0f)
+		rotVariance_(1.0f),
+		transVariance_(1.0f)
 	{
 	}
-	Link(int from, int to, Type type, const Transform & transform, float variance) :
+	Link(int from, int to, Type type, const Transform & transform, float rotVariance, float transVariance) :
 		from_(from),
 		to_(to),
 		transform_(transform),
 		type_(type),
-		variance_(variance)
+		rotVariance_(rotVariance),
+		transVariance_(transVariance)
 	{
 	}
 
@@ -58,20 +60,22 @@ public:
 	int to() const {return to_;}
 	const Transform & transform() const {return transform_;}
 	Type type() const {return type_;}
-	float variance() const {return variance_;}
+	float rotVariance() const {return rotVariance_;}
+	float transVariance() const {return transVariance_;}
 
 	void setFrom(int from) {from_ = from;}
 	void setTo(int to) {to_ = to;}
 	void setTransform(const Transform & transform) {transform_ = transform;}
 	void setType(Type type) {type_ = type;}
-	void setVariance(float variance) {variance_ = variance;}
+	void setVariance(float rotVariance, float transVariance) {rotVariance_ = rotVariance; transVariance_ = transVariance;}
 
 private:
 	int from_;
 	int to_;
 	Transform transform_;
 	Type type_;
-	float variance_;
+	float rotVariance_;
+	float transVariance_;
 };
 
 }

corelib/include/rtabmap/core/Memory.h

@@ -80,8 +80,8 @@ public:
 	std::list<int> cleanup(const std::list<int> & ignoredIds = std::list<int>());
 	void emptyTrash();
 	void joinTrashThread();
-	bool addLink(int to, int from, const Transform & transform, Link::Type type, float variance);
+	bool addLink(int to, int from, const Transform & transform, Link::Type type, float rotVariance, float transVariance);
-	void updateLink(int fromId, int toId, const Transform & transform, float variance);
+	void updateLink(int fromId, int toId, const Transform & transform, float rotVariance, float transVariance);
 	void removeAllVirtualLinks();
 	std::map<int, int> getNeighborsId(int signatureId,
 			int margin,
@@ -167,7 +167,7 @@ public:
 
 private:
 	void preUpdate();
-	void addSignatureToStm(Signature * signature, float odomVariance);
+	void addSignatureToStm(Signature * signature, float poseRotVariance, float poseTransVariance);
 	void clear();
 	void moveToTrash(Signature * s, bool saveToDatabase = true, std::list<int> * deletedWords = 0);
 

corelib/include/rtabmap/core/Parameters.h

@@ -332,6 +332,7 @@ class RTABMAP_EXP Parameters
 	// Loop closure constraint
 	RTABMAP_PARAM(LccIcp, Type,            int, 0, 			"0=No ICP, 1=ICP 3D, 2=ICP 2D");
 	RTABMAP_PARAM(LccIcp, MaxDistance,     float, 0.2,     "Maximum ICP correction distance accepted (m).");
+	RTABMAP_PARAM(LccIcp, HighTransitionalVariance,  bool, true, 	"Set high transitional variance on odometry pose correction and on local loop closure in space. Particularly useful when the robot is moving in hallways with short-range laser rangefinder.");
 
 	RTABMAP_PARAM(LccBow, MinInliers,      int, 20, 		"Minimum visual word correspondences to compute geometry transform.");
 	RTABMAP_PARAM(LccBow, InlierDistance,  float, 0.02, 	"Maximum distance for visual word correspondences.");
@@ -349,14 +350,14 @@ class RTABMAP_EXP Parameters
 	RTABMAP_PARAM(LccIcp3, VoxelSize,       float, 0.01, 	"Voxel size to be used for ICP computation.");
 	RTABMAP_PARAM(LccIcp3, Samples,         int, 0, 		"Random samples to be used for ICP computation. Not used if voxelSize is set.");
 	RTABMAP_PARAM(LccIcp3, MaxCorrespondenceDistance, float, 0.05, "ICP 3D: Max distance for point correspondences.");
-	RTABMAP_PARAM(LccIcp3, Iterations,      int, 30, 		"ICP 3D: Max iterations.");
+	RTABMAP_PARAM(LccIcp3, Iterations,      int, 30, 		"Max iterations.");
-	RTABMAP_PARAM(LccIcp3, CorrespondenceRatio, float, 0.7, 	"ICP 3D: Ratio of matching correspondences to accept the transform.");
+	RTABMAP_PARAM(LccIcp3, CorrespondenceRatio, float, 0.7, 	"Ratio of matching correspondences to accept the transform.");
-	RTABMAP_PARAM(LccIcp3, PointToPlane,      bool, false, 	"ICP 3D: Use point to plane ICP.");
+	RTABMAP_PARAM(LccIcp3, PointToPlane,      bool, false, 	"Use point to plane ICP.");
-	RTABMAP_PARAM(LccIcp3, PointToPlaneNormalNeighbors,      int, 20, 	"ICP 3D: Number of neighbors to compute normals for point to plane.");
+	RTABMAP_PARAM(LccIcp3, PointToPlaneNormalNeighbors,      int, 20, 	"Number of neighbors to compute normals for point to plane.");
 
-	RTABMAP_PARAM(LccIcp2, MaxCorrespondenceDistance, float, 0.1, 	"ICP 2D: Max distance for point correspondences.");
+	RTABMAP_PARAM(LccIcp2, MaxCorrespondenceDistance, float, 0.1, 	"Max distance for point correspondences.");
-	RTABMAP_PARAM(LccIcp2, Iterations,      int, 30, 				"ICP 2D: Max iterations.");
+	RTABMAP_PARAM(LccIcp2, Iterations,      int, 30, 				"Max iterations.");
-	RTABMAP_PARAM(LccIcp2, CorrespondenceRatio, float, 0.7, 		"ICP 2D: Ratio of matching correspondences to accept the transform.");
+	RTABMAP_PARAM(LccIcp2, CorrespondenceRatio, float, 0.7, 		"Ratio of matching correspondences to accept the transform.");
 	RTABMAP_PARAM(LccIcp2, VoxelSize,       float, 0.005, 			"Voxel size to be used for ICP computation.");
 
 	// Stereo disparity

corelib/include/rtabmap/core/Rtabmap.h

@@ -183,6 +183,7 @@ private:
 	bool _startNewMapOnLoopClosure;
 	float _goalReachedRadius; // meters
 	bool _planWithNearNodesLinked;
+	bool _icpHighTransVariance;
 
 	std::pair<int, float> _loopClosureHypothesis;
 	std::pair<int, float> _highestHypothesis;

corelib/include/rtabmap/core/RtabmapThread.h

@@ -110,7 +110,8 @@ private:
 	Rtabmap * _rtabmap;
 	bool _paused;
 	Transform lastPose_;
-	float _variance;
+	float _rotVariance;
+	float _transVariance;
 };
 
 } /* namespace rtabmap */

corelib/include/rtabmap/core/SensorData.h

@@ -54,8 +54,9 @@ public:
 		  float cy,
 		  const Transform & localTransform,
 		  const Transform & pose,
-		  float poseVariance,
+		  float poseRotVariance,
-		  int id = 0);
+		  float poseTransVariance,
+		  int id);
 
 	// Metric constructor + 2d laser scan
 	SensorData(const cv::Mat & laserScan,
@@ -67,8 +68,9 @@ public:
 		  float cy,
 		  const Transform & localTransform,
 		  const Transform & pose,
-		  float poseVariance,
+		  float poseRotVariance,
-		  int id = 0);
+		  float poseTransVariance,
+		  int id);
 
 	virtual ~SensorData() {}
 
@@ -82,7 +84,7 @@ public:
 	void setId(int id) {_id = id;}
 
 	bool isMetric() const {return !_depthOrRightImage.empty() || _fx != 0.0f || _fyOrBaseline != 0.0f || !_pose.isNull();}
-	void setPose(const Transform & pose, float variance) {_pose = pose; _poseVariance=variance;}
+	void setPose(const Transform & pose, float rotVariance, float transVariance) {_pose = pose; _poseRotVariance=rotVariance; _poseTransVariance = transVariance;}
 	cv::Mat depth() const {return (_depthOrRightImage.type()==CV_32FC1 || _depthOrRightImage.type()==CV_16UC1)?_depthOrRightImage:cv::Mat();}
 	cv::Mat rightImage() const {return _depthOrRightImage.type()==CV_8UC1?_depthOrRightImage:cv::Mat();}
 	const cv::Mat & depthOrRightImage() const {return _depthOrRightImage;}
@@ -95,7 +97,8 @@ public:
 	float fyOrBaseline() const {return _fyOrBaseline;}
 	const Transform & pose() const {return _pose;}
 	const Transform & localTransform() const {return _localTransform;}
-	float poseVariance() const {return _poseVariance;}
+	float poseRotVariance() const {return _poseRotVariance;}
+	float poseTransVariance() const {return _poseTransVariance;}
 
 	void setFeatures(const std::vector<cv::KeyPoint> & keypoints, const cv::Mat & descriptors)
 	{
@@ -118,7 +121,8 @@ private:
 	float _cy;
 	Transform _pose;
 	Transform _localTransform;
-	float _poseVariance;
+	float _poseRotVariance;
+	float _poseTransVariance;
 
 	// features
 	std::vector<cv::KeyPoint> _keypoints;

corelib/src/DBDriverSqlite3.cpp

@@ -1360,7 +1360,11 @@ void DBDriverSqlite3::loadLinksQuery(
 		sqlite3_stmt * ppStmt = 0;
 		std::stringstream query;
 
-		if(uStrNumCmp(_version, "0.7.4") >= 0)
+		if(uStrNumCmp(_version, "0.8.4") >= 0)
+		{
+			query << "SELECT to_id, type, transform, rot_variance, trans_variance FROM Link ";
+		}
+		else if(uStrNumCmp(_version, "0.7.4") >= 0)
 		{
 			query << "SELECT to_id, type, transform, variance FROM Link ";
 		}
@@ -1391,7 +1395,8 @@ void DBDriverSqlite3::loadLinksQuery(
 
 		int toId = -1;
 		int type = Link::kUndef;
-		float variance = 1.0f;
+		float rotVariance = 1.0f;
+		float transVariance = 1.0f;
 		const void * data = 0;
 		int dataSize = 0;
 
@@ -1417,15 +1422,21 @@ void DBDriverSqlite3::loadLinksQuery(
 				UERROR("Error while loading link transform from %d to %d! Setting to null...", signatureId, toId);
 			}
 
-			if(uStrNumCmp(_version, "0.7.4") >= 0)
+			if(uStrNumCmp(_version, "0.8.4") >= 0)
 			{
-				variance = sqlite3_column_double(ppStmt, index++);
+				rotVariance = sqlite3_column_double(ppStmt, index++);
-				neighbors.insert(neighbors.end(), std::make_pair(toId, Link(signatureId, toId, (Link::Type)type, transform, variance)));
+				transVariance = sqlite3_column_double(ppStmt, index++);
+				neighbors.insert(neighbors.end(), std::make_pair(toId, Link(signatureId, toId, (Link::Type)type, transform, rotVariance, transVariance)));
+			}
+			else if(uStrNumCmp(_version, "0.7.4") >= 0)
+			{
+				rotVariance = transVariance = sqlite3_column_double(ppStmt, index++);
+				neighbors.insert(neighbors.end(), std::make_pair(toId, Link(signatureId, toId, (Link::Type)type, transform, rotVariance, transVariance)));
 			}
 			else
 			{
 				// neighbor is 0, loop closures are 1 and 2 (child)
-				neighbors.insert(neighbors.end(), std::make_pair(toId, Link(signatureId, toId, type==0?Link::kNeighbor:Link::kGlobalClosure, transform, variance)));
+				neighbors.insert(neighbors.end(), std::make_pair(toId, Link(signatureId, toId, type==0?Link::kNeighbor:Link::kGlobalClosure, transform, rotVariance, transVariance)));
 			}
 
 			rc = sqlite3_step(ppStmt);
@@ -1455,7 +1466,13 @@ void DBDriverSqlite3::loadLinksQuery(std::list<Signature *> & signatures) const
 		std::stringstream query;
 		int totalLinksLoaded = 0;
 
-		if(uStrNumCmp(_version, "0.7.4") >= 0)
+		if(uStrNumCmp(_version, "0.8.4") >= 0)
+		{
+			query << "SELECT to_id, type, rot_variance, trans_variance, transform FROM Link "
+				  << "WHERE from_id = ? "
+				  << "ORDER BY to_id";
+		}
+		else if(uStrNumCmp(_version, "0.7.4") >= 0)
 		{
 			query << "SELECT to_id, type, variance, transform FROM Link "
 				  << "WHERE from_id = ? "
@@ -1479,7 +1496,8 @@ void DBDriverSqlite3::loadLinksQuery(std::list<Signature *> & signatures) const
 
 			int toId = -1;
 			int linkType = -1;
-			float variance = 1.0f;
+			float rotVariance = 1.0f;
+			float transVariance = 1.0f;
 			std::list<Link> links;
 			const void * data = 0;
 			int dataSize = 0;
@@ -1492,9 +1510,14 @@ void DBDriverSqlite3::loadLinksQuery(std::list<Signature *> & signatures) const
 
 				toId = sqlite3_column_int(ppStmt, index++);
 				linkType = sqlite3_column_int(ppStmt, index++);
-				if(uStrNumCmp(_version, "0.7.4") >= 0)
+				if(uStrNumCmp(_version, "0.8.4") >= 0)
 				{
-					variance = sqlite3_column_double(ppStmt, index++);
+					rotVariance = sqlite3_column_double(ppStmt, index++);
+					transVariance = sqlite3_column_double(ppStmt, index++);
+				}
+				else if(uStrNumCmp(_version, "0.7.4") >= 0)
+				{
+					rotVariance = transVariance = sqlite3_column_double(ppStmt, index++);
 				}
 
 				//transform
@@ -1514,11 +1537,11 @@ void DBDriverSqlite3::loadLinksQuery(std::list<Signature *> & signatures) const
 				{
 					if(uStrNumCmp(_version, "0.7.4") >= 0)
 					{
-						links.push_back(Link((*iter)->id(), toId, (Link::Type)linkType, transform, variance));
+						links.push_back(Link((*iter)->id(), toId, (Link::Type)linkType, transform, rotVariance, transVariance));
 					}
 					else // neighbor is 0, loop closures are 1 and 2 (child)
 					{
-						links.push_back(Link((*iter)->id(), toId, linkType == 0?Link::kNeighbor:Link::kGlobalClosure, transform, variance));
+						links.push_back(Link((*iter)->id(), toId, linkType == 0?Link::kNeighbor:Link::kGlobalClosure, transform, rotVariance, transVariance));
 					}
 				}
 				else
@@ -1631,7 +1654,7 @@ void DBDriverSqlite3::updateQuery(const std::list<Signature *> & nodes, bool upd
 				const std::map<int, Link> & links = (*j)->getLinks();
 				for(std::map<int, Link>::const_iterator i=links.begin(); i!=links.end(); ++i)
 				{
-					stepLink(ppStmt, (*j)->id(), i->first, i->second.type(), i->second.variance(), i->second.transform());
+					stepLink(ppStmt, (*j)->id(), i->first, i->second.type(), i->second.rotVariance(), i->second.transVariance(), i->second.transform());
 				}
 			}
 		}
@@ -1739,7 +1762,7 @@ void DBDriverSqlite3::saveQuery(const std::list<Signature *> & signatures) const
 			const std::map<int, Link> & links = (*jter)->getLinks();
 			for(std::map<int, Link>::const_iterator i=links.begin(); i!=links.end(); ++i)
 			{
-				stepLink(ppStmt, (*jter)->id(), i->first, i->second.type(), i->second.variance(), i->second.transform());
+				stepLink(ppStmt, (*jter)->id(), i->first, i->second.type(), i->second.rotVariance(), i->second.transVariance(), i->second.transform());
 			}
 		}
 		// Finalize (delete) the statement
@@ -2027,7 +2050,11 @@ void DBDriverSqlite3::stepDepth(sqlite3_stmt * ppStmt,
 
 std::string DBDriverSqlite3::queryStepLink() const
 {
-	if(uStrNumCmp(_version, "0.7.4") >= 0)
+	if(uStrNumCmp(_version, "0.8.4") >= 0)
+	{
+		return "INSERT INTO Link(from_id, to_id, type, rot_variance, trans_variance, transform) VALUES(?,?,?,?,?,?);";
+	}
+	else if(uStrNumCmp(_version, "0.7.4") >= 0)
 	{
 		return "INSERT INTO Link(from_id, to_id, type, variance, transform) VALUES(?,?,?,?,?);";
 	}
@@ -2036,7 +2063,14 @@ std::string DBDriverSqlite3::queryStepLink() const
 		return "INSERT INTO Link(from_id, to_id, type, transform) VALUES(?,?,?,?);";
 	}
 }
-void DBDriverSqlite3::stepLink(sqlite3_stmt * ppStmt, int fromId, int toId, Link::Type type, float variance, const Transform & transform) const
+void DBDriverSqlite3::stepLink(
+		sqlite3_stmt * ppStmt,
+		int fromId,
+		int toId,
+		Link::Type type,
+		float rotVariance,
+		float transVariance,
+		const Transform & transform) const
 {
 	if(!ppStmt)
 	{
@@ -2060,9 +2094,16 @@ void DBDriverSqlite3::stepLink(sqlite3_stmt * ppStmt, int fromId, int toId, Link
 	rc = sqlite3_bind_int(ppStmt, index++, type);
 	UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error: %s", sqlite3_errmsg(_ppDb)).c_str());
 
-	if(uStrNumCmp(_version, "0.7.4") >= 0)
+	if(uStrNumCmp(_version, "0.8.4") >= 0)
 	{
-		rc = sqlite3_bind_double(ppStmt, index++, variance);
+		rc = sqlite3_bind_double(ppStmt, index++, rotVariance);
+		UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error: %s", sqlite3_errmsg(_ppDb)).c_str());
+		rc = sqlite3_bind_double(ppStmt, index++, transVariance);
+		UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error: %s", sqlite3_errmsg(_ppDb)).c_str());
+	}
+	else if(uStrNumCmp(_version, "0.7.4") >= 0)
+	{
+		rc = sqlite3_bind_double(ppStmt, index++, rotVariance<transVariance?rotVariance:transVariance);
 		UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error: %s", sqlite3_errmsg(_ppDb)).c_str());
 	}
 

corelib/src/DBDriverSqlite3.h

@@ -109,7 +109,7 @@ private:
 			float cx,
 			float cy,
 			const Transform & localTransform) const;
-	void stepLink(sqlite3_stmt * ppStmt, int fromId, int toId, Link::Type type, float variance, const Transform & transform) const;
+	void stepLink(sqlite3_stmt * ppStmt, int fromId, int toId, Link::Type type, float rotVariance, float transVariance, const Transform & transform) const;
 	void stepWordsChanged(sqlite3_stmt * ppStmt, int signatureId, int oldWordId, int newWordId) const;
 	void stepKeypoint(sqlite3_stmt * ppStmt, int signatureId, int wordId, const cv::KeyPoint & kp, const pcl::PointXYZ & pt) const;
 

corelib/src/DBReader.cpp

@@ -193,7 +193,8 @@ SensorData DBReader::getNextData()
 			int mapId;
 			float fx,fy,cx,cy;
 			Transform localTransform, pose;
-			float variance = 1.0f;
+			float rotVariance = 1.0f;
+			float transVariance = 1.0f;
 			_dbDriver->getNodeData(*_currentId, imageBytes, depthBytes, laserScanBytes, fx, fy, cx, cy, localTransform);
 			if(!_odometryIgnored)
 			{
@@ -203,7 +204,8 @@ SensorData DBReader::getNextData()
 				if(links.size())
 				{
 					// assume the first is the backward neighbor, take its variance
-					variance = links.begin()->second.variance();
+					rotVariance = links.begin()->second.rotVariance();
+					transVariance = links.begin()->second.transVariance();
 				}
 			}
 			int seq = *_currentId;
@@ -229,7 +231,8 @@ SensorData DBReader::getNextData()
 					fx,fy,cx,cy,
 					localTransform,
 					pose,
-					variance,
+					rotVariance,
+					transVariance,
 					seq);
 			UDEBUG("Laser=%d RGB/Left=%d Depth=%d Right=%d",
 					data.laserScan().empty()?0:1,

corelib/src/Graph.cpp

@@ -185,7 +185,7 @@ void optimizeTOROGraph(
 			{
 				UASSERT(uContains(rtabmapToToro, iter->first) && uContains(rtabmapToToro, iter->second.to()));
 				UASSERT(!iter->second.transform().isNull());
-				edgeConstraintsToro.insert(std::make_pair(rtabmapToToro.at(iter->first), Link(rtabmapToToro.at(iter->first), rtabmapToToro.at(iter->second.to()), iter->second.type(), iter->second.transform(), iter->second.variance())));
+				edgeConstraintsToro.insert(std::make_pair(rtabmapToToro.at(iter->first), Link(rtabmapToToro.at(iter->first), rtabmapToToro.at(iter->second.to()), iter->second.type(), iter->second.transform(), iter->second.rotVariance(), iter->second.transVariance())));
 			}
 		}
 
@@ -285,14 +285,20 @@ void optimizeTOROGraph(
 			pcl::getTranslationAndEulerAngles(iter->second.transform().toEigen3f(), x,y,z, roll,pitch,yaw);
 			AISNavigation::TreePoseGraph3::Pose p(x, y, z, roll, pitch, yaw);
 			AISNavigation::TreePoseGraph3::InformationMatrix inf = DMatrix<double>::I(6);
-			if(!ignoreCovariance && iter->second.variance()>0)
+			if(!ignoreCovariance)
 			{
-				inf[0][0] = 1.0f/iter->second.variance(); // x
+				if(iter->second.rotVariance()>0)
-				inf[1][1] = 1.0f/iter->second.variance(); // y
+				{
-				inf[2][2] = 1.0f/iter->second.variance(); // z
+					inf[0][0] = 1.0f/iter->second.rotVariance(); // roll
-				inf[3][3] = 1.0f/iter->second.variance(); // roll
+					inf[1][1] = 1.0f/iter->second.rotVariance(); // pitch
-				inf[4][4] = 1.0f/iter->second.variance(); // pitch
+					inf[2][2] = 1.0f/iter->second.rotVariance(); // yaw
-				inf[5][5] = 1.0f/iter->second.variance(); // yaw
+				}
+				if(iter->second.transVariance()>0)
+				{
+					inf[3][3] = 1.0f/iter->second.transVariance(); // x
+					inf[4][4] = 1.0f/iter->second.transVariance(); // y
+					inf[5][5] = 1.0f/iter->second.transVariance(); // z
+				}
 			}
 
 			AISNavigation::TreePoseGraph<AISNavigation::Operations3D<double> >::Vertex* v1=pg.vertex(id1);
@@ -409,12 +415,12 @@ bool saveTOROGraph(
 					roll,
 					pitch,
 					yaw,
-					1.0f/iter->second.variance(),
+					1.0f/iter->second.rotVariance(),
-					1.0f/iter->second.variance(),
+					1.0f/iter->second.rotVariance(),
-					1.0f/iter->second.variance(),
+					1.0f/iter->second.rotVariance(),
-					1.0f/iter->second.variance(),
+					1.0f/iter->second.transVariance(),
-					1.0f/iter->second.variance(),
+					1.0f/iter->second.transVariance(),
-					1.0f/iter->second.variance());
+					1.0f/iter->second.transVariance());
 		}
 		UINFO("Graph saved to %s", fileName.c_str());
 		fclose(file);

corelib/src/Memory.cpp

@@ -548,7 +548,7 @@ bool Memory::update(const SensorData & data, Statistics * stats)
 	UDEBUG("time creating signature=%f ms", t);
 
 	// It will be added to the short-term memory, no need to delete it...
-	this->addSignatureToStm(signature, data.poseVariance());
+	this->addSignatureToStm(signature, data.poseRotVariance(), data.poseTransVariance());
 
 	_lastSignature = signature;
 
@@ -657,7 +657,7 @@ void Memory::setRoi(const std::string & roi)
 	}
 }
 
-void Memory::addSignatureToStm(Signature * signature, float poseVariance)
+void Memory::addSignatureToStm(Signature * signature, float poseRotVariance, float poseTransVariance)
 {
 	UTimer timer;
 	// add signature on top of the short-term memory
@@ -674,13 +674,13 @@ void Memory::addSignatureToStm(Signature * signature, float poseVariance)
 				   !_signatures.at(*_stMem.rbegin())->getPose().isNull())
 				{
 					motionEstimate = _signatures.at(*_stMem.rbegin())->getPose().inverse() * signature->getPose();
-					_signatures.at(*_stMem.rbegin())->addLink(Link(*_stMem.rbegin(), signature->id(), Link::kNeighbor, motionEstimate, poseVariance));
+					_signatures.at(*_stMem.rbegin())->addLink(Link(*_stMem.rbegin(), signature->id(), Link::kNeighbor, motionEstimate, poseRotVariance, poseTransVariance));
-					signature->addLink(Link(signature->id(), *_stMem.rbegin(), Link::kNeighbor, motionEstimate.inverse(), poseVariance));
+					signature->addLink(Link(signature->id(), *_stMem.rbegin(), Link::kNeighbor, motionEstimate.inverse(), poseRotVariance, poseTransVariance));
 				}
 				else
 				{
-					_signatures.at(*_stMem.rbegin())->addLink(Link(*_stMem.rbegin(), signature->id(), Link::kNeighbor, Transform(), 1.0f));
+					_signatures.at(*_stMem.rbegin())->addLink(Link(*_stMem.rbegin(), signature->id(), Link::kNeighbor, Transform(), 1.0f, 1.0f));
-					signature->addLink(Link(signature->id(), *_stMem.rbegin(), Link::kNeighbor, Transform(), 1.0f));
+					signature->addLink(Link(signature->id(), *_stMem.rbegin(), Link::kNeighbor, Transform(), 1.0f, 1.0f));
 				}
 				UDEBUG("Min STM id = %d", *_stMem.begin());
 			}
@@ -2200,7 +2200,7 @@ Transform Memory::computeScanMatchingTransform(
 }
 
 // Transform from new to old
-bool Memory::addLink(int oldId, int newId, const Transform & transform, Link::Type type, float variance)
+bool Memory::addLink(int oldId, int newId, const Transform & transform, Link::Type type, float rotVariance, float transVariance)
 {
 	UASSERT(type > Link::kNeighbor && type != Link::kUndef);
 
@@ -2218,8 +2218,8 @@ bool Memory::addLink(int oldId, int newId, const Transform & transform, Link::Ty
 
 		UDEBUG("Add link between %d and %d", oldS->id(), newS->id());
 
-		oldS->addLink(Link(oldS->id(), newS->id(), type, transform.inverse(), variance));
+		oldS->addLink(Link(oldS->id(), newS->id(), type, transform.inverse(), rotVariance, transVariance));
-		newS->addLink(Link(newS->id(), oldS->id(), type, transform, variance));
+		newS->addLink(Link(newS->id(), oldS->id(), type, transform, rotVariance, transVariance));
 
 		if(type!=Link::kVirtualClosure)
 		{
@@ -2258,7 +2258,7 @@ bool Memory::addLink(int oldId, int newId, const Transform & transform, Link::Ty
 	return false;
 }
 
-void Memory::updateLink(int fromId, int toId, const Transform & transform, float variance)
+void Memory::updateLink(int fromId, int toId, const Transform & transform, float rotVariance, float transVariance)
 {
 	Signature * fromS = this->_getSignature(fromId);
 	Signature * toS = this->_getSignature(toId);
@@ -2269,8 +2269,8 @@ void Memory::updateLink(int fromId, int toId, const Transform & transform, float
 		fromS->removeLink(toId);
 		toS->removeLink(fromId);
 
-		fromS->addLink(Link(fromId, toId, type, transform, variance));
+		fromS->addLink(Link(fromId, toId, type, transform, rotVariance, transVariance));
-		toS->addLink(Link(toId, fromId, type, transform.inverse(), variance));
+		toS->addLink(Link(toId, fromId, type, transform.inverse(), rotVariance, transVariance));
 
 		if(type!=Link::kVirtualClosure)
 		{
@@ -2535,7 +2535,7 @@ bool Memory::rehearsalMerge(int oldId, int newId)
 			}
 
 			oldS->removeLinks(); // remove all links
-			oldS->addLink(Link(oldS->id(), newS->id(), Link::kGlobalClosure, Transform(), 1.0f)); // to keep track of the merged location
+			oldS->addLink(Link(oldS->id(), newS->id(), Link::kGlobalClosure, Transform(), 1.0f, 1.0f)); // to keep track of the merged location
 
 			// Set old image to new signature
 			this->copyData(oldS, newS);
@@ -2550,7 +2550,7 @@ bool Memory::rehearsalMerge(int oldId, int newId)
 		}
 		else
 		{
-			newS->addLink(Link(newS->id(), oldS->id(), Link::kGlobalClosure, Transform(), 1.0f)); // to keep track of the merged location
+			newS->addLink(Link(newS->id(), oldS->id(), Link::kGlobalClosure, Transform(), 1.0f, 1.0f)); // to keep track of the merged location
 
 			// update weight
 			oldS->setWeight(newS->getWeight() + 1 + oldS->getWeight());

corelib/src/Odometry.cpp

@@ -1301,7 +1301,7 @@ void OdometryThread::mainLoop()
 	{
 		OdometryInfo info;
 		Transform pose = _odometry->process(data, &info);
-		data.setPose(pose, info.variance); // a null pose notify that odometry could not be computed
+		data.setPose(pose, info.variance, info.variance); // a null pose notify that odometry could not be computed
 		this->post(new OdometryEvent(data, info));
 	}
 }

corelib/src/Rtabmap.cpp

@@ -58,6 +58,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #define LOG_I "LogI.txt"
 
 #define GRAPH_FILE_NAME "Graph.dot"
+#define HIGH_VARIANCE 10000
 
 //
 //
@@ -110,6 +111,7 @@ Rtabmap::Rtabmap() :
 	_startNewMapOnLoopClosure(Parameters::defaultRtabmapStartNewMapOnLoopClosure()),
 	_goalReachedRadius(Parameters::defaultRGBDGoalReachedRadius()),
 	_planWithNearNodesLinked(Parameters::defaultRGBDPlanWithNearNodesLinked()),
+	_icpHighTransVariance(Parameters::defaultLccIcpHighTransitionalVariance()),
 	_loopClosureHypothesis(0,0.0f),
 	_highestHypothesis(0,0.0f),
 	_lastProcessTime(0.0),
@@ -376,6 +378,7 @@ void Rtabmap::parseParameters(const ParametersMap & parameters)
 	Parameters::parse(parameters, Parameters::kRtabmapStartNewMapOnLoopClosure(), _startNewMapOnLoopClosure);
 	Parameters::parse(parameters, Parameters::kRGBDGoalReachedRadius(), _goalReachedRadius);
 	Parameters::parse(parameters, Parameters::kRGBDPlanWithNearNodesLinked(), _planWithNearNodesLinked);
+	Parameters::parse(parameters, Parameters::kLccIcpHighTransitionalVariance(), _icpHighTransVariance);
 
 	// RGB-D SLAM stuff
 	if((iter=parameters.find(Parameters::kLccIcpType())) != parameters.end())
@@ -870,7 +873,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, variance);
+				_memory->updateLink(signature->id(), oldId, t, variance, _icpHighTransVariance?HIGH_VARIANCE:variance);
 			}
 			else
 			{
@@ -928,7 +931,7 @@ bool Rtabmap::process(const SensorData & data)
 								*iter,
 								transform.prettyPrint().c_str());
 						// Add a loop constraint
-						if(_memory->addLink(*iter, signature->id(), transform, Link::kLocalTimeClosure, variance))
+						if(_memory->addLink(*iter, signature->id(), transform, Link::kLocalTimeClosure, variance, variance))
 						{
 							++localLoopClosuresInTimeFound;
 							UINFO("Local loop closure found between %d and %d with t=%s",
@@ -1421,7 +1424,7 @@ bool Rtabmap::process(const SensorData & data)
 		if(!rejectedHypothesis)
 		{
 			// Make the new one the parent of the old one
-			rejectedHypothesis = !_memory->addLink(_loopClosureHypothesis.first, signature->id(), transform, Link::kGlobalClosure, variance);
+			rejectedHypothesis = !_memory->addLink(_loopClosureHypothesis.first, signature->id(), transform, Link::kGlobalClosure, variance, variance);
 		}
 
 		if(rejectedHypothesis)
@@ -1483,7 +1486,7 @@ bool Rtabmap::process(const SensorData & data)
 							signature->id(),
 							localSpaceNearestId,
 							t.prettyPrint().c_str());
-					_memory->addLink(localSpaceNearestId, signature->id(), t, Link::kLocalSpaceClosure, variance);
+					_memory->addLink(localSpaceNearestId, signature->id(), t, Link::kLocalSpaceClosure, variance, _icpHighTransVariance?HIGH_VARIANCE:variance);
 
 					// Old map -> new map, used for localization correction on loop closure
 					const Signature * oldS = _memory->getSignature(localSpaceNearestId);
@@ -1565,7 +1568,7 @@ bool Rtabmap::process(const SensorData & data)
 			Transform virtualLoop = _optimizedPoses.at(signature->id()).inverse() * _optimizedPoses.at(_path[_pathCurrentIndex].first);
 			if(_localRadius > 0.0f && virtualLoop.getNorm() < _localRadius)
 			{
-				_memory->addLink(_path[_pathCurrentIndex].first, signature->id(), virtualLoop, Link::kVirtualClosure, 99999);
+				_memory->addLink(_path[_pathCurrentIndex].first, signature->id(), virtualLoop, Link::kVirtualClosure, HIGH_VARIANCE, HIGH_VARIANCE);
 			}
 		}
 	}
@@ -2605,7 +2608,7 @@ void Rtabmap::updateGoalIndex()
 						if(!s->hasLink(_path[i-1].first) && _memory->getSignature(_path[i-1].first) != 0)
 						{
 							Transform virtualLoop = _path[i].second.inverse() * _path[i-1].second;
-							_memory->addLink(_path[i-1].first, _path[i].first, virtualLoop, Link::kVirtualClosure, 99999);
+							_memory->addLink(_path[i-1].first, _path[i].first, virtualLoop, Link::kVirtualClosure, HIGH_VARIANCE, HIGH_VARIANCE);
 							UINFO("Added Virtual link between %d and %d", _path[i-1].first, _path[i].first);
 						}
 					}

corelib/src/RtabmapThread.cpp

@@ -48,7 +48,8 @@ RtabmapThread::RtabmapThread(Rtabmap * rtabmap) :
 		_rtabmap(rtabmap),
 		_paused(false),
 		lastPose_(Transform::getIdentity()),
-		_variance(0)
+		_rotVariance(0),
+		_transVariance(0)
 
 {
 	UASSERT(rtabmap != 0);
@@ -85,7 +86,8 @@ void RtabmapThread::clearBufferedData()
 	{
 		_dataBuffer.clear();
 		lastPose_.setIdentity();
-		_variance = 0;
+		_rotVariance = 0;
+		_transVariance = 0;
 	}
 	_dataMutex.unlock();
 }
@@ -437,13 +439,18 @@ void RtabmapThread::addData(const SensorData & sensorData)
 		{
 			UWARN("Odometry is reset (identity pose detected). Increment map id!");
 			pushNewState(kStateTriggeringMap);
-			_variance = 0;
+			_rotVariance = 0;
+			_transVariance = 0;
 		}
 
 		lastPose_ = sensorData.pose();
-		if(sensorData.poseVariance() > _variance)
+		if(sensorData.poseRotVariance() > _rotVariance)
 		{
-			_variance = sensorData.poseVariance();
+			_rotVariance = sensorData.poseRotVariance();
+		}
+		if(sensorData.poseTransVariance() > _transVariance)
+		{
+			_transVariance = sensorData.poseTransVariance();
 		}
 
 		if(_rate>0.0f)
@@ -459,12 +466,17 @@ void RtabmapThread::addData(const SensorData & sensorData)
 		_dataMutex.lock();
 		{
 			_dataBuffer.push_back(sensorData);
-			if(_variance <= 0)
+			if(_rotVariance <= 0)
 			{
-				_variance = 1.0f;
+				_rotVariance = 1.0f;
 			}
-			_dataBuffer.back().setPose(_dataBuffer.back().pose(), _variance);
+			if(_transVariance <= 0)
-			_variance = 0;
+			{
+				_transVariance = 1.0f;
+			}
+			_dataBuffer.back().setPose(_dataBuffer.back().pose(), _rotVariance, _transVariance);
+			_rotVariance = 0;
+			_transVariance = 0;
 			while(_dataBufferMaxSize > 0 && _dataBuffer.size() > (unsigned int)_dataBufferMaxSize)
 			{
 				ULOGGER_WARN("Data buffer is full, the oldest data is removed to add the new one.");

corelib/src/SensorData.cpp

@@ -43,7 +43,8 @@ SensorData::SensorData() :
 	_cx(0.0f),
 	_cy(0.0f),
 	_localTransform(Transform::getIdentity()),
-	_poseVariance(1.0f)
+	_poseRotVariance(1.0f),
+	_poseTransVariance(1.0f)
 {
 }
 
@@ -56,7 +57,8 @@ SensorData::SensorData(const cv::Mat & image,
 	_cx(0.0f),
 	_cy(0.0f),
 	_localTransform(Transform::getIdentity()),
-	_poseVariance(1.0f)
+	_poseRotVariance(1.0f),
+	_poseTransVariance(1.0f)
 {
 	UASSERT(image.type() == CV_8UC1 || // Mono
 			image.type() == CV_8UC3);  // RGB
@@ -71,7 +73,8 @@ SensorData::SensorData(const cv::Mat & image,
 		  float cy,
 		  const Transform & localTransform,
 		  const Transform & pose,
-		  float poseVariance,
+		  float poseRotVariance,
+		  float poseTransVariance,
 		  int id) :
 	_image(image),
 	_id(id),
@@ -82,7 +85,8 @@ SensorData::SensorData(const cv::Mat & image,
 	_cy(cy),
 	_pose(pose),
 	_localTransform(localTransform),
-	_poseVariance(poseVariance)
+	_poseRotVariance(poseRotVariance),
+	_poseTransVariance(poseTransVariance)
 {
 	UASSERT(image.type() == CV_8UC1 || // Mono
 			image.type() == CV_8UC3);  // RGB
@@ -103,7 +107,8 @@ SensorData::SensorData(const cv::Mat & laserScan,
 		  float cy,
 		  const Transform & localTransform,
 		  const Transform & pose,
-		  float poseVariance,
+		  float poseRotVariance,
+		  float poseTransVariance,
 		  int id) :
 	_image(image),
 	_id(id),
@@ -115,7 +120,8 @@ SensorData::SensorData(const cv::Mat & laserScan,
 	_cy(cy),
 	_pose(pose),
 	_localTransform(localTransform),
-	_poseVariance(poseVariance)
+	_poseRotVariance(poseRotVariance),
+	_poseTransVariance(poseTransVariance)
 {
 	UASSERT(_laserScan.empty() || _laserScan.type() == CV_32FC2);
 	UASSERT(image.type() == CV_8UC1 || // Mono

corelib/src/Signature.cpp

@@ -229,7 +229,8 @@ void Signature::setDepthCompressed(const cv::Mat & bytes, float fx, float fy, fl
 SensorData Signature::toSensorData()
 {
 	this->uncompressData();
-	float variance = 1.0f;
+	float rotVariance = 1.0f;
+	float transVariance = 1.0f;
 	if(_links.size())
 	{
 		for(std::map<int, Link>::iterator iter = _links.begin(); iter!=_links.end(); ++iter)
@@ -239,7 +240,8 @@ SensorData Signature::toSensorData()
 				//Assume the first neighbor to be the backward neighbor link
 				if(iter->second.to() < iter->second.from())
 				{
-					variance = iter->second.variance();
+					rotVariance = iter->second.rotVariance();
+					transVariance = iter->second.transVariance();
 					break;
 				}
 			}
@@ -254,7 +256,8 @@ SensorData Signature::toSensorData()
 			_cy,
 			_localTransform,
 			_pose,
-			variance,
+			rotVariance,
+			transVariance,
 			_id);
 }
 

corelib/src/resources/DatabaseSchema.sql.in

@@ -46,7 +46,8 @@ CREATE TABLE Link (
 	from_id INTEGER NOT NULL,
 	to_id INTEGER NOT NULL,
 	type INTEGER NOT NULL, -- neighbor=0, loop=1, child=2
-	variance FLOAT NOT NULL,
+	rot_variance FLOAT NOT NULL,
+	trans_variance FLOAT NOT NULL,
 	transform BLOB,
 	FOREIGN KEY (from_id) REFERENCES Node(id),
 	FOREIGN KEY (to_id) REFERENCES Node(id)

guilib/src/DatabaseViewer.cpp

@@ -244,11 +244,23 @@ void DatabaseViewer::closeEvent(QCloseEvent* event)
 				std::multimap<int, rtabmap::Link>::iterator refinedIter = rtabmap::graph::findLink(linksRefined_, iter->second.from(), iter->second.to());
 				if(refinedIter != linksRefined_.end())
 				{
-					memory_->addLink(refinedIter->second.to(), refinedIter->second.from(), refinedIter->second.transform(), refinedIter->second.type(), refinedIter->second.variance());
+					memory_->addLink(
+							refinedIter->second.to(),
+							refinedIter->second.from(),
+							refinedIter->second.transform(),
+							refinedIter->second.type(),
+							refinedIter->second.rotVariance(),
+							refinedIter->second.transVariance());
 				}
 				else
 				{
-					memory_->addLink(iter->second.to(), iter->second.from(), iter->second.transform(), iter->second.type(), iter->second.variance());
+					memory_->addLink(
+							iter->second.to(),
+							iter->second.from(),
+							iter->second.transform(),
+							iter->second.type(),
+							iter->second.rotVariance(),
+							iter->second.transVariance());
 				}
 			}
 
@@ -257,7 +269,12 @@ void DatabaseViewer::closeEvent(QCloseEvent* event)
 			{
 				if(!containsLink(linksAdded_, iter->second.from(), iter->second.to()))
 				{
-					memory_->updateLink(iter->second.from(), iter->second.to(), iter->second.transform(), iter->second.variance());
+					memory_->updateLink(
+							iter->second.from(),
+							iter->second.to(),
+							iter->second.transform(),
+							iter->second.rotVariance(),
+							iter->second.transVariance());
 				}
 			}
 
@@ -1431,7 +1448,7 @@ void DatabaseViewer::updateConstraintView(const rtabmap::Link & linkIn,
 	ui_->checkBox_showOptimized->setEnabled(false);
 	UASSERT(!t.isNull() && memory_);
 
-	ui_->label_constraint->setText(QString("%1 (%2=%3)").arg(t.prettyPrint().c_str()).arg(QChar(0xc3, 0x03)).arg(sqrt(link.variance())));
+	ui_->label_constraint->setText(QString("%1 (%2r=%3 %4t=%5)").arg(t.prettyPrint().c_str()).arg(QChar(0xc3, 0x03)).arg(sqrt(link.rotVariance())).arg(QChar(0xc3, 0x03)).arg(sqrt(link.transVariance())));
 	if(link.type() == Link::kNeighbor &&
 	   graphes_.size() &&
 	   (int)graphes_.size()-1 == ui_->horizontalSlider_iterations->maximum())
@@ -2042,7 +2059,7 @@ void DatabaseViewer::refineConstraint(int from, int to, bool updateGraph)
 
 	if(hasConverged && !transform.isNull())
 	{
-		Link newLink(currentLink.from(), currentLink.to(), currentLink.type(), transform*t, variance);
+		Link newLink(currentLink.from(), currentLink.to(), currentLink.type(), transform*t, variance, variance);
 
 		bool updated = false;
 		std::multimap<int, Link>::iterator iter = linksRefined_.find(currentLink.from());
@@ -2147,7 +2164,7 @@ void DatabaseViewer::refineConstraintVisually(int from, int to, bool updateGraph
 
 	if(!t.isNull())
 	{
-		Link newLink(currentLink.from(), currentLink.to(), currentLink.type(), t, variance);
+		Link newLink(currentLink.from(), currentLink.to(), currentLink.type(), t, variance, variance);
 
 		bool updated = false;
 		std::multimap<int, Link>::iterator iter = linksRefined_.find(currentLink.from());
@@ -2275,7 +2292,7 @@ bool DatabaseViewer::addConstraint(int from, int to, bool silent, bool updateGra
 			}
 
 			// transform is valid, make a link
-			linksAdded_.insert(std::make_pair(from, Link(from, to, Link::kUserClosure, t, variance)));
+			linksAdded_.insert(std::make_pair(from, Link(from, to, Link::kUserClosure, t, variance, variance)));
 			updateSlider = true;
 		}
 	}

guilib/src/MainWindow.cpp

@@ -3193,7 +3193,7 @@ void MainWindow::postProcessing()
 							UINFO("Added new loop closure between %d and %d.", from, to);
 							addedLinks.insert(from);
 							addedLinks.insert(to);
-							_currentLinksMap.insert(std::make_pair(from, Link(from, to, Link::kUserClosure, transform, variance)));
+							_currentLinksMap.insert(std::make_pair(from, Link(from, to, Link::kUserClosure, transform, variance, variance)));
 							++loopClosuresAdded;
 							_initProgressDialog->appendText(tr("Detected loop closure %1->%2! (%3/%4)").arg(from).arg(to).arg(i+1).arg(clusters.size()));
 						}
@@ -3349,7 +3349,7 @@ void MainWindow::postProcessing()
 					if(!transform.isNull() && hasConverged &&
 					   correspondencesRatio >= correspondenceRatio)
 					{
-						Link newLink(from, to, iter->second.type(), transform*iter->second.transform(), variance);
+						Link newLink(from, to, iter->second.type(), transform*iter->second.transform(), variance, variance);
 						iter->second = newLink;
 					}
 					else

guilib/src/PreferencesDialog.cpp

@@ -495,6 +495,8 @@ PreferencesDialog::PreferencesDialog(QWidget * parent) :
 	_ui->loopClosure_icp2Iterations->setObjectName(Parameters::kLccIcp2Iterations().c_str());
 	_ui->loopClosure_icp2Ratio->setObjectName(Parameters::kLccIcp2CorrespondenceRatio().c_str());
 	_ui->loopClosure_icp2Voxel->setObjectName(Parameters::kLccIcp2VoxelSize().c_str());
+	_ui->loopClosure_highICPTransitionalVariance->setObjectName(Parameters::kLccIcpHighTransitionalVariance().c_str());
+
 
 	//Odometry
 	_ui->odom_strategy->setObjectName(Parameters::kOdomStrategy().c_str());
@@ -2478,7 +2480,6 @@ void PreferencesDialog::addParameter(const QObject * object, bool value)
 				this->addParameters(_ui->groupBox_graphOptimization);
 				this->addParameters(_ui->groupBox_localDetection_time);
 				this->addParameters(_ui->groupBox_localDetection_space);
-				this->addParameters(_ui->groupBox_globalConstraints);
 				this->addParameters(_ui->groupBox_visualTransform2);
 			}
 
@@ -2487,7 +2488,6 @@ void PreferencesDialog::addParameter(const QObject * object, bool value)
 				// RGBD panel
 				if(value && groupBox == _ui->groupBox_localDetection_time)
 				{
-					this->addParameters(_ui->groupBox_globalConstraints);
 					this->addParameters(_ui->groupBox_visualTransform2);
 				}
 				if(value && groupBox == _ui->groupBox_localDetection_space)

guilib/src/ui/preferencesDialog.ui

@@ -63,9 +63,9 @@
           <property name="geometry">
            <rect>
             <x>0</x>
-            <y>-522</y>
+            <y>0</y>
             <width>744</width>
-            <height>1179</height>
+            <height>1101</height>
            </rect>
           </property>
           <layout class="QVBoxLayout" name="verticalLayout_16">
@@ -86,7 +86,7 @@
                 <enum>QFrame::Raised</enum>
                </property>
                <property name="currentIndex">
-                <number>3</number>
+                <number>21</number>
                </property>
                <widget class="QWidget" name="page_22">
                 <layout class="QVBoxLayout" name="verticalLayout_29">
@@ -4986,7 +4986,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 for odometry pose correction. ICP 2D only is used here.</string>
+                          <string>Odometry pose correction using laser scan matching. ICP 2D only is used here.</string>
                          </property>
                          <property name="wordWrap">
                           <bool>true</bool>
@@ -5029,125 +5029,6 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag
                       </layout>
                      </widget>
                     </item>
-                    <item>
-                     <widget class="QGroupBox" name="groupBox_globalConstraints">
-                      <property name="title">
-                       <string>Global loop closure constraints</string>
-                      </property>
-                      <layout class="QVBoxLayout" name="verticalLayout_20">
-                       <item>
-                        <widget class="QLabel" name="label_55">
-                         <property name="text">
-                          <string>When a global loop closure is detected by the Bayes filter, a transform is computed between the current location and the old one. If a transform cannot be computed, the loop closure is rejected. The computation of the transform is done in 2 steps: </string>
-                         </property>
-                         <property name="wordWrap">
-                          <bool>true</bool>
-                         </property>
-                        </widget>
-                       </item>
-                       <item>
-                        <layout class="QGridLayout" name="gridLayout_10" columnstretch="0,1">
-                         <item row="0" column="1">
-                          <widget class="QLabel" name="label_57">
-                           <property name="text">
-                            <string>A transformation is computed using the 3D visual word correspondences. See &quot;Loop closure constraint&quot; panel for parameters.</string>
-                           </property>
-                           <property name="wordWrap">
-                            <bool>true</bool>
-                           </property>
-                          </widget>
-                         </item>
-                         <item row="0" column="0">
-                          <widget class="QLabel" name="label_17">
-                           <property name="text">
-                            <string>1)</string>
-                           </property>
-                          </widget>
-                         </item>
-                         <item row="1" column="1">
-                          <widget class="QLabel" name="label_64">
-                           <property name="text">
-                            <string>Then if enabled, the visual transform is used as a guess for ICP estimation (3D or 2D). See &quot;ICP&quot; panel for parameters.</string>
-                           </property>
-                           <property name="wordWrap">
-                            <bool>true</bool>
-                           </property>
-                          </widget>
-                         </item>
-                         <item row="1" column="0">
-                          <widget class="QLabel" name="label_18">
-                           <property name="text">
-                            <string>2)</string>
-                           </property>
-                          </widget>
-                         </item>
-                        </layout>
-                       </item>
-                       <item>
-                        <layout class="QGridLayout" name="gridLayout_48" columnstretch="0,1">
-                         <item row="0" column="0">
-                          <widget class="QComboBox" name="globalDetection_icpType">
-                           <property name="sizeAdjustPolicy">
-                            <enum>QComboBox::AdjustToContents</enum>
-                           </property>
-                           <item>
-                            <property name="text">
-                             <string>No ICP</string>
-                            </property>
-                           </item>
-                           <item>
-                            <property name="text">
-                             <string>ICP 3D (e.g. Kinect depth)</string>
-                            </property>
-                           </item>
-                           <item>
-                            <property name="text">
-                             <string>ICP 2D (e.g. Laser scan)</string>
-                            </property>
-                           </item>
-                          </widget>
-                         </item>
-                         <item row="0" column="1">
-                          <widget class="QLabel" name="label_19">
-                           <property name="text">
-                            <string>Use ICP estimation. If ICP 2D is selected, laser scans are required.</string>
-                           </property>
-                          </widget>
-                         </item>
-                         <item row="1" column="0">
-                          <widget class="QDoubleSpinBox" name="globalDetection_icpMaxDistance">
-                           <property name="suffix">
-                            <string> m</string>
-                           </property>
-                           <property name="decimals">
-                            <number>2</number>
-                           </property>
-                           <property name="maximum">
-                            <double>1.000000000000000</double>
-                           </property>
-                           <property name="singleStep">
-                            <double>0.010000000000000</double>
-                           </property>
-                           <property name="value">
-                            <double>0.200000000000000</double>
-                           </property>
-                          </widget>
-                         </item>
-                         <item row="1" column="1">
-                          <widget class="QLabel" name="label_51">
-                           <property name="text">
-                            <string>Maximum ICP correction distance accepted. A large translation difference between the visual transformation and ICP transformation results in wrong transformations in most cases.</string>
-                           </property>
-                           <property name="wordWrap">
-                            <bool>true</bool>
-                           </property>
-                          </widget>
-                         </item>
-                        </layout>
-                       </item>
-                      </layout>
-                     </widget>
-                    </item>
                     <item>
                      <widget class="QGroupBox" name="groupBox_graphOptimization">
                       <property name="title">
@@ -5405,7 +5286,7 @@ Warning when set to false: when some nodes are transferred, the first referentia
                     <item>
                      <widget class="QLabel" name="label_16">
                       <property name="text">
-                       <string>Parameters to compute a transform from 3D visual word correspondences.</string>
+                       <string>When a global loop closure is detected by the Bayes filter, a transform is computed between the current location and the old one. If a transform cannot be computed, the loop closure is rejected. A transformation is computed using the 3D visual word correspondences.</string>
                       </property>
                       <property name="wordWrap">
                        <bool>true</bool>
@@ -5523,12 +5404,44 @@ Warning when set to false: when some nodes are transferred, the first referentia
                         </property>
                        </widget>
                       </item>
+                      <item row="5" column="0">
+                       <widget class="QComboBox" name="globalDetection_icpType">
+                        <property name="sizeAdjustPolicy">
+                         <enum>QComboBox::AdjustToContents</enum>
+                        </property>
+                        <item>
+                         <property name="text">
+                          <string>No ICP</string>
+                         </property>
+                        </item>
+                        <item>
+                         <property name="text">
+                          <string>ICP 3D (e.g. Kinect depth)</string>
+                         </property>
+                        </item>
+                        <item>
+                         <property name="text">
+                          <string>ICP 2D (e.g. Laser scan)</string>
+                         </property>
+                        </item>
+                       </widget>
+                      </item>
+                      <item row="5" column="1">
+                       <widget class="QLabel" name="label_57">
+                        <property name="text">
+                         <string>When enabled, the visual transform is used as a guess for ICP estimation (3D or 2D). See &quot;ICP&quot; panel for parameters.</string>
+                        </property>
+                        <property name="wordWrap">
+                         <bool>true</bool>
+                        </property>
+                       </widget>
+                      </item>
                      </layout>
                     </item>
                     <item>
                      <widget class="QGroupBox" name="groupBox_reextract">
                       <property name="title">
-                       <string>Re-extract features</string>
+                       <string>Re-extract features on global loop closure</string>
                       </property>
                       <property name="checkable">
                        <bool>true</bool>
@@ -5537,16 +5450,6 @@ Warning when set to false: when some nodes are transferred, the first referentia
                        <bool>true</bool>
                       </property>
                       <layout class="QVBoxLayout" name="verticalLayout_46">
-                       <item>
-                        <widget class="QLabel" name="label_171">
-                         <property name="text">
-                          <string>If not activated, when a loop closure constraint must be computed, the words already extracted for the loop closure detector are used. These words are limited (see Visual Word-&gt;Words Per Image) and matched to the loop closure detection vocabulary. When the vocabulary is large, there maybe less corresponding words on a loop closure.</string>
-                         </property>
-                         <property name="wordWrap">
-                          <bool>true</bool>
-                         </property>
-                        </widget>
-                       </item>
                        <item>
                         <widget class="QLabel" name="label_182">
                          <property name="text">
@@ -5558,9 +5461,9 @@ Warning when set to false: when some nodes are transferred, the first referentia
                         </widget>
                        </item>
                        <item>
-                        <widget class="QLabel" name="label_183">
+                        <widget class="QLabel" name="label_171">
                          <property name="text">
-                          <string>Note that this is used only for global loop closure, so not for local loop closure in time.</string>
+                          <string>If not activated, when a loop closure constraint must be computed, the words already extracted for the loop closure detector are used. These words are limited (see Visual Word-&gt;Words Per Image) and matched to the loop closure detection vocabulary. When the vocabulary is large, there maybe less corresponding words on a loop closure.</string>
                          </property>
                          <property name="wordWrap">
                           <bool>true</bool>
@@ -5749,7 +5652,7 @@ Lower the ratio -&gt; higher the precision. 0 means disabled, matching the neare
                    <property name="title">
                     <string>ICP</string>
                    </property>
-                   <layout class="QVBoxLayout" name="verticalLayout_51">
+                   <layout class="QVBoxLayout" name="verticalLayout_20">
                     <item>
                      <widget class="QLabel" name="label_7">
                       <property name="text">
@@ -5757,15 +5660,62 @@ Lower the ratio -&gt; higher the precision. 0 means disabled, matching the neare
                       </property>
                      </widget>
                     </item>
+                    <item>
+                     <layout class="QGridLayout" name="gridLayout_48" columnstretch="0,1">
+                      <item row="1" column="0">
+                       <widget class="QCheckBox" name="loopClosure_highICPTransitionalVariance">
+                        <property name="text">
+                         <string/>
+                        </property>
+                       </widget>
+                      </item>
+                      <item row="0" column="0">
+                       <widget class="QDoubleSpinBox" name="globalDetection_icpMaxDistance">
+                        <property name="suffix">
+                         <string> m</string>
+                        </property>
+                        <property name="decimals">
+                         <number>2</number>
+                        </property>
+                        <property name="maximum">
+                         <double>1.000000000000000</double>
+                        </property>
+                        <property name="singleStep">
+                         <double>0.010000000000000</double>
+                        </property>
+                        <property name="value">
+                         <double>0.200000000000000</double>
+                        </property>
+                       </widget>
+                      </item>
+                      <item row="0" column="1">
+                       <widget class="QLabel" name="label_51">
+                        <property name="text">
+                         <string>Maximum ICP correction distance accepted. A large translation difference between the visual transformation and ICP transformation results in wrong transformations in most cases.</string>
+                        </property>
+                        <property name="wordWrap">
+                         <bool>true</bool>
+                        </property>
+                       </widget>
+                      </item>
+                      <item row="1" column="1">
+                       <widget class="QLabel" name="label_80">
+                        <property name="text">
+                         <string>Set high transitional variance on odometry pose correction and on local loop closure in space. Particularly useful when the robot is moving in hallways with short-range laser rangefinder. Only used with ICP 2D.</string>
+                        </property>
+                        <property name="wordWrap">
+                         <bool>true</bool>
+                        </property>
+                       </widget>
+                      </item>
+                     </layout>
+                    </item>
                     <item>
                      <widget class="QGroupBox" name="groupBox_loopClosure_icp3">
                       <property name="title">
                        <string>ICP 3D - Point To Plane (e.g. Kinect depth)</string>
                       </property>
-                      <layout class="QFormLayout" name="formLayout_3">
+                      <layout class="QGridLayout" name="gridLayout_53" columnstretch="0,1">
-                       <property name="fieldGrowthPolicy">
-                        <enum>QFormLayout::AllNonFixedFieldsGrow</enum>
-                       </property>
                        <item row="0" column="0">
                         <widget class="QSpinBox" name="loopClosure_icpDecimation">
                          <property name="minimum">
@@ -5936,6 +5886,13 @@ Lower the ratio -&gt; higher the precision. 0 means disabled, matching the neare
                          </property>
                         </widget>
                        </item>
+                       <item row="7" column="0">
+                        <widget class="QCheckBox" name="loopClosure_icpPointToPlane">
+                         <property name="text">
+                          <string/>
+                         </property>
+                        </widget>
+                       </item>
                        <item row="7" column="1">
                         <widget class="QLabel" name="label_144">
                          <property name="text">
@@ -5946,13 +5903,6 @@ Lower the ratio -&gt; higher the precision. 0 means disabled, matching the neare
                          </property>
                         </widget>
                        </item>
-                       <item row="7" column="0">
-                        <widget class="QCheckBox" name="loopClosure_icpPointToPlane">
-                         <property name="text">
-                          <string/>
-                         </property>
-                        </widget>
-                       </item>
                        <item row="8" column="0">
                         <widget class="QSpinBox" name="loopClosure_icpPointToPlaneNormals">
                          <property name="minimum">
@@ -5984,10 +5934,7 @@ Lower the ratio -&gt; higher the precision. 0 means disabled, matching the neare
                       <property name="title">
                        <string>ICP 2D (e.g. Laser scan)</string>
                       </property>
-                      <layout class="QFormLayout" name="formLayout_4">
+                      <layout class="QGridLayout" name="gridLayout_52" columnstretch="0,1">
-                       <property name="fieldGrowthPolicy">
-                        <enum>QFormLayout::AllNonFixedFieldsGrow</enum>
-                       </property>
                        <item row="0" column="0">
                         <widget class="QDoubleSpinBox" name="loopClosure_icp2MaxCorrespondenceDistance">
                          <property name="minimum">

package.xml

@@ -1,7 +1,7 @@
 <?xml version="1.0"?>
 <package>
   <name>rtabmap</name>
-  <version>0.8.3</version>
+  <version>0.8.4</version>
   <description>RTAB-Map's standalone library. RTAB-Map is an RGB-D SLAM approach with real-time constraints.</description>
   <maintainer email="[email protected]">Mathieu Labbe</maintainer>
   <author>Mathieu Labbe</author>