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

DbViewer: fixed refineConstraints to make sure from id is the smallest id. Fixed correspondence lines drawn when images don't have the same size.

Browse Source
This commit is contained in:
matlabbe
2020-04-15 14:54:36 -04:00
parent de354b901d
commit 2ff582f06f
1 changed files with 84 additions and 40 deletions
Download Patch File Download Diff File Expand all files Collapse all files

124
guilib/src/DatabaseViewer.cpp
View File

@@ -5106,31 +5106,46 @@ void DatabaseViewer::updateWordsMatching()
ui_->graphicsView_A->setFeatureColor(ids[i], ui_->graphicsView_A->getDefaultMatchingFeatureColor());
ui_->graphicsView_B->setFeatureColor(ids[i], ui_->graphicsView_B->getDefaultMatchingFeatureColor());
// Add lines
// Draw lines between corresponding features...
float scaleX = ui_->graphicsView_A->viewScale();
// Add lines
// Draw lines between corresponding features...
float scaleAX = ui_->graphicsView_A->viewScale();
float scaleBX = ui_->graphicsView_B->viewScale();
float scaleDiff = ui_->graphicsView_A->viewScale() / ui_->graphicsView_B->viewScale();
float deltaAX = 0;
float deltaAY = 0;
if(ui_->actionVertical_Layout->isChecked())
if(ui_->actionVertical_Layout->isChecked())
{
deltaAY = ui_->graphicsView_A->height()/scaleAX;
}
else
else
{
deltaAX = ui_->graphicsView_A->width()/scaleAX;
}
float deltaBX = 0;
float deltaBY = 0;
if(ui_->actionVertical_Layout->isChecked())
{
deltaBY = ui_->graphicsView_B->height()/scaleBX;
}
else
{
deltaBX = ui_->graphicsView_A->width()/scaleBX;
}
const KeypointItem * kptA = wordsA.value(ids[i]);
const KeypointItem * kptB = wordsB.value(ids[i]);
ui_->graphicsView_A->addLine(
kptA->rect().x()+kptA->rect().width()/2,
kptA->rect().y()+kptA->rect().height()/2,
kptA->rect().x()+kptA->rect().width()/2,
kptA->rect().y()+kptA->rect().height()/2,
kptB->rect().x()/scaleDiff+kptB->rect().width()/scaleDiff/2+deltaAX,
kptB->rect().y()/scaleDiff+kptB->rect().height()/scaleDiff/2+deltaAY,
ui_->graphicsView_A->getDefaultMatchingLineColor());
ui_->graphicsView_B->addLine(
ui_->graphicsView_B->addLine(
kptA->rect().x()*scaleDiff+kptA->rect().width()*scaleDiff/2-deltaBX,
kptA->rect().y()*scaleDiff+kptA->rect().height()*scaleDiff/2-deltaBY,
kptB->rect().x()+kptB->rect().width()/2,
@@ -6720,19 +6735,22 @@ void DatabaseViewer::refineConstraint()
refineConstraint(from, to, false);
}
void DatabaseViewer::refineConstraint(int from, int to, bool silent)
{
bool switchedIds = false;
if(from == to)
{
UWARN("Cannot refine link to same node");
return;
}
Link currentLink = findActiveLink(from, to);
if(!currentLink.isValid())
{
UERROR("Not found link! (%d->%d)", from, to);
return;
return;
}
UDEBUG("%d -> %d (type=%d)", currentLink.from(), currentLink.to(), currentLink.type());
Transform t = currentLink.transform();
@@ -6752,7 +6770,7 @@ void DatabaseViewer::refineConstraint(int from, int to, bool silent)
t = topt;
}
}
}
}
else if(ui_->checkBox_ignorePoseCorrection->isChecked() &&
graph::findLink(linksRefined_, currentLink.from(), currentLink.to()) == linksRefined_.end())
{
@@ -7009,10 +7027,20 @@ void DatabaseViewer::refineConstraint(int from, int to, bool silent)
UINFO("Uncompress time: %f s", timer.ticks());
fromS = Signature(dataFrom);
fromS = Signature(dataFrom);
toS = Signature(dataTo);
if(fromS.id() < toS.id())
{
transform = registration->computeTransformationMod(fromS, toS, t, &info);
}
else
{
transform = registration->computeTransformationMod(toS, fromS, t.isNull()?t:t.inverse(), &info);
switchedIds = true;
}
delete registration;
delete registration;
}
UINFO("(%d ->%d) Registration time: %f s", currentLink.from(), currentLink.to(), timer.ticks());
@@ -7023,6 +7051,11 @@ void DatabaseViewer::refineConstraint(int from, int to, bool silent)
if(info.covariance.at<double>(0,0)<=0.0)
{
info.covariance = cv::Mat::eye(6,6,CV_64FC1)*0.0001; // epsilon if exact transform
}
}
if(switchedIds)
{
transform = transform.inverse();
}
Link newLink(currentLink.from(), currentLink.to(), currentLink.type(), transform, info.covariance.inv(), currentLink.userDataCompressed());
@@ -7054,9 +7087,9 @@ void DatabaseViewer::refineConstraint(int from, int to, bool silent)
if(!silent && ui_->dockWidget_constraints->isVisible())
{
if(fromS.id() > 0 && toS.id() > 0)
{
{
this->updateConstraintView(newLink, true, fromS, toS);
ui_->graphicsView_A->setFeatures(fromS.getWords(), fromS.sensorData().depthRaw());
ui_->graphicsView_B->setFeatures(toS.getWords(), toS.sensorData().depthRaw());
updateWordsMatching();
@@ -7070,7 +7103,7 @@ void DatabaseViewer::refineConstraint(int from, int to, bool silent)
else if(!silent)
{
QMessageBox::warning(this,
QMessageBox::warning(this,
tr("Refine link"),
tr("Cannot find a transformation between nodes %1 and %2: %3").arg(currentLink.from()).arg(currentLink.to()).arg(info.rejectedMsg.c_str()));
}
@@ -7091,11 +7124,14 @@ bool DatabaseViewer::addConstraint(int from, int to, bool silent)
UWARN("Cannot add link to same node");
return false;
}
else if(from > to)
else if(from > to)
{
int tmp = from;
from = to;
to = tmp;
switchedIds = true;
}
std::list<Signature*> signatures;
Signature * fromS=0;
Signature * toS=0;
@@ -7121,7 +7157,6 @@ bool DatabaseViewer::addConstraint(int from, int to, bool silent)
RegistrationInfo info;
std::list<int> ids;
ids.push_back(from);
ids.push_back(from);
ids.push_back(to);
dbDriver_->loadSignatures(ids, signatures);
@@ -7131,8 +7166,8 @@ bool DatabaseViewer::addConstraint(int from, int to, bool silent)
{
delete *iter;
return false;
}
}
}
}
fromS = *signatures.begin();
toS = *signatures.rbegin();
@@ -7227,21 +7262,6 @@ bool DatabaseViewer::addConstraint(int from, int to, bool silent)
t = reg->computeTransformationMod(*fromS, *toS, guess, &info);
delete reg;
UDEBUG("");
if(!silent)
{
if(switchedIds)
{
ui_->graphicsView_A->setFeatures(toS->getWords(), toS->sensorData().depthRaw());
ui_->graphicsView_B->setFeatures(fromS->getWords(), fromS->sensorData().depthRaw());
}
else
{
ui_->graphicsView_A->setFeatures(fromS->getWords(), fromS->sensorData().depthRaw());
ui_->graphicsView_B->setFeatures(toS->getWords(), toS->sensorData().depthRaw());
}
updateWordsMatching();
UDEBUG("");
if(!t.isNull())
@@ -7264,11 +7284,6 @@ bool DatabaseViewer::addConstraint(int from, int to, bool silent)
{
QMessageBox::warning(this,
tr("Add link"),
tr("Cannot find a transformation between nodes %1 and %2: %3").arg(from).arg(to).arg(info.rejectedMsg.c_str()));
}
for(std::list<Signature*>::iterator iter=signatures.begin(); iter!=signatures.end(); ++iter)
{
tr("Cannot find a transformation between nodes %1 and %2: %3").arg(from).arg(to).arg(info.rejectedMsg.c_str()));
}
}
@@ -7405,13 +7420,42 @@ bool DatabaseViewer::addConstraint(int from, int to, bool silent)
{
newLink = newLink.inverse();
}
linksAdded_.insert(std::make_pair(newLink.from(), newLink));
linksAdded_.insert(std::make_pair(newLink.from(), newLink));
}
}
if(!silent)
{
if(fromS && toS)
{
if((updateConstraints && newLink.from() > newLink.to()) || (!updateConstraints && switchedIds))
{
Signature * tmpS = fromS;
fromS = toS;
toS = tmpS;
}
if(updateConstraints)
{
updateLoopClosuresSlider(fromS->id(), toS->id());
this->updateGraphView();
this->updateConstraintView(newLink, false, *fromS, *toS);
}
ui_->graphicsView_A->setFeatures(fromS->getWords(), fromS->sensorData().depthRaw());
ui_->graphicsView_B->setFeatures(toS->getWords(), toS->sensorData().depthRaw());
updateWordsMatching();
}
else if(updateConstraints)
{
updateLoopClosuresSlider(from, to);
this->updateGraphView();
}
}
for(std::list<Signature*>::iterator iter=signatures.begin(); iter!=signatures.end(); ++iter)
{
delete *iter;
}
return updateConstraints;