From dbb29c76615fcba7aacc9e5a3baf3e07dae21a04 Mon Sep 17 00:00:00 2001 From: matlabbe Date: Sun, 24 Jul 2016 13:23:22 -0400 Subject: [PATCH] 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 | 8 ++++---- corelib/src/Link.cpp | 32 ----------------------------- corelib/src/Signature.cpp | 2 +- guilib/src/DatabaseViewer.cpp | 31 ++++++++++++++++++++++------ 4 files changed, 30 insertions(+), 43 deletions(-) diff --git a/corelib/include/rtabmap/core/Link.h b/corelib/include/rtabmap/core/Link.h index 6a6dbe83..b9645b4f 100644 --- a/corelib/include/rtabmap/core/Link.h +++ b/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_; diff --git a/corelib/src/Link.cpp b/corelib/src/Link.cpp index 5d3512be..59768115 100644 --- a/corelib/src/Link.cpp +++ b/corelib/src/Link.cpp @@ -120,38 +120,6 @@ void Link::setVariance(double rotVariance, double transVariance) { infMatrix_.at(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(); diff --git a/corelib/src/Signature.cpp b/corelib/src/Signature.cpp index 01a46251..65fda094 100644 --- a/corelib/src/Signature.cpp +++ b/corelib/src/Signature.cpp @@ -115,7 +115,7 @@ void Signature::addLinks(const std::map & 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::iterator, bool> pair = _links.insert(std::make_pair(link.to(), link)); diff --git a/guilib/src/DatabaseViewer.cpp b/guilib/src/DatabaseViewer.cpp index 7eceab8f..1db6af93 100644 --- a/guilib/src/DatabaseViewer.cpp +++ b/guilib/src/DatabaseViewer.cpp @@ -1446,7 +1446,11 @@ void DatabaseViewer::generateTOROGraph() std::multimap links = graphLinks_; for(std::multimap::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 links = graphLinks_; for(std::multimap::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(), robust); } @@ -2892,6 +2900,7 @@ void DatabaseViewer::updateConstraintView( { std::multimap::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 tmp = links; - for(std::multimap::iterator iter=tmp.begin(); iter!=tmp.end(); ++iter) + std::multimap::iterator jter=links.begin(); + for(std::multimap::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 - iter->second.setVariance(1.0f, 1.0f); // reset variance + // reset to identity covariance + iter->second = Link( + iter->second.from(), + iter->second.to(), + iter->second.type(), + poseFrom.inverse() * poseTo); } } }