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 matching lines when two images don't have the same number of cameras. Fixed nodes with w=-9 can be used to add constraint (in case localization data is saved in database). Add constraint: compute transform in order of the sliders.

Browse Source
This commit is contained in:
matlabbe
2022-10-04 14:23:12 -07:00
parent 8d23f19f6c
commit 7e3e5ec50b
1 changed files with 58 additions and 51 deletions
Download Patch File Download Diff File Expand all files Collapse all files

109
guilib/src/DatabaseViewer.cpp
View File

@@ -1830,7 +1830,6 @@ void DatabaseViewer::updateIds()
previousPose=p;
//links
bool addPose = links.find(ids_[i]) == links.end();
for(std::multimap<int, Link>::iterator jter=links.find(ids_[i]); jter!=links.end() && jter->first == ids_[i]; ++jter)
{
if(jter->second.type() == Link::kNeighborMerged)
@@ -1856,34 +1855,27 @@ void DatabaseViewer::updateIds()
{
links_.insert(std::make_pair(ids_[i], jter->second));
}
addPose = true;
}
else if(graph::findLink(links_, jter->second.from(), jter->second.to()) != links_.end())
{
addPose = true;
}
}
if(addPose)
// Add pose
odomPoses_.insert(std::make_pair(ids_[i], p));
if(!g.isNull())
{
odomPoses_.insert(std::make_pair(ids_[i], p));
if(!g.isNull())
{
groundTruthPoses_.insert(std::make_pair(ids_[i], g));
}
if(gps.stamp() > 0.0)
{
gpsValues_.insert(std::make_pair(ids_[i], gps));
groundTruthPoses_.insert(std::make_pair(ids_[i], g));
}
if(gps.stamp() > 0.0)
{
gpsValues_.insert(std::make_pair(ids_[i], gps));
cv::Point3f p(0.0f,0.0f,0.0f);
if(!gpsPoses_.empty())
{
GeodeticCoords coords = gps.toGeodeticCoords();
GPS originGPS = gpsValues_.begin()->second;
p = coords.toENU_WGS84(originGPS.toGeodeticCoords());
}
Transform pose(p.x, p.y, p.z, 0.0f, 0.0f, (float)((-(gps.bearing()-90))*M_PI/180.0));
gpsPoses_.insert(std::make_pair(ids_[i], pose));
cv::Point3f p(0.0f,0.0f,0.0f);
if(!gpsPoses_.empty())
{
GeodeticCoords coords = gps.toGeodeticCoords();
GPS originGPS = gpsValues_.begin()->second;
p = coords.toENU_WGS84(originGPS.toGeodeticCoords());
}
Transform pose(p.x, p.y, p.z, 0.0f, 0.0f, (float)((-(gps.bearing()-90))*M_PI/180.0));
gpsPoses_.insert(std::make_pair(ids_[i], pose));
}
}
@@ -4553,9 +4545,9 @@ void DatabaseViewer::update(int value,
float x,y,z,roll,pitch,yaw;
odomPose.getTranslationAndEulerAngles(x,y,z,roll, pitch,yaw);
labelPose->setText(QString("%1xyz=(%2,%3,%4)\nrpy=(%5,%6,%7)").arg(odomPose.isIdentity()?"* ":"").arg(x).arg(y).arg(z).arg(roll).arg(pitch).arg(yaw));
if(odomPoses_.size() && odomPoses_.find(id) == odomPoses_.end())
if(graphes_.size() && graphes_.back().find(id) == graphes_.back().end())
{
labelPose->setText(labelPose->text() + "\n<Not in graph>");
labelPose->setText(labelPose->text() + "\n<Not in optimized graph>");
}
if(s!=0.0)
{
@@ -5609,28 +5601,21 @@ void DatabaseViewer::updateWordsMatching(const std::vector<int> & inliers)
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;
float marginAX = (ui_->graphicsView_A->width() - ui_->graphicsView_A->sceneRect().width()*scaleAX)/2.0f;
float marginAY = (ui_->graphicsView_A->height() - ui_->graphicsView_A->sceneRect().height()*scaleAX)/2.0f;
float marginBX = (ui_->graphicsView_B->width() - ui_->graphicsView_B->sceneRect().width()*scaleBX)/2.0f;
float marginBY = (ui_->graphicsView_B->height() - ui_->graphicsView_B->sceneRect().height()*scaleBX)/2.0f;
float deltaX = 0;
float deltaY = 0;
if(ui_->actionVertical_Layout->isChecked())
{
deltaAY = ui_->graphicsView_A->height()/scaleAX;
deltaY = ui_->graphicsView_A->height();
}
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;
deltaX = ui_->graphicsView_A->width();
}
const KeypointItem * kptA = wordsA.value(ids[i]);
@@ -5652,17 +5637,17 @@ void DatabaseViewer::updateWordsMatching(const std::vector<int> & inliers)
}
ui_->graphicsView_A->addLine(
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,
kptA->keypoint().pt.x,
kptA->keypoint().pt.y,
(kptB->keypoint().pt.x*scaleBX+marginBX+deltaX-marginAX)/scaleAX,
(kptB->keypoint().pt.y*scaleBX+marginBY+deltaY-marginAY)/scaleAX,
cA);
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,
kptB->rect().y()+kptB->rect().height()/2,
(kptA->keypoint().pt.x*scaleAX+marginAX-deltaX-marginBX)/scaleBX,
(kptA->keypoint().pt.y*scaleAX+marginAY-deltaY-marginBY)/scaleBX,
kptB->keypoint().pt.x,
kptB->keypoint().pt.y,
cB);
}
}
@@ -6526,6 +6511,17 @@ void DatabaseViewer::updateConstraintButtons()
ui_->pushButton_add->setEnabled(true);
}
}
else if(ui_->checkBox_enableForAll->isChecked())
{
if(odomPoses_.find(from) == odomPoses_.end())
{
UWARN("Button \"Add\" cannot be enabled even if \"all\" checkbox is checked, as node %d doesn't have odometry set.", from);
}
else if(odomPoses_.find(to) == odomPoses_.end())
{
UWARN("Button \"Add\" cannot be enabled even if \"all\" checkbox is checked, as node %d doesn't have odometry set.", to);
}
}
currentLink = findActiveLink(from ,to);
}
@@ -8237,7 +8233,18 @@ bool DatabaseViewer::addConstraint(int from, int to, bool silent)
}
}
t = reg->computeTransformationMod(*fromS, *toS, guess, &info);
if(switchedIds)
{
t = reg->computeTransformationMod(*toS, *fromS, guess.isNull()?guess:guess.inverse(), &info);
if(!t.isNull())
{
t = t.inverse();
}
}
else
{
t = reg->computeTransformationMod(*fromS, *toS, guess, &info);
}
delete reg;
UDEBUG("");