i/rtabmap
1
0
Fork 0
mirror of https://github.com/introlab/rtabmap.git synced 2026-09-01 17:10:26 +08:00
Code Issues Packages Projects Releases Wiki Activity

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.

Browse Source
This commit is contained in:
matlabbe
2016-07-24 13:23:22 -04:00
parent 1a1cf5f672
commit dbb29c7661
4 changed files with 30 additions and 43 deletions
Download Patch File Download Diff File Expand all files Collapse all files

8
corelib/include/rtabmap/core/Link.h
View File

@@ -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_;

32
corelib/src/Link.cpp
View File

@@ -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();

2
corelib/src/Signature.cpp
View File

@@ -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));

31
guilib/src/DatabaseViewer.cpp
View File

@@ -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
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);
}
}
}