databaseViewer: Fixed link's covariance modified when pose correction was disabled. Link: setVariance() and setInfMatrix are now private, to force re-cloning the link if we want to change these info.

corelib/include/rtabmap/core/Link.h

@@ -76,11 +76,7 @@ public:
 	void setTo(int to) {to_ = to;}
 	void setTransform(const Transform & transform) {transform_ = transform;}
 	void setType(Type type) {type_ = type;}
-	void setInfMatrix(const cv::Mat & infMatrix);
-	void setVariance(double rotVariance, double transVariance);
 
-	void setUserDataRaw(const cv::Mat & userDataRaw); // only set raw
-	void setUserData(const cv::Mat & userData); // detect automatically if raw or compressed. If raw, the data is compressed too.
 	const cv::Mat & userDataRaw() const {return _userDataRaw;}
 	const cv::Mat & userDataCompressed() const {return _userDataCompressed;}
 	void uncompressUserData();
@@ -89,6 +85,10 @@ public:
 	Link merge(const Link & link, Type outputType) const;
 	Link inverse() const;
 
+private:
+	void setInfMatrix(const cv::Mat & infMatrix);
+	void setVariance(double rotVariance, double transVariance);
+
 private:
 	int from_;
 	int to_;

corelib/src/Link.cpp

@@ -120,38 +120,6 @@ void Link::setVariance(double rotVariance, double transVariance) {
 	infMatrix_.at<double>(5,5) = 1.0/rotVariance;
 }
 
-void Link::setUserDataRaw(const cv::Mat & userDataRaw)
-{
-	if(!_userDataRaw.empty())
-	{
-		UWARN("Writing new user data over existing user data. This may result in data loss.");
-	}
-	_userDataRaw = userDataRaw;
-}
-
-void Link::setUserData(const cv::Mat & userData)
-{
-	if(!userData.empty() && (!_userDataCompressed.empty() || !_userDataRaw.empty()))
-	{
-		UWARN("Writing new user data over existing user data. This may result in data loss.");
-	}
-	_userDataRaw = cv::Mat();
-	_userDataCompressed = cv::Mat();
-
-	if(!userData.empty())
-	{
-		if(userData.type() == CV_8UC1) // Bytes
-		{
-			_userDataCompressed = userData; // assume compressed
-		}
-		else
-		{
-			_userDataRaw = userData;
-			_userDataCompressed = compressData2(userData);
-		}
-	}
-}
-
 void Link::uncompressUserData()
 {
 	cv::Mat dataRaw = uncompressUserDataConst();

corelib/src/Signature.cpp

@@ -115,7 +115,7 @@ void Signature::addLinks(const std::map<int, Link> & links)
 }
 void Signature::addLink(const Link & link)
 {
-	UDEBUG("Add link %d to %d (type=%d)", link.to(), this->id(), (int)link.type());
+	UDEBUG("Add link %d to %d (type=%d var=%f,%f)", link.to(), this->id(), (int)link.type(), link.transVariance(), link.rotVariance());
 	UASSERT_MSG(link.from() == this->id(), uFormat("%d->%d for signature %d (type=%d)", link.from(), link.to(), this->id(), link.type()).c_str());
 	UASSERT_MSG(link.to() != this->id(), uFormat("%d->%d for signature %d (type=%d)", link.from(), link.to(), this->id(), link.type()).c_str());
 	std::pair<std::map<int, Link>::iterator, bool> pair = _links.insert(std::make_pair(link.to(), link));

guilib/src/DatabaseViewer.cpp

@@ -1446,7 +1446,11 @@ void DatabaseViewer::generateTOROGraph()
 				std::multimap<int, rtabmap::Link> links = graphLinks_;
 				for(std::multimap<int, rtabmap::Link>::iterator iter=links.begin(); iter!=links.end(); ++iter)
 				{
-					iter->second.setInfMatrix(cv::Mat::eye(6,6,CV_64FC1));
+					// reset to identity covariance
+					iter->second = Link(iter->second.from(),
+							iter->second.to(),
+							iter->second.type(),
+							iter->second.transform());
 				}
 				graph::exportPoses(path.toStdString(), 3, uValueAt(graphes_, id), links);
 			}
@@ -1490,7 +1494,11 @@ void DatabaseViewer::generateG2OGraph()
 				std::multimap<int, rtabmap::Link> links = graphLinks_;
 				for(std::multimap<int, rtabmap::Link>::iterator iter=links.begin(); iter!=links.end(); ++iter)
 				{
-					iter->second.setInfMatrix(cv::Mat::eye(6,6,CV_64FC1));
+					// reset to identity covariance
+					iter->second = Link(iter->second.from(),
+							iter->second.to(),
+							iter->second.type(),
+							iter->second.transform());
 				}
 				graph::exportPoses(path.toStdString(), 4, uValueAt(graphes_, id), links, std::map<int, double>(), robust);
 			}
@@ -2892,6 +2900,7 @@ void DatabaseViewer::updateConstraintView(
 {
 	std::multimap<int, Link>::iterator iterLink = rtabmap::graph::findLink(linksRefined_, linkIn.from(), linkIn.to());
 	rtabmap::Link link = linkIn;
+
 	if(iterLink != linksRefined_.end())
 	{
 		link = iterLink->second;
@@ -2905,7 +2914,12 @@ void DatabaseViewer::updateConstraintView(
 			Transform poseTo = uValue(poses_, link.to(), Transform());
 			if(!poseFrom.isNull() && !poseTo.isNull())
 			{
-				link.setTransform(poseFrom.inverse() * poseTo); // recompute raw odom transformation
+				// recompute raw odom transformation and
+				// reset to identity covariance
+				link = Link(link.from(),
+						link.to(),
+						link.type(),
+						poseFrom.inverse() * poseTo);
 			}
 		}
 	}
@@ -3612,7 +3626,8 @@ void DatabaseViewer::updateGraphView()
 		if(ui_->checkBox_ignorePoseCorrection->isChecked())
 		{
 			std::multimap<int, Link> tmp = links;
-			for(std::multimap<int, Link>::iterator iter=tmp.begin(); iter!=tmp.end(); ++iter)
+			std::multimap<int, Link>::iterator jter=links.begin();
+			for(std::multimap<int, Link>::iterator iter=tmp.begin(); iter!=tmp.end(); ++iter, ++jter)
 			{
 				if(iter->second.type() == Link::kNeighbor ||
 				   iter->second.type() == Link::kNeighborMerged)
@@ -3621,8 +3636,12 @@ void DatabaseViewer::updateGraphView()
 					Transform poseTo = uValue(poses, iter->second.to(), Transform());
 					if(!poseFrom.isNull() && !poseTo.isNull())
 					{
-						iter->second.setTransform(poseFrom.inverse() * poseTo); // recompute raw odom transformation
+						// reset to identity covariance
-						iter->second.setVariance(1.0f, 1.0f); // reset variance
+						iter->second = Link(
+								iter->second.from(),
+								iter->second.to(),
+								iter->second.type(),
+								poseFrom.inverse() * poseTo);
 					}
 				}
 			}