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 calibration not found on refining without re-extracting features, fixed visualized ids switched on refine in some cases, fixed clouds not shown in constraints view after refine without re-extracting features

Browse Source
This commit is contained in:
matlabbe
2020-04-30 14:17:46 -04:00
parent ace93b0356
commit 5d16ae8cfe
5 changed files with 138 additions and 47 deletions
Download Patch File Download Diff File Expand all files Collapse all files

136
guilib/src/DatabaseViewer.cpp
View File

@@ -5303,12 +5303,20 @@ void DatabaseViewer::updateConstraintView(
const rtabmap::Link & linkIn,
bool updateImageSliders,
const Signature & signatureFrom,
const Signature & signatureTo)
{
UDEBUG("%d -> %d", linkIn.from(), linkIn.to());
std::multimap<int, Link>::iterator iterLink = rtabmap::graph::findLink(linksRefined_, linkIn.from(), linkIn.to());
rtabmap::Link link = linkIn;
if(iterLink != linksRefined_.end())
if(iterLink != linksRefined_.end())
{
if(iterLink->second.from() == link.to())
{
link = iterLink->second.inverse();
}
else
{
link = iterLink->second;
}
}
@@ -5328,6 +5336,7 @@ void DatabaseViewer::updateConstraintView(
link.type(),
poseFrom.inverse() * poseTo);
}
}
}
UDEBUG("%d -> %d", link.from(), link.to());
rtabmap::Transform t = link.transform();
@@ -6751,6 +6760,7 @@ void DatabaseViewer::refineConstraint()
refineConstraint(from, to, false);
}
void DatabaseViewer::refineConstraint(int from, int to, bool silent)
{
UDEBUG("%d -> %d", from, to);
bool switchedIds = false;
@@ -6802,11 +6812,15 @@ void DatabaseViewer::refineConstraint(int from, int to, bool silent)
}
}
Transform transform;
RegistrationInfo info;
Transform transform;
RegistrationInfo info;
Signature * fromS = 0;
Signature toS;
Signature * toS = 0;
fromS = dbDriver_->loadSignature(currentLink.from());
if(fromS == 0)
{
UERROR("Signature %d not found!", currentLink.from());
return;
}
@@ -6895,7 +6909,8 @@ void DatabaseViewer::refineConstraint(int from, int to, bool silent)
filteredScanPoses.insert(*scanPoses.find(currentLink.to()));
}
Transform toPoseInv = filteredScanPoses.at(currentLink.to()).inverse();
Transform toPoseInv = filteredScanPoses.at(currentLink.to()).inverse();
LaserScan fromScan;
dbDriver_->loadNodeData(fromS, !silent, true, !silent, !silent);
fromS->sensorData().uncompressData();
int maxPoints = fromScan.size();
@@ -6980,7 +6995,7 @@ void DatabaseViewer::refineConstraint(int from, int to, bool silent)
fromScan.rangeMax(),
fromScan.format(),
fromScan.is2d()?Transform(0,0,fromScan.localTransform().z(),0,0,0):Transform::getIdentity()));
RegistrationIcp registrationIcp(parameters);
transform = registrationIcp.computeTransformation(fromS->sensorData(), assembledData, currentLink.transform(), &info);
if(!transform.isNull())
@@ -6989,36 +7004,70 @@ void DatabaseViewer::refineConstraint(int from, int to, bool silent)
info.covariance*=100.0;
}
}
else
{
SensorData dataTo;
dbDriver_->getNodeData(currentLink.to(), dataTo);
else
{
toS = dbDriver_->loadSignature(currentLink.to());
if(toS == 0)
{
UERROR("Signature %d not found!", currentLink.to());
delete fromS;
return;
}
bool reextractVisualFeatures = uStr2Bool(parameters.at(Parameters::kRGBDLoopClosureReextractFeatures()));
Registration * reg = Registration::create(parameters);
if( reg->isScanRequired() ||
reg->isUserDataRequired() ||
reextractVisualFeatures ||
!silent)
{
dbDriver_->loadNodeData(fromS, reextractVisualFeatures || !silent, reg->isScanRequired() || !silent, reg->isUserDataRequired() || !silent, !silent);
dbDriver_->loadNodeData(toS, reextractVisualFeatures || !silent, reg->isScanRequired() || !silent, reg->isUserDataRequired() || !silent, !silent);
if(!silent)
{
fromS->sensorData().uncompressData();
toS->sensorData().uncompressData();
}
}
if(reextractVisualFeatures)
{
fromS->setWords(std::multimap<int, cv::KeyPoint>());
fromS->setWords3(std::multimap<int, cv::Point3f>());
fromS->setWordsDescriptors(std::multimap<int, cv::Mat>());
fromS->sensorData().setFeatures(std::vector<cv::KeyPoint>(), std::vector<cv::Point3f>(), cv::Mat());
toS->setWords(std::multimap<int, cv::KeyPoint>());
toS->setWords3(std::multimap<int, cv::Point3f>());
toS->setWordsDescriptors(std::multimap<int, cv::Mat>());
toS->sensorData().setFeatures(std::vector<cv::KeyPoint>(), std::vector<cv::Point3f>(), cv::Mat());
}
if(reg->isScanRequired())
{
if(ui_->checkBox_icp_from_depth->isChecked())
{
// generate laser scans from depth image
cv::Mat tmpA, tmpB, tmpC, tmpD;
// generate laser scans from depth image
cv::Mat tmpA, tmpB, tmpC, tmpD;
fromS->sensorData().uncompressData(&tmpA, &tmpB, 0);
dataTo.uncompressData(&tmpC, &tmpD, 0);
toS->sensorData().uncompressData(&tmpC, &tmpD, 0);
pcl::PointCloud<pcl::PointXYZ>::Ptr cloudFrom = util3d::cloudFromSensorData(
fromS->sensorData(),
ui_->spinBox_icp_decimation->value()==0?1:ui_->spinBox_icp_decimation->value(),
ui_->doubleSpinBox_icp_maxDepth->value(),
ui_->doubleSpinBox_icp_minDepth->value(),
0,
ui_->parameters_toolbox->getParameters());
ui_->parameters_toolbox->getParameters());
pcl::PointCloud<pcl::PointXYZ>::Ptr cloudTo = util3d::cloudFromSensorData(
toS->sensorData(),
ui_->spinBox_icp_decimation->value()==0?1:ui_->spinBox_icp_decimation->value(),
ui_->doubleSpinBox_icp_maxDepth->value(),
ui_->doubleSpinBox_icp_minDepth->value(),
0,
ui_->parameters_toolbox->getParameters());
int maxLaserScans = cloudFrom->size();
ui_->parameters_toolbox->getParameters());
int maxLaserScans = cloudFrom->size();
fromS->sensorData().setLaserScan(LaserScan(util3d::laserScanFromPointCloud(*util3d::removeNaNFromPointCloud(cloudFrom), Transform()), maxLaserScans, 0, LaserScan::kXYZ));
dataTo.setLaserScan(LaserScan(util3d::laserScanFromPointCloud(*util3d::removeNaNFromPointCloud(cloudTo), Transform()), maxLaserScans, 0, LaserScan::kXYZ));
toS->sensorData().setLaserScan(LaserScan(util3d::laserScanFromPointCloud(*util3d::removeNaNFromPointCloud(cloudTo), Transform()), maxLaserScans, 0, LaserScan::kXYZ));
if(!fromS->sensorData().laserScanCompressed().isEmpty() || !toS->sensorData().laserScanCompressed().isEmpty())
{
@@ -7027,34 +7076,31 @@ void DatabaseViewer::refineConstraint(int from, int to, bool silent)
}
}
else
{
LaserScan tmpA, tmpB;
{
LaserScan tmpA, tmpB;
fromS->sensorData().uncompressData(0, 0, &tmpA);
toS->sensorData().uncompressData(0, 0, &tmpB);
}
}
}
if(reg->isImageRequired() && reextractVisualFeatures)
{
cv::Mat tmpA, tmpB, tmpC, tmpD;
{
cv::Mat tmpA, tmpB, tmpC, tmpD;
fromS->sensorData().uncompressData(&tmpA, &tmpB, 0);
toS->sensorData().uncompressData(&tmpC, &tmpD, 0);
}
UINFO("Uncompress time: %f s", timer.ticks());
fromS = Signature(dataFrom);
toS = Signature(dataTo);
UINFO("Uncompress time: %f s", timer.ticks());
if(fromS.id() < toS.id())
if(fromS->id() < toS->id())
{
transform = reg->computeTransformationMod(*fromS, *toS, t, &info);
}
else
else
{
transform = reg->computeTransformationMod(*toS, *fromS, t.isNull()?t:t.inverse(), &info);
switchedIds = true;
}
}
delete reg;
}
@@ -7100,11 +7146,21 @@ void DatabaseViewer::refineConstraint(int from, int to, bool silent)
this->updateGraphView();
}
if(!silent && ui_->dockWidget_constraints->isVisible())
if(!silent && ui_->dockWidget_constraints->isVisible())
{
if(fromS.id() > 0 && toS.id() > 0)
{
this->updateConstraintView(newLink, true, fromS, toS);
if(fromS->id() > 0 && toS->id() > 0)
{
updateLoopClosuresSlider(fromS->id(), toS->id());
if(newLink.type() != Link::kNeighbor && fromS->id() < toS->id())
{
this->updateConstraintView(newLink.inverse(), true, *toS, *fromS);
ui_->graphicsView_A->setFeatures(toS->getWords(), toS->sensorData().depthRaw());
ui_->graphicsView_B->setFeatures(fromS->getWords(), fromS->sensorData().depthRaw());
}
else
{
this->updateConstraintView(newLink, true, *fromS, *toS);
ui_->graphicsView_A->setFeatures(fromS->getWords(), fromS->sensorData().depthRaw());
ui_->graphicsView_B->setFeatures(toS->getWords(), toS->sensorData().depthRaw());
}
@@ -7121,6 +7177,8 @@ void DatabaseViewer::refineConstraint(int from, int to, bool silent)
{
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()));
}
delete fromS;
delete toS;
}
@@ -7179,8 +7237,8 @@ bool DatabaseViewer::addConstraint(int from, int to, bool silent)
if(signatures.size() != 2)
{
for(std::list<Signature*>::iterator iter=signatures.begin(); iter!=signatures.end(); ++iter)
{
{
delete *iter;
}
return false;
}
@@ -7188,12 +7246,13 @@ bool DatabaseViewer::addConstraint(int from, int to, bool silent)
toS = *signatures.rbegin();
bool reextractVisualFeatures = uStr2Bool(parameters.at(Parameters::kRGBDLoopClosureReextractFeatures()));
if(reg->isScanRequired() ||
if(reg->isScanRequired() ||
reg->isUserDataRequired() ||
reextractVisualFeatures ||
!silent)
{
{
// Add sensor data to generate features
dbDriver_->getNodeData(from, fromS->sensorData(), reextractVisualFeatures, reg->isScanRequired(), reg->isUserDataRequired(), false);
dbDriver_->loadNodeData(fromS, reextractVisualFeatures || !silent, reg->isScanRequired() || !silent, reg->isUserDataRequired() || !silent, !silent);
fromS->sensorData().uncompressData();
dbDriver_->loadNodeData(toS, reextractVisualFeatures || !silent, reg->isScanRequired() || !silent, reg->isUserDataRequired() || !silent, !silent);
toS->sensorData().uncompressData();
@@ -7622,6 +7681,7 @@ std::multimap<int, rtabmap::Link> DatabaseViewer::updateLinksWithModifications(
return links;
}
void DatabaseViewer::updateLoopClosuresSlider(int from, int to)
{
UDEBUG("%d %d", from, to);
int size = loopLinks_.size();