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

GUI: Added "Edit->Post processing..." action to detect more loop closures and/or refine all links in the graph with ICP 3D.

Browse Source 
git-svn-id: http://rtabmap.googlecode.com/svn/trunk/rtabmap@1940 f169173b-cf89-36c8-b27e-44dbe73f0c83
This commit is contained in:
matlabbe
2014-10-30 14:43:43 +00:00
parent 4fbefc9ec5
commit 1597cfc293
20 changed files with 1328 additions and 446 deletions
Download Patch File Download Diff File Expand all files Collapse all files

3
guilib/src/CMakeLists.txt
View File

@@ -19,6 +19,7 @@ SET(headers_ui
../include/${PROJECT_PREFIX}/gui/DataRecorder.h
../include/${PROJECT_PREFIX}/gui/CalibrationDialog.h
./ExportDialog.h
./PostProcessingDialog.h
./ExportCloudsDialog.h
./MapVisibilityWidget.h
)
@@ -31,6 +32,7 @@ SET(uis
./ui/DatabaseViewer.ui
./ui/loopClosureViewer.ui
./ui/exportDialog.ui
./ui/postProcessingDialog.ui
./ui/exportCloudsDialog.ui
./ui/calibrationDialog.ui
)
@@ -70,6 +72,7 @@ SET(SRC_FILES
./DataRecorder.cpp
./CalibrationDialog.cpp
./ExportDialog.cpp
./PostProcessingDialog.cpp
./ExportCloudsDialog.cpp
./MapVisibilityWidget.cpp
./GraphViewer.cpp

471
guilib/src/DatabaseViewer.cpp
View File

@@ -227,7 +227,7 @@ void DatabaseViewer::closeEvent(QCloseEvent* event)
// Added links
for(std::multimap<int, rtabmap::Link>::iterator iter=linksAdded_.begin(); iter!=linksAdded_.end(); ++iter)
{
std::multimap<int, rtabmap::Link>::iterator refinedIter = this->findLink(linksRefined_, iter->second.from(), iter->second.to());
std::multimap<int, rtabmap::Link>::iterator refinedIter = util3d::findLink(linksRefined_, iter->second.from(), iter->second.to());
if(refinedIter != linksRefined_.end())
{
memory_->addLoopClosureLink(refinedIter->second.to(), refinedIter->second.from(), refinedIter->second.transform(), true);
@@ -541,156 +541,6 @@ void DatabaseViewer::generateTOROGraph()
}
}
// margin=0 means infinite margin
std::map<int, int> DatabaseViewer::generateGraph(int fromNode, int margin)
{
UASSERT(margin >= 0);
//UDEBUG("signatureId=%d, neighborsMargin=%d", signatureId, margin);
std::map<int, int> ids;
if(fromNode<=0)
{
return ids;
}
std::list<int> curentMarginList;
std::set<int> currentMargin;
std::set<int> nextMargin;
nextMargin.insert(fromNode);
int m = 0;
while((margin == 0 || m < margin) && nextMargin.size())
{
curentMarginList = std::list<int>(nextMargin.begin(), nextMargin.end());
nextMargin.clear();
for(std::list<int>::iterator jter = curentMarginList.begin(); jter!=curentMarginList.end(); ++jter)
{
if(ids.find(*jter) == ids.end())
{
std::set<int> marginIds;
ids.insert(std::pair<int, int>(*jter, m));
for(int i=0; i<neighborLinks_.size(); ++i)
{
if(neighborLinks_[i].from() == *jter)
{
marginIds.insert(neighborLinks_[i].to());
}
else if(neighborLinks_[i].to() == *jter)
{
marginIds.insert(neighborLinks_[i].from());
}
}
for(int i=0; i<loopLinks_.size(); ++i)
{
if(loopLinks_[i].from() == *jter)
{
marginIds.insert(loopLinks_[i].to());
}
else if(loopLinks_[i].to() == *jter)
{
marginIds.insert(loopLinks_[i].from());
}
}
// Margin links
for(std::set<int>::const_iterator iter=marginIds.begin(); iter!=marginIds.end(); ++iter)
{
if( !uContains(ids, *iter) && nextMargin.find(*iter) == nextMargin.end())
{
nextMargin.insert(*iter);
}
}
}
}
++m;
}
return ids;
}
std::map<int, Transform> DatabaseViewer::optimizeGraph(
const std::map<int, int> & ids,
const std::map<int, Transform> & poses,
const std::multimap<int, Link> & links,
std::list<std::map<int, rtabmap::Transform> > * graphes)
{
std::map<int, rtabmap::Transform> optimizedPoses;
if(ids.size() && poses.size())
{
// Modify IDs using the margin from the current signature (TORO root will be the last signature)
int m = 0;
int toroId = 1;
std::map<int, int> rtabmapToToro; // <RTAB-Map ID, TORO ID>
std::map<int, int> toroToRtabmap; // <TORO ID, RTAB-Map ID>
std::map<int, int> idsTmp = ids;
while(idsTmp.size())
{
for(std::map<int, int>::iterator iter = idsTmp.begin(); iter!=idsTmp.end();)
{
if(m == iter->second)
{
rtabmapToToro.insert(std::make_pair(iter->first, toroId));
toroToRtabmap.insert(std::make_pair(toroId, iter->first));
++toroId;
idsTmp.erase(iter++);
}
else
{
++iter;
}
}
++m;
}
std::map<int, rtabmap::Transform> posesToro;
std::multimap<int, rtabmap::Link> edgeConstraintsToro;
for(std::map<int, rtabmap::Transform>::const_iterator iter = poses.begin(); iter!=poses.end(); ++iter)
{
if(uContains(ids, iter->first))
{
posesToro.insert(std::make_pair(rtabmapToToro.at(iter->first), iter->second));
}
}
for(std::multimap<int, rtabmap::Link>::const_iterator iter = links.begin();
iter!=links.end();
++iter)
{
if(uContains(ids, iter->second.from()) && uContains(ids, iter->second.to()))
{
edgeConstraintsToro.insert(std::make_pair(rtabmapToToro.at(iter->first), rtabmap::Link(rtabmapToToro.at(iter->first), rtabmapToToro.at(iter->second.to()), iter->second.transform(), iter->second.type())));
}
}
std::map<int, rtabmap::Transform> optimizedPosesToro;
rtabmap::Transform mapCorrectionToro;
// Optimize!
if(posesToro.size() && edgeConstraintsToro.size())
{
std::list<std::map<int, rtabmap::Transform> > graphesToro;
rtabmap::util3d::optimizeTOROGraph(posesToro, edgeConstraintsToro, optimizedPosesToro, mapCorrectionToro, ui_->spinBox_iterations->value(), ui_->checkBox_initGuess->isChecked(), &graphesToro);
for(std::map<int, rtabmap::Transform>::iterator iter=optimizedPosesToro.begin(); iter!=optimizedPosesToro.end(); ++iter)
{
optimizedPoses.insert(std::make_pair(toroToRtabmap.at(iter->first), iter->second));
}
if(graphes)
{
for(std::list<std::map<int, rtabmap::Transform> >::iterator iter = graphesToro.begin(); iter!=graphesToro.end(); ++iter)
{
std::map<int, rtabmap::Transform> tmp;
for(std::map<int, rtabmap::Transform>::iterator jter=iter->begin(); jter!=iter->end(); ++jter)
{
tmp.insert(std::make_pair(toroToRtabmap.at(jter->first), jter->second));
}
graphes->push_back(tmp);
}
}
}
}
return optimizedPoses;
}
void DatabaseViewer::view3DMap()
{
if(!ids_.size() || !memory_)
@@ -715,17 +565,19 @@ void DatabaseViewer::view3DMap()
double maxDepth = QInputDialog::getDouble(this, tr("Camera depth?"), tr("Maximum depth (m, 0=no max):"), 4.0, 0, 10, 2, &ok);
if(ok)
{
std::multimap<int, rtabmap::Link> links = updateLinksWithModifications(links_);
// <id, depth>
std::map<int, int> ids = generateGraph(ui_->spinBox_optimizationsFrom->value(), margin);
if(ids.size() > 0)
std::map<int, int> depthGraph = util3d::generateDepthGraph(links, ui_->spinBox_optimizationsFrom->value(), margin);
if(depthGraph.size() > 0)
{
rtabmap::DetailedProgressDialog progressDialog(this);
progressDialog.setMaximumSteps(ids.size()+2);
progressDialog.setMaximumSteps(depthGraph.size()+2);
progressDialog.show();
progressDialog.appendText("Graph optimization...");
std::multimap<int, Link> links = updateLinksWithModifications(links_);
std::map<int, Transform> optimizedPoses = optimizeGraph(ids, poses_, links);
std::map<int, Transform> optimizedPoses;
util3d::optimizeTOROGraph(depthGraph, poses_, links, optimizedPoses, ui_->spinBox_iterations->value(), ui_->checkBox_initGuess->isChecked());
progressDialog.appendText("Graph optimization... done!");
progressDialog.incrementStep();
@@ -846,28 +698,29 @@ void DatabaseViewer::generate3DMap()
QString path = QFileDialog::getExistingDirectory(this, tr("Save directory"), pathDatabase_);
if(!path.isEmpty())
{
std::map<int, int> ids = this->generateGraph(id, margin);
if(ids.size() > 0)
std::multimap<int, rtabmap::Link> links = updateLinksWithModifications(links_);
// <id, depth>
std::map<int, int> depthGraph = util3d::generateDepthGraph(links, id, margin);
if(depthGraph.size() > 0)
{
rtabmap::DetailedProgressDialog progressDialog;
progressDialog.setMaximumSteps((int)ids.size()+2);
progressDialog.setMaximumSteps((int)depthGraph.size()+2);
progressDialog.show();
progressDialog.appendText("Graph generation...");
std::map<int, rtabmap::Transform> poses, optimizedPoses;
std::multimap<int, rtabmap::Link> edgeConstraints;
memory_->getMetricConstraints(uKeys(ids), poses, edgeConstraints, true);
memory_->getMetricConstraints(uKeys(depthGraph), poses, edgeConstraints, true);
edgeConstraints = updateLinksWithModifications(edgeConstraints);
progressDialog.appendText("Graph generation... done!");
progressDialog.incrementStep();
progressDialog.appendText("Graph optimization...");
rtabmap::Transform mapCorrection;
rtabmap::util3d::optimizeTOROGraph(poses, edgeConstraints, optimizedPoses, mapCorrection, 100, true);
rtabmap::util3d::optimizeTOROGraph(poses, edgeConstraints, optimizedPoses, ui_->spinBox_iterations->value(), ui_->checkBox_initGuess->isChecked());
progressDialog.appendText("Graph optimization... done!");
progressDialog.incrementStep();
for(std::map<int, int>::iterator iter = ids.begin(); iter!=ids.end(); ++iter)
for(std::map<int, int>::iterator iter = depthGraph.begin(); iter!=depthGraph.end(); ++iter)
{
rtabmap::Transform pose = uValue(optimizedPoses, iter->first, rtabmap::Transform());
if(!pose.isNull())
@@ -920,7 +773,7 @@ void DatabaseViewer::generate3DMap()
}
progressDialog.setValue(progressDialog.maximumSteps());
QMessageBox::information(this, tr("Finished"), tr("%1 clouds generated to %2.").arg(ids.size()).arg(path));
QMessageBox::information(this, tr("Finished"), tr("%1 clouds generated to %2.").arg(depthGraph.size()).arg(path));
}
else
{
@@ -936,29 +789,50 @@ void DatabaseViewer::generate3DMap()
void DatabaseViewer::detectMoreLoopClosures()
{
std::map<int, rtabmap::Transform> optimizedPoses;
std::map<int, int> ids = this->generateGraph(ui_->spinBox_optimizationsFrom->value(), 0);
std::multimap<int, rtabmap::Link> links = updateLinksWithModifications(links_);
optimizedPoses = optimizeGraph(ids, poses_, links);
std::multimap<int, int> clusters = util3d::radiusPosesClustering(
optimizedPoses,
ui_->doubleSpinBox_detectMore_radius->value(),
ui_->doubleSpinBox_detectMore_angle->value());
std::multimap<int, rtabmap::Link> links = updateLinksWithModifications(links_);
std::map<int, int> depthGraph = util3d::generateDepthGraph(links, ui_->spinBox_optimizationsFrom->value());
util3d::optimizeTOROGraph(depthGraph, poses_, links, optimizedPoses, ui_->spinBox_iterations->value(), ui_->checkBox_initGuess->isChecked());
int iterations = ui_->doubleSpinBox_detectMore_iterations->value();
UASSERT(iterations > 0);
int added = 0;
for(std::multimap<int, int>::iterator iter=clusters.begin(); iter!= clusters.end(); ++iter)
for(int n=0; n<iterations; ++n)
{
int from = iter->first;
int to = iter->second;
if(!findActiveLink(from, to).isValid() && !containsLink(linksRemoved_, from, to))
UINFO("iteration %d/%d", n+1, iterations);
std::multimap<int, int> clusters = util3d::radiusPosesClustering(
optimizedPoses,
ui_->doubleSpinBox_detectMore_radius->value(),
ui_->doubleSpinBox_detectMore_angle->value()*CV_PI/180.0);
std::set<int> addedLinks;
for(std::multimap<int, int>::iterator iter=clusters.begin(); iter!= clusters.end(); ++iter)
{
if(addConstraint(from, to, true))
int from = iter->first;
int to = iter->second;
if(from < to)
{
UINFO("Added new loop closure between %d and %d.", from, to);
++added;
from = iter->second;
to = iter->first;
}
if(!findActiveLink(from, to).isValid() && !containsLink(linksRemoved_, from, to) &&
addedLinks.find(from) == addedLinks.end() && addedLinks.find(to) == addedLinks.end())
{
if(addConstraint(from, to, true))
{
UINFO("Added new loop closure between %d and %d.", from, to);
++added;
addedLinks.insert(from);
addedLinks.insert(to);
}
}
}
UINFO("Iteration %d/%d: added %d loop closures.", n+1, iterations, (int)addedLinks.size()/2);
if(addedLinks.size() == 0)
{
break;
}
}
UINFO("Added %d loop closures.", added);
UINFO("Total added %d loop closures.", added);
}
void DatabaseViewer::refineAllNeighborLinks()
@@ -1292,7 +1166,7 @@ void DatabaseViewer::updateConstraintView(const rtabmap::Link & link,
const pcl::PointCloud<pcl::PointXYZ>::Ptr & cloudFrom,
const pcl::PointCloud<pcl::PointXYZ>::Ptr & cloudTo)
{
std::multimap<int, Link>::iterator iter = findLink(linksRefined_, link.from(), link.to());
std::multimap<int, Link>::iterator iter = util3d::findLink(linksRefined_, link.from(), link.to());
rtabmap::Transform t = link.transform();
if(iter != linksRefined_.end())
{
@@ -1342,101 +1216,104 @@ void DatabaseViewer::updateConstraintView(const rtabmap::Link & link,
false); // don't update constraints view!
}
if(cloudFrom->size() == 0 && cloudTo->size() == 0)
if(ui_->constraintsViewer->isVisible())
{
Signature dataFrom, dataTo;
dataFrom = memory_->getSignatureData(link.from(), true);
UASSERT(dataFrom.getImageRaw().empty() || dataFrom.getImageRaw().type()==CV_8UC3 || dataFrom.getImageRaw().type() == CV_8UC1);
UASSERT(dataFrom.getDepthRaw().empty() || dataFrom.getDepthRaw().type()==CV_8UC1 || dataFrom.getDepthRaw().type() == CV_16UC1 || dataFrom.getDepthRaw().type() == CV_32FC1);
dataTo = memory_->getSignatureData(link.to(), true);
UASSERT(dataTo.getImageRaw().empty() || dataTo.getImageRaw().type()==CV_8UC3 || dataTo.getImageRaw().type() == CV_8UC1);
UASSERT(dataTo.getDepthRaw().empty() || dataTo.getDepthRaw().type()==CV_8UC1 || dataTo.getDepthRaw().type() == CV_16UC1 || dataTo.getDepthRaw().type() == CV_32FC1);
//cloud 3d
pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloudFrom;
if(dataFrom.getDepthRaw().type() == CV_8UC1)
if(cloudFrom->size() == 0 && cloudTo->size() == 0)
{
cloudFrom = rtabmap::util3d::cloudFromStereoImages(
dataFrom.getImageRaw(),
dataFrom.getDepthRaw(),
dataFrom.getDepthCx(), dataFrom.getDepthCy(),
dataFrom.getDepthFx(), dataFrom.getDepthFy(),
1);
Signature dataFrom, dataTo;
dataFrom = memory_->getSignatureData(link.from(), true);
UASSERT(dataFrom.getImageRaw().empty() || dataFrom.getImageRaw().type()==CV_8UC3 || dataFrom.getImageRaw().type() == CV_8UC1);
UASSERT(dataFrom.getDepthRaw().empty() || dataFrom.getDepthRaw().type()==CV_8UC1 || dataFrom.getDepthRaw().type() == CV_16UC1 || dataFrom.getDepthRaw().type() == CV_32FC1);
dataTo = memory_->getSignatureData(link.to(), true);
UASSERT(dataTo.getImageRaw().empty() || dataTo.getImageRaw().type()==CV_8UC3 || dataTo.getImageRaw().type() == CV_8UC1);
UASSERT(dataTo.getDepthRaw().empty() || dataTo.getDepthRaw().type()==CV_8UC1 || dataTo.getDepthRaw().type() == CV_16UC1 || dataTo.getDepthRaw().type() == CV_32FC1);
//cloud 3d
pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloudFrom;
if(dataFrom.getDepthRaw().type() == CV_8UC1)
{
cloudFrom = rtabmap::util3d::cloudFromStereoImages(
dataFrom.getImageRaw(),
dataFrom.getDepthRaw(),
dataFrom.getDepthCx(), dataFrom.getDepthCy(),
dataFrom.getDepthFx(), dataFrom.getDepthFy(),
1);
}
else
{
cloudFrom = rtabmap::util3d::cloudFromDepthRGB(
dataFrom.getImageRaw(),
dataFrom.getDepthRaw(),
dataFrom.getDepthCx(), dataFrom.getDepthCy(),
dataFrom.getDepthFx(), dataFrom.getDepthFy(),
1);
}
cloudFrom = rtabmap::util3d::removeNaNFromPointCloud<pcl::PointXYZRGB>(cloudFrom);
cloudFrom = rtabmap::util3d::transformPointCloud<pcl::PointXYZRGB>(cloudFrom, dataFrom.getLocalTransform());
pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloudTo;
if(dataTo.getDepthRaw().type() == CV_8UC1)
{
cloudTo = rtabmap::util3d::cloudFromStereoImages(
dataTo.getImageRaw(),
dataTo.getDepthRaw(),
dataTo.getDepthCx(), dataTo.getDepthCy(),
dataTo.getDepthFx(), dataTo.getDepthFy(),
1);
}
else
{
cloudTo = rtabmap::util3d::cloudFromDepthRGB(
dataTo.getImageRaw(),
dataTo.getDepthRaw(),
dataTo.getDepthCx(), dataTo.getDepthCy(),
dataTo.getDepthFx(), dataTo.getDepthFy(),
1);
}
cloudTo = rtabmap::util3d::removeNaNFromPointCloud<pcl::PointXYZRGB>(cloudTo);
cloudTo = rtabmap::util3d::transformPointCloud<pcl::PointXYZRGB>(cloudTo, t*dataTo.getLocalTransform());
//cloud 2d
pcl::PointCloud<pcl::PointXYZ>::Ptr scanA, scanB;
scanA = rtabmap::util3d::depth2DToPointCloud(dataFrom.getDepth2DRaw());
scanB = rtabmap::util3d::depth2DToPointCloud(dataTo.getDepth2DRaw());
scanB = rtabmap::util3d::transformPointCloud<pcl::PointXYZ>(scanB, t);
if(cloudFrom->size())
{
ui_->constraintsViewer->addOrUpdateCloud("cloud0", cloudFrom);
}
if(cloudTo->size())
{
ui_->constraintsViewer->addOrUpdateCloud("cloud1", cloudTo);
}
if(scanA->size())
{
ui_->constraintsViewer->addOrUpdateCloud("scan0", scanA);
}
if(scanB->size())
{
ui_->constraintsViewer->addOrUpdateCloud("scan1", scanB);
}
}
else
{
cloudFrom = rtabmap::util3d::cloudFromDepthRGB(
dataFrom.getImageRaw(),
dataFrom.getDepthRaw(),
dataFrom.getDepthCx(), dataFrom.getDepthCy(),
dataFrom.getDepthFx(), dataFrom.getDepthFy(),
1);
}
cloudFrom = rtabmap::util3d::removeNaNFromPointCloud<pcl::PointXYZRGB>(cloudFrom);
cloudFrom = rtabmap::util3d::transformPointCloud<pcl::PointXYZRGB>(cloudFrom, dataFrom.getLocalTransform());
pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloudTo;
if(dataTo.getDepthRaw().type() == CV_8UC1)
{
cloudTo = rtabmap::util3d::cloudFromStereoImages(
dataTo.getImageRaw(),
dataTo.getDepthRaw(),
dataTo.getDepthCx(), dataTo.getDepthCy(),
dataTo.getDepthFx(), dataTo.getDepthFy(),
1);
}
else
{
cloudTo = rtabmap::util3d::cloudFromDepthRGB(
dataTo.getImageRaw(),
dataTo.getDepthRaw(),
dataTo.getDepthCx(), dataTo.getDepthCy(),
dataTo.getDepthFx(), dataTo.getDepthFy(),
1);
}
cloudTo = rtabmap::util3d::removeNaNFromPointCloud<pcl::PointXYZRGB>(cloudTo);
cloudTo = rtabmap::util3d::transformPointCloud<pcl::PointXYZRGB>(cloudTo, t*dataTo.getLocalTransform());
//cloud 2d
pcl::PointCloud<pcl::PointXYZ>::Ptr scanA, scanB;
scanA = rtabmap::util3d::depth2DToPointCloud(dataFrom.getDepth2DRaw());
scanB = rtabmap::util3d::depth2DToPointCloud(dataTo.getDepth2DRaw());
scanB = rtabmap::util3d::transformPointCloud<pcl::PointXYZ>(scanB, t);
if(cloudFrom->size())
{
ui_->constraintsViewer->addOrUpdateCloud("cloud0", cloudFrom);
}
if(cloudTo->size())
{
ui_->constraintsViewer->addOrUpdateCloud("cloud1", cloudTo);
}
if(scanA->size())
{
ui_->constraintsViewer->addOrUpdateCloud("scan0", scanA);
}
if(scanB->size())
{
ui_->constraintsViewer->addOrUpdateCloud("scan1", scanB);
if(cloudFrom->size())
{
ui_->constraintsViewer->addOrUpdateCloud("cloud0", cloudFrom);
}
if(cloudTo->size())
{
ui_->constraintsViewer->addOrUpdateCloud("cloud1", cloudTo);
}
}
ui_->constraintsViewer->render();
}
else
{
if(cloudFrom->size())
{
ui_->constraintsViewer->addOrUpdateCloud("cloud0", cloudFrom);
}
if(cloudTo->size())
{
ui_->constraintsViewer->addOrUpdateCloud("cloud1", cloudTo);
}
}
ui_->constraintsViewer->render();
// update buttons
updateConstraintButtons();
@@ -1472,7 +1349,7 @@ void DatabaseViewer::updateConstraintButtons()
//check for modified link
bool modified = false;
std::multimap<int, Link>::iterator iter = findLink(linksRefined_, currentLink.from(), currentLink.to());
std::multimap<int, Link>::iterator iter = util3d::findLink(linksRefined_, currentLink.from(), currentLink.to());
if(iter != linksRefined_.end())
{
currentLink = iter->second;
@@ -1560,9 +1437,9 @@ void DatabaseViewer::updateGraphView()
std::map<int, rtabmap::Transform> finalPoses;
graphes_.push_back(poses_);
ui_->actionGenerate_TORO_graph_graph->setEnabled(true);
std::map<int, int> ids = this->generateGraph(ui_->spinBox_optimizationsFrom->value(), 0);
std::multimap<int, rtabmap::Link> links = updateLinksWithModifications(links_);
finalPoses = optimizeGraph(ids, poses_, links, &graphes_);
std::map<int, int> depthGraph = util3d::generateDepthGraph(links, ui_->spinBox_optimizationsFrom->value(), 0);
util3d::optimizeTOROGraph(depthGraph, poses_, links, finalPoses, ui_->spinBox_iterations->value(), ui_->checkBox_initGuess->isChecked(), &graphes_);
graphes_.push_back(finalPoses);
}
if(graphes_.size())
@@ -1578,52 +1455,24 @@ void DatabaseViewer::updateGraphView()
}
}
std::multimap<int, Link>::iterator DatabaseViewer::findLink(
std::multimap<int, Link> & links,
int from,
int to)
{
std::multimap<int, Link>::iterator iter = links.find(from);
while(iter != links.end() && iter->first == from)
{
if(iter->second.to() == to)
{
return iter;
}
++iter;
}
// let's try to -> from
iter = links.find(to);
while(iter != links.end() && iter->first == to)
{
if(iter->second.to() == from)
{
return iter;
}
++iter;
}
return links.end();
}
Link DatabaseViewer::findActiveLink(int from, int to)
{
Link link;
std::multimap<int, Link>::iterator findIter = findLink(linksRefined_, from ,to);
std::multimap<int, Link>::iterator findIter = util3d::findLink(linksRefined_, from ,to);
if(findIter != linksRefined_.end())
{
link = findIter->second;
}
else
{
findIter = findLink(linksAdded_, from ,to);
findIter = util3d::findLink(linksAdded_, from ,to);
if(findIter != linksAdded_.end())
{
link = findIter->second;
}
else if(!containsLink(linksRemoved_, from ,to))
{
findIter = findLink(links_, from ,to);
findIter = util3d::findLink(links_, from ,to);
if(findIter != links_.end())
{
link = findIter->second;
@@ -1635,7 +1484,7 @@ Link DatabaseViewer::findActiveLink(int from, int to)
bool DatabaseViewer::containsLink(std::multimap<int, Link> & links, int from, int to)
{
return findLink(links, from, to) != links.end();
return util3d::findLink(links, from, to) != links.end();
}
void DatabaseViewer::refineConstraint()
@@ -1900,7 +1749,7 @@ bool DatabaseViewer::addConstraint(int from, int to, bool silent)
else if(containsLink(linksRemoved_, from, to))
{
//simply remove from linksRemoved
linksRemoved_.erase(findLink(linksRemoved_, from, to));
linksRemoved_.erase(util3d::findLink(linksRemoved_, from, to));
updateSlider = true;
}
@@ -1929,18 +1778,18 @@ void DatabaseViewer::resetConstraint()
}
std::multimap<int, Link>::iterator iter = findLink(linksRefined_, from, to);
std::multimap<int, Link>::iterator iter = util3d::findLink(linksRefined_, from, to);
if(iter != linksRefined_.end())
{
linksRefined_.erase(iter);
}
iter = findLink(links_, from, to);
iter = util3d::findLink(links_, from, to);
if(iter != links_.end())
{
this->updateConstraintView(iter->second);
}
iter = findLink(linksAdded_, from, to);
iter = util3d::findLink(linksAdded_, from, to);
if(iter != linksAdded_.end())
{
this->updateConstraintView(iter->second);
@@ -1966,7 +1815,7 @@ void DatabaseViewer::rejectConstraint()
// find the original one
std::multimap<int, Link>::iterator iter;
iter = findLink(links_, from, to);
iter = util3d::findLink(links_, from, to);
if(iter != links_.end())
{
if(iter->second.type() == Link::kNeighbor)
@@ -1978,12 +1827,12 @@ void DatabaseViewer::rejectConstraint()
}
// remove from refined and added
iter = findLink(linksRefined_, from, to);
iter = util3d::findLink(linksRefined_, from, to);
if(iter != linksRefined_.end())
{
linksRefined_.erase(iter);
}
iter = findLink(linksAdded_, from, to);
iter = util3d::findLink(linksAdded_, from, to);
if(iter != linksAdded_.end())
{
linksAdded_.erase(iter);
@@ -2001,7 +1850,7 @@ std::multimap<int, rtabmap::Link> DatabaseViewer::updateLinksWithModifications(
{
std::multimap<int, rtabmap::Link>::iterator findIter;
findIter = findLink(linksRemoved_, iter->second.from(), iter->second.to());
findIter = util3d::findLink(linksRemoved_, iter->second.from(), iter->second.to());
if(findIter != linksRemoved_.end())
{
if(!(iter->second.from() == findIter->second.from() &&
@@ -2019,7 +1868,7 @@ std::multimap<int, rtabmap::Link> DatabaseViewer::updateLinksWithModifications(
}
}
findIter = findLink(linksRefined_, iter->second.from(), iter->second.to());
findIter = util3d::findLink(linksRefined_, iter->second.from(), iter->second.to());
if(findIter!=linksRefined_.end())
{
if(iter->second.from() == findIter->second.from() &&

407
guilib/src/MainWindow.cpp
View File

@@ -36,6 +36,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
#include "rtabmap/core/Parameters.h"
#include "rtabmap/core/ParamEvent.h"
#include "rtabmap/core/Signature.h"
#include "rtabmap/core/Memory.h"
#include "rtabmap/gui/ImageView.h"
#include "rtabmap/gui/KeypointItem.h"
@@ -55,6 +56,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
#include "PdfPlot.h"
#include "StatsToolBox.h"
#include "DetailedProgressDialog.h"
#include "PostProcessingDialog.h"
#include <QtGui/QCloseEvent>
#include <QtGui/QPixmap>
@@ -139,6 +141,7 @@ MainWindow::MainWindow(PreferencesDialog * prefDialog, QWidget * parent) :
// Create dialogs
_aboutDialog = new AboutDialog(this);
_exportDialog = new ExportCloudsDialog(this);
_postProcessingDialog = new PostProcessingDialog(this);
_ui = new Ui_mainWindow();
_ui->setupUi(this);
@@ -281,6 +284,7 @@ MainWindow::MainWindow(PreferencesDialog * prefDialog, QWidget * parent) :
connect(_ui->actionReset_Odometry, SIGNAL(triggered()), this, SLOT(resetOdometry()));
connect(_ui->actionTrigger_a_new_map, SIGNAL(triggered()), this, SLOT(triggerNewMap()));
connect(_ui->actionData_recorder, SIGNAL(triggered()), this, SLOT(dataRecorder()));
connect(_ui->actionPost_processing, SIGNAL(triggered()), this, SLOT(postProcessing()));
_ui->actionPause->setShortcut(Qt::Key_Space);
_ui->actionSave_point_cloud->setEnabled(false);
@@ -289,6 +293,7 @@ MainWindow::MainWindow(PreferencesDialog * prefDialog, QWidget * parent) :
_ui->actionView_scans->setEnabled(false);
_ui->actionView_high_res_point_cloud->setEnabled(false);
_ui->actionReset_Odometry->setEnabled(false);
_ui->actionPost_processing->setEnabled(false);
#if defined(Q_WS_MAC) || defined(Q_WS_WIN)
connect(_ui->actionOpen_working_directory, SIGNAL(triggered()), SLOT(openWorkingDirectory()));
@@ -1011,6 +1016,8 @@ void MainWindow::updateMapCloud(
_ui->actionExport_2D_Grid_map_bmp_png->setEnabled(true);
}
}
_ui->actionPost_processing->setEnabled(_currentPosesMap.size() >= 2 && _currentLinksMap.size() >= 1);
}
// filter duplicated poses
@@ -1166,7 +1173,7 @@ void MainWindow::updateMapCloud(
int fillEmptyRadius = _preferencesDialog->getGridMapFillEmptyRadius();
map8S = util3d::create2DMapFromOccupancyLocalMaps(poses, _occupancyLocalMaps, resolution, xMin, yMin, fillEmptyRadius);
}
else
else if(_createdScans.size())
{
bool fillEmptySpace = _preferencesDialog->getGridMapFillEmptySpace();
map8S = util3d::create2DMap(poses, _createdScans, resolution, fillEmptySpace, xMin, yMin);
@@ -1602,7 +1609,7 @@ void MainWindow::applyPrefSettings(PreferencesDialog::PANEL_FLAGS flags)
UDEBUG("Cloud rendering settings changed...");
if(_currentPosesMap.size())
{
this->updateMapCloud(std::map<int, Transform>(_currentPosesMap), Transform(), std::multimap<int, Link>());
this->updateMapCloud(std::map<int, Transform>(_currentPosesMap), Transform(), std::multimap<int, Link>(_currentLinksMap));
}
}
@@ -2542,6 +2549,400 @@ void MainWindow::generateTOROMap()
}
}
void MainWindow::postProcessing()
{
if(_postProcessingDialog->exec() != QDialog::Accepted)
{
return;
}
bool detectMoreLoopClosures = _postProcessingDialog->isDetectMoreLoopClosures();
bool reextractFeatures = _postProcessingDialog->isReextractFeatures();
bool refineNeighborLinks = _postProcessingDialog->isRefineNeighborLinks();
bool refineLoopClosureLinks = _postProcessingDialog->isRefineLoopClosureLinks();
double clusterRadius = _postProcessingDialog->clusterRadius();
double clusterAngle = _postProcessingDialog->clusterAngle();
int detectLoopClosureIterations = _postProcessingDialog->iterations();
if(!detectMoreLoopClosures && !refineNeighborLinks && !refineLoopClosureLinks)
{
UWARN("No post-processing selection...");
return;
}
// First, verify that we have all data required in the GUI
bool allDataAvailable = true;
std::map<int, Transform> odomPoses;
for(std::map<int, Transform>::iterator iter = _currentPosesMap.begin();
iter!=_currentPosesMap.end() && allDataAvailable;
++iter)
{
QMap<int, Signature>::iterator jter = _cachedSignatures.find(iter->first);
if(jter != _cachedSignatures.end())
{
if(jter->getPose().isNull())
{
UWARN("Odometry pose of %d is null.", iter->first);
allDataAvailable = false;
}
else
{
odomPoses.insert(*iter); // fill raw poses
}
if(jter->getLocalTransform().isNull())
{
UWARN("Local transform of %d is null.", iter->first);
allDataAvailable = false;
}
if(refineNeighborLinks || refineLoopClosureLinks || reextractFeatures)
{
// depth data required
if(jter->getDepthCompressed().empty() || jter->getDepthFx() <= 0.0f || jter->getDepthFy() <= 0.0f)
{
UWARN("Depth data of %d missing.", iter->first);
allDataAvailable = false;
}
if(reextractFeatures)
{
// rgb required
if(jter->getImageCompressed().empty())
{
UWARN("Rgb of %d missing.", iter->first);
allDataAvailable = false;
}
}
}
}
else
{
UWARN("Node %d missing.", iter->first);
allDataAvailable = false;
}
}
if(!allDataAvailable)
{
QMessageBox::warning(this, tr("Not all data available in the GUI..."),
tr("Some data missing in the cache to respect the constraints chosen. "
"Try \"Edit->Download all clouds\" to update the cache and try again."));
return;
}
_initProgressDialog->setAutoClose(false, 1);
_initProgressDialog->resetProgress();
_initProgressDialog->clear();
_initProgressDialog->show();
_initProgressDialog->appendText("Post-processing beginning!");
int totalSteps = 0;
if(refineNeighborLinks)
{
totalSteps+=odomPoses.size();
}
if(refineLoopClosureLinks)
{
totalSteps+=_currentLinksMap.size() - odomPoses.size();
}
_initProgressDialog->setMaximumSteps(totalSteps);
_initProgressDialog->show();
ParametersMap parameters = _preferencesDialog->getAllParameters();
int toroIterations = 100;
bool toroOptimizeFromGraphEnd = false;
Parameters::parse(parameters, Parameters::kRGBDToroIterations(), toroIterations);
Parameters::parse(parameters, Parameters::kRGBDOptimizeFromGraphEnd(), toroOptimizeFromGraphEnd);
int loopClosuresAdded = 0;
if(detectMoreLoopClosures)
{
Memory memory(parameters);
if(reextractFeatures)
{
ParametersMap customParameters;
// override some parameters
customParameters.insert(ParametersPair(Parameters::kMemRehearsalSimilarity(), "1.0")); // desactivate rehearsal
customParameters.insert(ParametersPair(Parameters::kMemImageKept(), "false"));
customParameters.insert(ParametersPair(Parameters::kMemSTMSize(), "0"));
customParameters.insert(ParametersPair(Parameters::kKpNewWordsComparedTogether(), "false"));
customParameters.insert(ParametersPair(Parameters::kKpNNStrategy(), parameters.at(Parameters::kLccReextractNNType())));
customParameters.insert(ParametersPair(Parameters::kKpNndrRatio(), parameters.at(Parameters::kLccReextractNNDR())));
customParameters.insert(ParametersPair(Parameters::kKpDetectorStrategy(), parameters.at(Parameters::kLccReextractFeatureType())));
customParameters.insert(ParametersPair(Parameters::kKpWordsPerImage(), parameters.at(Parameters::kLccReextractMaxWords())));
customParameters.insert(ParametersPair(Parameters::kMemGenerateIds(), "false"));
memory.parseParameters(customParameters);
}
UASSERT(detectLoopClosureIterations>0);
for(int n=0; n<detectLoopClosureIterations; ++n)
{
_initProgressDialog->appendText(tr("Looking for more loop closures, clustering poses... (iteration=%1/%2, radius=%3 m angle=%4 degrees)")
.arg(n+1).arg(detectLoopClosureIterations).arg(clusterRadius).arg(clusterAngle));
std::multimap<int, int> clusters = util3d::radiusPosesClustering(
_currentPosesMap,
clusterRadius,
clusterAngle*CV_PI/180.0);
_initProgressDialog->setMaximumSteps(_initProgressDialog->maximumSteps()+clusters.size());
_initProgressDialog->appendText(tr("Looking for more loop closures, clustering poses... found %1 clusters.").arg(clusters.size()));
int i=0;
std::set<int> addedLinks;
for(std::multimap<int, int>::iterator iter=clusters.begin(); iter!= clusters.end(); ++iter, ++i)
{
int from = iter->first;
int to = iter->second;
if(iter->first < iter->second)
{
from = iter->second;
to = iter->first;
}
// only add new links and one per cluster per iteration
if(addedLinks.find(from) == addedLinks.end() && addedLinks.find(to) == addedLinks.end() &&
util3d::findLink(_currentLinksMap, from, to) == _currentLinksMap.end())
{
if(!_cachedSignatures.contains(from))
{
UERROR("Didn't find signature %d", from);
}
else if(!_cachedSignatures.contains(to))
{
UERROR("Didn't find signature %d", to);
}
else
{
_initProgressDialog->incrementStep();
QApplication::processEvents();
Signature & signatureFrom = _cachedSignatures[from];
Signature & signatureTo = _cachedSignatures[to];
Transform transform;
std::string rejectedMsg;
int inliers;
if(reextractFeatures)
{
memory.init("", true); // clear previously added signatures
// Add signatures
SensorData dataFrom = signatureFrom.toSensorData();
SensorData dataTo = signatureTo.toSensorData();
if(dataFrom.isValid() &&
dataFrom.isMetric() &&
dataTo.isValid() &&
dataTo.isMetric() &&
dataFrom.id() != Memory::kIdInvalid &&
signatureFrom.id() != Memory::kIdInvalid)
{
if(from > to)
{
memory.update(dataTo);
memory.update(dataFrom);
}
else
{
memory.update(dataFrom);
memory.update(dataTo);
}
transform = memory.computeVisualTransform(dataTo.id(), dataFrom.id(), &rejectedMsg, &inliers);
}
else
{
UERROR("not supposed to be here!");
}
}
else
{
transform = memory.computeVisualTransform(signatureTo, signatureFrom, &rejectedMsg, &inliers);
}
if(!transform.isNull())
{
UINFO("Added new loop closure between %d and %d.", from, to);
addedLinks.insert(from);
addedLinks.insert(to);
_currentLinksMap.insert(std::make_pair(from, Link(from, to, transform, Link::kUserClosure)));
++loopClosuresAdded;
_initProgressDialog->appendText(tr("Detected loop closure %1->%2! (%3/%4)").arg(from).arg(to).arg(i+1).arg(clusters.size()));
}
}
}
}
_initProgressDialog->appendText(tr("Iteration %1/%2: Detected %3 loop closures!")
.arg(n+1).arg(detectLoopClosureIterations).arg(addedLinks.size()/2));
if(addedLinks.size() == 0)
{
break;
}
if(n+1 < detectLoopClosureIterations)
{
_initProgressDialog->appendText(tr("Optimizing graph with new links (%1 nodes, %2 constraints)...")
.arg(odomPoses.size()).arg(_currentLinksMap.size()));
std::map<int, rtabmap::Transform> optimizedPoses;
std::map<int, int> depthGraph = util3d::generateDepthGraph(_currentLinksMap, toroOptimizeFromGraphEnd?odomPoses.rbegin()->first:odomPoses.begin()->first);
util3d::optimizeTOROGraph(depthGraph, odomPoses, _currentLinksMap, optimizedPoses, toroIterations);
_currentPosesMap = optimizedPoses;
_initProgressDialog->appendText(tr("Optimizing graph with new links... done!"));
}
}
UINFO("Added %d loop closures.", loopClosuresAdded);
_initProgressDialog->appendText(tr("Total new loop closures detected=%1").arg(loopClosuresAdded));
}
if(refineNeighborLinks || refineLoopClosureLinks)
{
if(refineLoopClosureLinks)
{
_initProgressDialog->setMaximumSteps(_initProgressDialog->maximumSteps()+loopClosuresAdded);
}
_initProgressDialog->appendText(tr("Refining links..."));
int decimation=8;
float maxDepth=2.0f;
float voxelSize=0.01f;
int samples = 0;
float minFitness = 1.0f;
float maxCorrespondences = 0.05f;
float icpIterations = 30;
Parameters::parse(parameters, Parameters::kLccIcp3Decimation(), decimation);
Parameters::parse(parameters, Parameters::kLccIcp3MaxDepth(), maxDepth);
Parameters::parse(parameters, Parameters::kLccIcp3VoxelSize(), voxelSize);
Parameters::parse(parameters, Parameters::kLccIcp3Samples(), samples);
Parameters::parse(parameters, Parameters::kLccIcp3MaxFitness(), minFitness);
Parameters::parse(parameters, Parameters::kLccIcp3MaxCorrespondenceDistance(), maxCorrespondences);
Parameters::parse(parameters, Parameters::kLccIcp3Iterations(), icpIterations);
bool pointToPlane = false;
int pointToPlaneNormalNeighbors = 20;
Parameters::parse(parameters, Parameters::kLccIcp3PointToPlane(), pointToPlane);
Parameters::parse(parameters, Parameters::kLccIcp3PointToPlaneNormalNeighbors(), pointToPlaneNormalNeighbors);
int i=0;
for(std::multimap<int, Link>::iterator iter = _currentLinksMap.begin(); iter!=_currentLinksMap.end(); ++iter, ++i)
{
int type = iter->second.type();
if((refineNeighborLinks && type==Link::kNeighbor) ||
(refineLoopClosureLinks && type!=Link::kNeighbor))
{
int from = iter->second.from();
int to = iter->second.to();
_initProgressDialog->appendText(tr("Refining link %1->%2 (%3/%4)").arg(from).arg(to).arg(i+1).arg(_currentLinksMap.size()));
_initProgressDialog->incrementStep();
QApplication::processEvents();
if(!_cachedSignatures.contains(from))
{
UERROR("Didn't find signature %d",from);
}
else if(!_cachedSignatures.contains(to))
{
UERROR("Didn't find signature %d", to);
}
else
{
Signature & signatureFrom = _cachedSignatures[from];
Signature & signatureTo = _cachedSignatures[to];
//3D
cv::Mat depthA, depthB;
signatureFrom.uncompressData(0, &depthA, 0);
signatureTo.uncompressData(0, &depthB, 0);
if(depthA.type() == CV_8UC1 || depthB.type() == CV_8UC1)
{
QMessageBox::critical(this, tr("ICP failed"), tr("ICP cannot be done on stereo images!"));
UERROR("ICP 3D cannot be done on stereo images! Aborting refining links with ICP...");
break;
}
pcl::PointCloud<pcl::PointXYZ>::Ptr cloudA = util3d::getICPReadyCloud(depthA,
signatureFrom.getDepthFx(), signatureFrom.getDepthFy(), signatureFrom.getDepthCx(), signatureFrom.getDepthCy(),
decimation,
maxDepth,
voxelSize,
samples,
signatureFrom.getLocalTransform());
pcl::PointCloud<pcl::PointXYZ>::Ptr cloudB = util3d::getICPReadyCloud(depthB,
signatureTo.getDepthFx(), signatureTo.getDepthFy(), signatureTo.getDepthCx(), signatureTo.getDepthCy(),
decimation,
maxDepth,
voxelSize,
samples,
iter->second.transform() * signatureTo.getLocalTransform());
bool hasConverged = false;
double fitness = -1;
Transform transform;
if(pointToPlane)
{
pcl::PointCloud<pcl::PointNormal>::Ptr cloudANormals = util3d::computeNormals(cloudA, pointToPlaneNormalNeighbors);
pcl::PointCloud<pcl::PointNormal>::Ptr cloudBNormals = util3d::computeNormals(cloudB, pointToPlaneNormalNeighbors);
cloudANormals = util3d::removeNaNNormalsFromPointCloud<pcl::PointNormal>(cloudANormals);
if(cloudA->size() != cloudANormals->size())
{
UWARN("removed nan normals...");
}
cloudBNormals = util3d::removeNaNNormalsFromPointCloud<pcl::PointNormal>(cloudBNormals);
if(cloudB->size() != cloudBNormals->size())
{
UWARN("removed nan normals...");
}
transform = util3d::icpPointToPlane(cloudBNormals,
cloudANormals,
maxCorrespondences,
icpIterations,
hasConverged,
fitness);
}
else
{
transform = util3d::icp(cloudB,
cloudA,
maxCorrespondences,
icpIterations,
hasConverged,
fitness);
}
if(hasConverged && !transform.isNull() && fitness>=0.0f && fitness <= minFitness)
{
Link newLink(from, to, transform*iter->second.transform(), iter->second.type());
iter->second = newLink;
}
else
{
UWARN("Cannot refine link %d->%d (converged=%s fitness=%f)", from, to, hasConverged?"true":"false", fitness);
}
}
}
}
_initProgressDialog->appendText(tr("Refining links...done!"));
}
_initProgressDialog->appendText(tr("Optimizing graph with updated links (%1 nodes, %2 constraints)...")
.arg(odomPoses.size()).arg(_currentLinksMap.size()));
std::map<int, rtabmap::Transform> optimizedPoses;
std::map<int, int> depthGraph = util3d::generateDepthGraph(_currentLinksMap, toroOptimizeFromGraphEnd?odomPoses.rbegin()->first:odomPoses.begin()->first);
util3d::optimizeTOROGraph(depthGraph, odomPoses, _currentLinksMap, optimizedPoses, toroIterations);
_initProgressDialog->appendText(tr("Optimizing graph with updated links... done!"));
_initProgressDialog->incrementStep();
_initProgressDialog->appendText(tr("Updating map..."));
this->updateMapCloud(optimizedPoses, Transform(), _currentLinksMap, false);
_initProgressDialog->appendText(tr("Updating map... done!"));
_initProgressDialog->setValue(_initProgressDialog->maximumSteps());
_initProgressDialog->appendText("Post-processing finished!");
}
void MainWindow::deleteMemory()
{
QMessageBox::StandardButton button;
@@ -2778,12 +3179,14 @@ void MainWindow::clearTheCache()
_ui->widget_cloudViewer->removeAllClouds();
_ui->widget_cloudViewer->setBackgroundColor(Qt::black);
_ui->widget_cloudViewer->clearTrajectory();
_ui->widget_mapVisibility->clear();
_currentPosesMap.clear();
_odometryCorrection = Transform::getIdentity();
_lastOdomPose.setNull();
//disable save cloud action
_ui->actionExport_2D_Grid_map_bmp_png->setEnabled(false);
_ui->actionExport_2D_scans_ply_pcd->setEnabled(false);
_ui->actionPost_processing->setEnabled(false);
_ui->actionSave_point_cloud->setEnabled(false);
_ui->actionView_scans->setEnabled(false);
_ui->actionView_high_res_point_cloud->setEnabled(false);

7
guilib/src/MapVisibilityWidget.cpp
View File

@@ -57,6 +57,13 @@ void MapVisibilityWidget::showEvent(QShowEvent * event)
updateCheckBoxes();
}
void MapVisibilityWidget::clear()
{
_poses.clear();
_mask.clear();
updateCheckBoxes();
}
void MapVisibilityWidget::updateCheckBoxes()
{
QWidget * area = this->findChild<QWidget*>("area");

2
guilib/src/MapVisibilityWidget.h
View File

@@ -43,6 +43,8 @@ public:
void setMap(const std::map<int, Transform> & poses, const std::map<int, bool> & mask);
std::map<int, Transform> getVisiblePoses() const;
void clear();
protected:
virtual void showEvent(QShowEvent * event);

92
guilib/src/PostProcessingDialog.cpp Normal file
View File

@@ -0,0 +1,92 @@
/*
Copyright (c) 2010-2014, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
All rights reserved.
Redistribution and use in source and binary forms, with or without
modification, are permitted provided that the following conditions are met:
* Redistributions of source code must retain the above copyright
notice, this list of conditions and the following disclaimer.
* Redistributions in binary form must reproduce the above copyright
notice, this list of conditions and the following disclaimer in the
documentation and/or other materials provided with the distribution.
* Neither the name of the Universite de Sherbrooke nor the
names of its contributors may be used to endorse or promote products
derived from this software without specific prior written permission.
THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY
DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
*/
#include "PostProcessingDialog.h"
#include "ui_postProcessingDialog.h"
#include <QtGui/QPushButton>
namespace rtabmap {
PostProcessingDialog::PostProcessingDialog(QWidget * parent) :
QDialog(parent)
{
_ui = new Ui_PostProcessingDialog();
_ui->setupUi(this);
connect(_ui->detectMoreLoopClosures, SIGNAL(clicked(bool)), this, SLOT(updateButtonBox()));
connect(_ui->refineNeighborLinks, SIGNAL(stateChanged(int)), this, SLOT(updateButtonBox()));
connect(_ui->refineLoopClosureLinks, SIGNAL(stateChanged(int)), this, SLOT(updateButtonBox()));
}
PostProcessingDialog::~PostProcessingDialog()
{
delete _ui;
}
void PostProcessingDialog::updateButtonBox()
{
_ui->buttonBox->button(QDialogButtonBox::Ok)->setEnabled(
isDetectMoreLoopClosures() || isRefineNeighborLinks() || isRefineLoopClosureLinks());
}
bool PostProcessingDialog::isDetectMoreLoopClosures() const
{
return _ui->detectMoreLoopClosures->isChecked();
}
double PostProcessingDialog::clusterRadius() const
{
return _ui->clusterRadius->value();
}
double PostProcessingDialog::clusterAngle() const
{
return _ui->clusterAngle->value();
}
int PostProcessingDialog::iterations() const
{
return _ui->iterations->value();
}
bool PostProcessingDialog::isReextractFeatures() const
{
return _ui->reextractFeatures->isChecked();
}
bool PostProcessingDialog::isRefineNeighborLinks() const
{
return _ui->refineNeighborLinks->isChecked();
}
bool PostProcessingDialog::isRefineLoopClosureLinks() const
{
return _ui->refineLoopClosureLinks->isChecked();
}
}

63
guilib/src/PostProcessingDialog.h Normal file
View File

@@ -0,0 +1,63 @@
/*
Copyright (c) 2010-2014, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
All rights reserved.
Redistribution and use in source and binary forms, with or without
modification, are permitted provided that the following conditions are met:
* Redistributions of source code must retain the above copyright
notice, this list of conditions and the following disclaimer.
* Redistributions in binary form must reproduce the above copyright
notice, this list of conditions and the following disclaimer in the
documentation and/or other materials provided with the distribution.
* Neither the name of the Universite de Sherbrooke nor the
names of its contributors may be used to endorse or promote products
derived from this software without specific prior written permission.
THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY
DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
*/
#ifndef POSTPROCESSINGDIALOG_H_
#define POSTPROCESSINGDIALOG_H_
#include <QtGui/QDialog>
class Ui_PostProcessingDialog;
namespace rtabmap {
class PostProcessingDialog : public QDialog
{
Q_OBJECT
public:
PostProcessingDialog(QWidget * parent = 0);
virtual ~PostProcessingDialog();
bool isDetectMoreLoopClosures() const;
double clusterRadius() const;
double clusterAngle() const;
int iterations() const;
bool isReextractFeatures() const;
bool isRefineNeighborLinks() const;
bool isRefineLoopClosureLinks() const;
private slots:
void updateButtonBox();
private:
Ui_PostProcessingDialog * _ui;
};
}
#endif /* POSTPROCESSINGDIALOG_H_ */

2
guilib/src/PreferencesDialog.cpp
View File

@@ -441,6 +441,8 @@ PreferencesDialog::PreferencesDialog(QWidget * parent) :
_ui->loopClosure_icpMaxCorrespondenceDistance->setObjectName(Parameters::kLccIcp3MaxCorrespondenceDistance().c_str());
_ui->loopClosure_icpIterations->setObjectName(Parameters::kLccIcp3Iterations().c_str());
_ui->loopClosure_icpMaxFitness->setObjectName(Parameters::kLccIcp3MaxFitness().c_str());
_ui->loopClosure_icpPointToPlane->setObjectName(Parameters::kLccIcp3PointToPlane().c_str());
_ui->loopClosure_icpPointToPlaneNormals->setObjectName(Parameters::kLccIcp3PointToPlaneNormalNeighbors().c_str());
_ui->loopClosure_icp2MaxCorrespondenceDistance->setObjectName(Parameters::kLccIcp2MaxCorrespondenceDistance().c_str());
_ui->loopClosure_icp2Iterations->setObjectName(Parameters::kLccIcp2Iterations().c_str());

36
guilib/src/ui/DatabaseViewer.ui
View File

@@ -7,7 +7,7 @@
<x>0</x>
<y>0</y>
<width>1076</width>
<height>675</height>
<height>711</height>
</rect>
</property>
<property name="windowTitle">
@@ -825,7 +825,7 @@
<number>1000</number>
</property>
<property name="value">
<number>3</number>
<number>10</number>
</property>
</widget>
</item>
@@ -851,7 +851,7 @@
<double>1.000000000000000</double>
</property>
<property name="value">
<double>0.010000000000000</double>
<double>0.020000000000000</double>
</property>
</widget>
</item>
@@ -948,19 +948,39 @@
<item row="9" column="1">
<widget class="QDoubleSpinBox" name="doubleSpinBox_detectMore_angle">
<property name="suffix">
<string> rad</string>
<string> degrees</string>
</property>
<property name="decimals">
<number>2</number>
<number>0</number>
</property>
<property name="maximum">
<double>3.140000000000000</double>
<double>180.000000000000000</double>
</property>
<property name="singleStep">
<double>0.100000000000000</double>
<double>1.000000000000000</double>
</property>
<property name="value">
<double>0.300000000000000</double>
<double>30.000000000000000</double>
</property>
</widget>
</item>
<item row="10" column="0">
<widget class="QLabel" name="label_31">
<property name="text">
<string>Detect more LCs: iterations</string>
</property>
</widget>
</item>
<item row="10" column="1">
<widget class="QSpinBox" name="doubleSpinBox_detectMore_iterations">
<property name="minimum">
<number>1</number>
</property>
<property name="maximum">
<number>100</number>
</property>
<property name="value">
<number>1</number>
</property>
</widget>
</item>

7
guilib/src/ui/mainWindow.ui
View File

@@ -68,6 +68,8 @@
<addaction name="separator"/>
<addaction name="actionData_recorder"/>
<addaction name="separator"/>
<addaction name="actionPost_processing"/>
<addaction name="separator"/>
<addaction name="actionView_high_res_point_cloud"/>
<addaction name="actionView_scans"/>
</widget>
@@ -1088,6 +1090,11 @@
<string>Edit database...</string>
</property>
</action>
<action name="actionPost_processing">
<property name="text">
<string>Post-processing...</string>
</property>
</action>
</widget>
<customwidgets>
<customwidget>

252
guilib/src/ui/postProcessingDialog.ui Normal file
View File

@@ -0,0 +1,252 @@
<?xml version="1.0" encoding="UTF-8"?>
<ui version="4.0">
<class>PostProcessingDialog</class>
<widget class="QDialog" name="PostProcessingDialog">
<property name="geometry">
<rect>
<x>0</x>
<y>0</y>
<width>486</width>
<height>369</height>
</rect>
</property>
<property name="windowTitle">
<string>Post-Processing</string>
</property>
<property name="windowIcon">
<iconset resource="../GuiLib.qrc">
<normaloff>:/images/RTAB-Map.ico</normaloff>:/images/RTAB-Map.ico</iconset>
</property>
<layout class="QVBoxLayout" name="verticalLayout_3">
<item>
<widget class="QGroupBox" name="detectMoreLoopClosures">
<property name="title">
<string>Detect more loop closures</string>
</property>
<property name="checkable">
<bool>true</bool>
</property>
<layout class="QVBoxLayout" name="verticalLayout">
<item>
<layout class="QGridLayout" name="gridLayout" columnstretch="0,1">
<item row="1" column="0">
<widget class="QDoubleSpinBox" name="clusterAngle">
<property name="suffix">
<string> degrees</string>
</property>
<property name="decimals">
<number>0</number>
</property>
<property name="minimum">
<double>0.000000000000000</double>
</property>
<property name="maximum">
<double>180.000000000000000</double>
</property>
<property name="value">
<double>30.000000000000000</double>
</property>
</widget>
</item>
<item row="0" column="0">
<widget class="QDoubleSpinBox" name="clusterRadius">
<property name="suffix">
<string> m</string>
</property>
<property name="minimum">
<double>0.010000000000000</double>
</property>
<property name="maximum">
<double>99.000000000000000</double>
</property>
<property name="singleStep">
<double>0.010000000000000</double>
</property>
<property name="value">
<double>0.300000000000000</double>
</property>
</widget>
</item>
<item row="0" column="1">
<widget class="QLabel" name="label_3">
<property name="text">
<string>Cluster radius</string>
</property>
<property name="wordWrap">
<bool>true</bool>
</property>
</widget>
</item>
<item row="1" column="1">
<widget class="QLabel" name="clusterAngle_label">
<property name="text">
<string>Cluster angle</string>
</property>
<property name="wordWrap">
<bool>true</bool>
</property>
</widget>
</item>
<item row="3" column="1">
<widget class="QLabel" name="label_5">
<property name="text">
<string>Re-extract features</string>
</property>
<property name="wordWrap">
<bool>true</bool>
</property>
</widget>
</item>
<item row="3" column="0">
<widget class="QCheckBox" name="reextractFeatures">
<property name="text">
<string/>
</property>
<property name="checked">
<bool>false</bool>
</property>
</widget>
</item>
<item row="2" column="1">
<widget class="QLabel" name="label_6">
<property name="text">
<string>Iterations</string>
</property>
<property name="wordWrap">
<bool>true</bool>
</property>
</widget>
</item>
<item row="2" column="0">
<widget class="QSpinBox" name="iterations">
<property name="minimum">
<number>1</number>
</property>
<property name="maximum">
<number>100</number>
</property>
<property name="value">
<number>1</number>
</property>
</widget>
</item>
</layout>
</item>
<item>
<widget class="QLabel" name="label">
<property name="text">
<string>See &quot;RTAB-Map settings/RGB-D SLAM/Loop closure constraint&quot; to tune the parameters used.</string>
</property>
<property name="wordWrap">
<bool>true</bool>
</property>
</widget>
</item>
</layout>
</widget>
</item>
<item>
<widget class="QGroupBox" name="groupBox_3">
<property name="title">
<string>Refine links with ICP 3D</string>
</property>
<property name="checkable">
<bool>false</bool>
</property>
<layout class="QVBoxLayout" name="verticalLayout_2">
<item>
<widget class="QLabel" name="label_2">
<property name="text">
<string>See &quot;RTAB-Map settings/RGB-D SLAM/ICP&quot; to tune the parameters used.</string>
</property>
<property name="wordWrap">
<bool>true</bool>
</property>
</widget>
</item>
<item>
<widget class="QCheckBox" name="refineNeighborLinks">
<property name="text">
<string>Refine neighbor links</string>
</property>
<property name="checked">
<bool>false</bool>
</property>
</widget>
</item>
<item>
<widget class="QCheckBox" name="refineLoopClosureLinks">
<property name="text">
<string>Refine loop closure links</string>
</property>
<property name="checked">
<bool>false</bool>
</property>
</widget>
</item>
</layout>
</widget>
</item>
<item>
<spacer name="verticalSpacer">
<property name="orientation">
<enum>Qt::Vertical</enum>
</property>
<property name="sizeHint" stdset="0">
<size>
<width>20</width>
<height>40</height>
</size>
</property>
</spacer>
</item>
<item>
<widget class="QDialogButtonBox" name="buttonBox">
<property name="orientation">
<enum>Qt::Horizontal</enum>
</property>
<property name="standardButtons">
<set>QDialogButtonBox::Cancel|QDialogButtonBox::Ok</set>
</property>
</widget>
</item>
</layout>
</widget>
<resources>
<include location="../GuiLib.qrc"/>
</resources>
<connections>
<connection>
<sender>buttonBox</sender>
<signal>accepted()</signal>
<receiver>PostProcessingDialog</receiver>
<slot>accept()</slot>
<hints>
<hint type="sourcelabel">
<x>248</x>
<y>254</y>
</hint>
<hint type="destinationlabel">
<x>157</x>
<y>274</y>
</hint>
</hints>
</connection>
<connection>
<sender>buttonBox</sender>
<signal>rejected()</signal>
<receiver>PostProcessingDialog</receiver>
<slot>reject()</slot>
<hints>
<hint type="sourcelabel">
<x>316</x>
<y>260</y>
</hint>
<hint type="destinationlabel">
<x>286</x>
<y>274</y>
</hint>
</hints>
</connection>
</connections>
</ui>

42
guilib/src/ui/preferencesDialog.ui
View File

@@ -86,7 +86,7 @@
<enum>QFrame::Raised</enum>
</property>
<property name="currentIndex">
<number>5</number>
<number>21</number>
</property>
<widget class="QWidget" name="page_22">
<layout class="QVBoxLayout" name="verticalLayout_29">
@@ -5642,6 +5642,46 @@ Lower the ratio -&gt; higher the precision. 0 means disabled, matching the neare
</property>
</widget>
</item>
<item row="7" column="1">
<widget class="QLabel" name="label_144">
<property name="text">
<string>Point to plane ICP.</string>
</property>
<property name="wordWrap">
<bool>true</bool>
</property>
</widget>
</item>
<item row="7" column="0">
<widget class="QCheckBox" name="loopClosure_icpPointToPlane">
<property name="text">
<string/>
</property>
</widget>
</item>
<item row="8" column="0">
<widget class="QSpinBox" name="loopClosure_icpPointToPlaneNormals">
<property name="minimum">
<number>1</number>
</property>
<property name="maximum">
<number>1000</number>
</property>
<property name="value">
<number>20</number>
</property>
</widget>
</item>
<item row="8" column="1">
<widget class="QLabel" name="label_212">
<property name="text">
<string>Number of neighbors to compute normals for point to plane.</string>
</property>
<property name="wordWrap">
<bool>true</bool>
</property>
</widget>
</item>
</layout>
</widget>
</item>