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

Updated version to 0.8.0

Browse Source 
Libraries are installed in lib directly with symbolic links, not in lib/rtabmap-0.8. Removed the need of RPATH in cmake.
Saving variance of each link in database (new field Link.variance). The variance is used to generate the constraint information matrices for TORO optimization.
ICP: computing variance instead of fitness.
ICP3: added correspondences ratio parameter
Added OdometryInfo class
Refactoring: renamed depth2d stuff to laserScan. rtabmap::Memory and rtabmap::Signature classes (no more distinct neighbor, loop closure or child loop closure links, only links with different types)
This commit is contained in:
Mathieu Labbe
2014-12-14 16:42:10 -05:00
parent 6acf374063
commit 744e2fb3c7
42 changed files with 1764 additions and 1460 deletions
Download Patch File Download Diff File Expand all files Collapse all files

579
guilib/src/DatabaseViewer.cpp
View File

@@ -137,6 +137,8 @@ DatabaseViewer::DatabaseViewer(QWidget * parent) :
connect(ui_->horizontalSlider_loops, SIGNAL(valueChanged(int)), this, SLOT(sliderLoopValueChanged(int)));
connect(ui_->horizontalSlider_neighbors, SIGNAL(sliderMoved(int)), this, SLOT(sliderNeighborValueChanged(int)));
connect(ui_->horizontalSlider_loops, SIGNAL(sliderMoved(int)), this, SLOT(sliderLoopValueChanged(int)));
connect(ui_->checkBox_showOptimized, SIGNAL(stateChanged(int)), this, SLOT(updateConstraintView()));
ui_->checkBox_showOptimized->setEnabled(false);
ui_->horizontalSlider_iterations->setTracking(false);
ui_->dockWidget_graphView->setEnabled(false);
@@ -145,9 +147,10 @@ DatabaseViewer::DatabaseViewer(QWidget * parent) :
connect(ui_->spinBox_iterations, SIGNAL(editingFinished()), this, SLOT(updateGraphView()));
connect(ui_->spinBox_optimizationsFrom, SIGNAL(editingFinished()), this, SLOT(updateGraphView()));
connect(ui_->checkBox_initGuess, SIGNAL(stateChanged(int)), this, SLOT(updateGraphView()));
connect(ui_->checkBox_ignoreCovariance, SIGNAL(stateChanged(int)), this, SLOT(updateGraphView()));
ui_->constraintsViewer->setCameraLockZ(false);
ui_->constraintsViewer->updateCameraPosition(Transform::getIdentity());
ui_->constraintsViewer->setCameraFree();
}
DatabaseViewer::~DatabaseViewer()
@@ -189,6 +192,7 @@ bool DatabaseViewer::openDatabase(const QString & path)
linksRemoved_.clear();
scans_.clear();
ui_->actionGenerate_TORO_graph_graph->setEnabled(false);
ui_->checkBox_showOptimized->setEnabled(false);
}
std::string driverType = "sqlite3";
@@ -238,11 +242,11 @@ void DatabaseViewer::closeEvent(QCloseEvent* event)
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);
memory_->addLoopClosureLink(refinedIter->second.to(), refinedIter->second.from(), refinedIter->second.transform(), refinedIter->second.type(), refinedIter->second.variance());
}
else
{
memory_->addLoopClosureLink(iter->second.to(), iter->second.from(), iter->second.transform(), true);
memory_->addLoopClosureLink(iter->second.to(), iter->second.from(), iter->second.transform(), iter->second.type(), iter->second.variance());
}
}
@@ -252,7 +256,7 @@ void DatabaseViewer::closeEvent(QCloseEvent* event)
if(!containsLink(linksAdded_, iter->second.from(), iter->second.to()))
{
memory_->rejectLoopClosure(iter->second.to(), iter->second.from());
memory_->addLoopClosureLink(iter->second.to(), iter->second.from(), iter->second.transform(), true);
memory_->addLoopClosureLink(iter->second.to(), iter->second.from(), iter->second.transform(), iter->second.type(), iter->second.variance());
}
}
@@ -550,6 +554,128 @@ void DatabaseViewer::generateTOROGraph()
}
void DatabaseViewer::view3DMap()
{
if(!ids_.size() || !memory_)
{
QMessageBox::warning(this, tr("Cannot view 3D map"), tr("The database is empty..."));
return;
}
if(graphes_.empty())
{
this->updateGraphView();
if(graphes_.empty() || ui_->horizontalSlider_iterations->maximum() != (int)graphes_.size()-1)
{
QMessageBox::warning(this, tr("Cannot generate a graph"), tr("No graph in database?!"));
return;
}
}
bool ok = false;
QStringList items;
items.append("1");
items.append("2");
items.append("4");
items.append("8");
items.append("16");
QString item = QInputDialog::getItem(this, tr("Decimation?"), tr("Image decimation"), items, 2, false, &ok);
if(ok)
{
int decimation = item.toInt();
double maxDepth = QInputDialog::getDouble(this, tr("Camera depth?"), tr("Maximum depth (m, 0=no max):"), 4.0, 0, 10, 2, &ok);
if(ok)
{
const std::map<int, Transform> & optimizedPoses = uValueAt(graphes_, ui_->horizontalSlider_iterations->value());
if(optimizedPoses.size() > 0)
{
rtabmap::DetailedProgressDialog progressDialog(this);
progressDialog.setMaximumSteps(optimizedPoses.size());
progressDialog.show();
// create a window
QDialog * window = new QDialog(this, Qt::Window);
window->setModal(this->isModal());
window->setWindowTitle(tr("3D Map"));
window->setMinimumWidth(800);
window->setMinimumHeight(600);
rtabmap::CloudViewer * viewer = new rtabmap::CloudViewer(window);
QVBoxLayout *layout = new QVBoxLayout();
layout->addWidget(viewer);
viewer->setCameraLockZ(false);
window->setLayout(layout);
connect(window, SIGNAL(finished(int)), viewer, SLOT(clear()));
window->show();
for(std::map<int, Transform>::const_iterator iter = optimizedPoses.begin(); iter!=optimizedPoses.end(); ++iter)
{
rtabmap::Transform pose = iter->second;
if(!pose.isNull())
{
Signature data = memory_->getSignatureData(iter->first, true);
pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud;
UASSERT(data.getImageRaw().empty() || data.getImageRaw().type()==CV_8UC3 || data.getImageRaw().type() == CV_8UC1);
UASSERT(data.getDepthRaw().empty() || data.getDepthRaw().type()==CV_8UC1 || data.getDepthRaw().type() == CV_16UC1 || data.getDepthRaw().type() == CV_32FC1);
if(data.getDepthRaw().type() == CV_8UC1)
{
cv::Mat leftImg;
if(data.getImageRaw().channels() == 3)
{
cv::cvtColor(data.getImageRaw(), leftImg, CV_BGR2GRAY);
}
else
{
leftImg = data.getImageRaw();
}
cloud = rtabmap::util3d::cloudFromDisparityRGB(
data.getImageRaw(),
util3d::disparityFromStereoImages(leftImg, data.getDepthRaw()),
data.getDepthCx(), data.getDepthCy(),
data.getDepthFx(), data.getDepthFy(),
decimation);
}
else
{
cloud = rtabmap::util3d::cloudFromDepthRGB(
data.getImageRaw(),
data.getDepthRaw(),
data.getDepthCx(), data.getDepthCy(),
data.getDepthFx(), data.getDepthFy(),
decimation);
}
if(maxDepth)
{
cloud = rtabmap::util3d::passThrough<pcl::PointXYZRGB>(cloud, "z", 0, maxDepth);
}
cloud = rtabmap::util3d::transformPointCloud<pcl::PointXYZRGB>(cloud, data.getLocalTransform());
QColor color = Qt::red;
int mapId = memory_->getMapId(iter->first);
if(mapId >= 0)
{
color = (Qt::GlobalColor)(mapId % 12 + 7 );
}
viewer->addCloud(uFormat("cloud%d", iter->first), cloud, pose, color);
UINFO("Generated %d (%d points)", iter->first, cloud->size());
progressDialog.appendText(QString("Generated %1 (%2 points)").arg(iter->first).arg(cloud->size()));
progressDialog.incrementStep();
QApplication::processEvents();
}
}
progressDialog.setValue(progressDialog.maximumSteps());
}
else
{
QMessageBox::critical(this, tr("Error"), tr("No neighbors found for node %1.").arg(ui_->spinBox_optimizationsFrom->value()));
}
}
}
}
void DatabaseViewer::generate3DMap()
{
if(!ids_.size() || !memory_)
{
@@ -557,58 +683,32 @@ void DatabaseViewer::view3DMap()
return;
}
bool ok = false;
int margin = QInputDialog::getInt(this, tr("Depth around the location?"), tr("Margin (0=no limit)"), 0, 0, 100, 1, &ok);
QStringList items;
items.append("1");
items.append("2");
items.append("4");
items.append("8");
items.append("16");
QString item = QInputDialog::getItem(this, tr("Decimation?"), tr("Image decimation"), items, 2, false, &ok);
if(ok)
{
QStringList items;
items.append("1");
items.append("2");
items.append("4");
items.append("8");
items.append("16");
QString item = QInputDialog::getItem(this, tr("Decimation?"), tr("Image decimation"), items, 2, false, &ok);
int decimation = item.toInt();
double maxDepth = QInputDialog::getDouble(this, tr("Camera depth?"), tr("Maximum depth (m, 0=no max):"), 4.0, 0, 10, 2, &ok);
if(ok)
{
int decimation = item.toInt();
double maxDepth = QInputDialog::getDouble(this, tr("Camera depth?"), tr("Maximum depth (m, 0=no max):"), 4.0, 0, 10, 2, &ok);
if(ok)
QString path = QFileDialog::getExistingDirectory(this, tr("Save directory"), pathDatabase_);
if(!path.isEmpty())
{
std::multimap<int, rtabmap::Link> links = updateLinksWithModifications(links_);
// <id, depth>
std::map<int, int> depthGraph = util3d::generateDepthGraph(links, ui_->spinBox_optimizationsFrom->value(), margin);
if(depthGraph.size() > 0)
const std::map<int, Transform> & optimizedPoses = uValueAt(graphes_, ui_->horizontalSlider_iterations->value());
if(optimizedPoses.size() > 0)
{
rtabmap::DetailedProgressDialog progressDialog(this);
progressDialog.setMaximumSteps(depthGraph.size()+2);
rtabmap::DetailedProgressDialog progressDialog;
progressDialog.setMaximumSteps((int)optimizedPoses.size());
progressDialog.show();
progressDialog.appendText("Graph optimization...");
std::multimap<int, Link> links = updateLinksWithModifications(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();
// create a window
QDialog * window = new QDialog(this, Qt::Window);
window->setModal(this->isModal());
window->setWindowTitle(tr("3D Map"));
window->setMinimumWidth(800);
window->setMinimumHeight(600);
rtabmap::CloudViewer * viewer = new rtabmap::CloudViewer(window);
QVBoxLayout *layout = new QVBoxLayout();
layout->addWidget(viewer);
viewer->setCameraLockZ(false);
window->setLayout(layout);
connect(window, SIGNAL(finished(int)), viewer, SLOT(clear()));
window->show();
for(std::map<int, Transform>::iterator iter = optimizedPoses.begin(); iter!=optimizedPoses.end(); ++iter)
for(std::map<int, Transform>::const_iterator iter = optimizedPoses.begin(); iter!=optimizedPoses.end(); ++iter)
{
rtabmap::Transform pose = iter->second;
const rtabmap::Transform & pose = iter->second;
if(!pose.isNull())
{
Signature data = memory_->getSignatureData(iter->first, true);
@@ -648,23 +748,18 @@ void DatabaseViewer::view3DMap()
cloud = rtabmap::util3d::passThrough<pcl::PointXYZRGB>(cloud, "z", 0, maxDepth);
}
cloud = rtabmap::util3d::transformPointCloud<pcl::PointXYZRGB>(cloud, data.getLocalTransform());
QColor color = Qt::red;
int mapId = memory_->getMapId(iter->first);
if(mapId >= 0)
{
color = (Qt::GlobalColor)(mapId % 12 + 7 );
}
viewer->addCloud(uFormat("cloud%d", iter->first), cloud, pose, color);
UINFO("Generated %d (%d points)", iter->first, cloud->size());
progressDialog.appendText(QString("Generated %1 (%2 points)").arg(iter->first).arg(cloud->size()));
cloud = rtabmap::util3d::transformPointCloud<pcl::PointXYZRGB>(cloud, pose*data.getLocalTransform());
std::string name = uFormat("%s/node%d.pcd", path.toStdString().c_str(), iter->first);
pcl::io::savePCDFile(name, *cloud);
UINFO("Saved %s (%d points)", name.c_str(), cloud->size());
progressDialog.appendText(QString("Saved %1 (%2 points)").arg(name.c_str()).arg(cloud->size()));
progressDialog.incrementStep();
QApplication::processEvents();
}
}
progressDialog.setValue(progressDialog.maximumSteps());
QMessageBox::information(this, tr("Finished"), tr("%1 clouds generated to %2.").arg(optimizedPoses.size()).arg(path));
}
else
{
@@ -675,132 +770,9 @@ void DatabaseViewer::view3DMap()
}
}
void DatabaseViewer::generate3DMap()
{
if(!ids_.size() || !memory_)
{
QMessageBox::warning(this, tr("Cannot generate a graph"), tr("The database is empty..."));
return;
}
bool ok = false;
int id = QInputDialog::getInt(this, tr("Around which location?"), tr("Location ID"), ids_.first(), ids_.first(), ids_.last(), 1, &ok);
if(ok)
{
int margin = QInputDialog::getInt(this, tr("Depth around the location?"), tr("Margin (0=no limit)"), 0, 0, 100, 1, &ok);
if(ok)
{
QStringList items;
items.append("1");
items.append("2");
items.append("4");
items.append("8");
items.append("16");
QString item = QInputDialog::getItem(this, tr("Decimation?"), tr("Image decimation"), items, 2, false, &ok);
if(ok)
{
int decimation = item.toInt();
double maxDepth = QInputDialog::getDouble(this, tr("Camera depth?"), tr("Maximum depth (m, 0=no max):"), 4.0, 0, 10, 2, &ok);
if(ok)
{
QString path = QFileDialog::getExistingDirectory(this, tr("Save directory"), pathDatabase_);
if(!path.isEmpty())
{
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)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(depthGraph), poses, edgeConstraints, true);
edgeConstraints = updateLinksWithModifications(edgeConstraints);
progressDialog.appendText("Graph generation... done!");
progressDialog.incrementStep();
progressDialog.appendText("Graph optimization...");
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 = depthGraph.begin(); iter!=depthGraph.end(); ++iter)
{
rtabmap::Transform pose = uValue(optimizedPoses, iter->first, rtabmap::Transform());
if(!pose.isNull())
{
Signature data = memory_->getSignatureData(iter->first, true);
pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud;
UASSERT(data.getImageRaw().empty() || data.getImageRaw().type()==CV_8UC3 || data.getImageRaw().type() == CV_8UC1);
UASSERT(data.getDepthRaw().empty() || data.getDepthRaw().type()==CV_8UC1 || data.getDepthRaw().type() == CV_16UC1 || data.getDepthRaw().type() == CV_32FC1);
if(data.getDepthRaw().type() == CV_8UC1)
{
cv::Mat leftImg;
if(data.getImageRaw().channels() == 3)
{
cv::cvtColor(data.getImageRaw(), leftImg, CV_BGR2GRAY);
}
else
{
leftImg = data.getImageRaw();
}
cloud = rtabmap::util3d::cloudFromDisparityRGB(
data.getImageRaw(),
util3d::disparityFromStereoImages(leftImg, data.getDepthRaw()),
data.getDepthCx(), data.getDepthCy(),
data.getDepthFx(), data.getDepthFy(),
decimation);
}
else
{
cloud = rtabmap::util3d::cloudFromDepthRGB(
data.getImageRaw(),
data.getDepthRaw(),
data.getDepthCx(), data.getDepthCy(),
data.getDepthFx(), data.getDepthFy(),
decimation);
}
if(maxDepth)
{
cloud = rtabmap::util3d::passThrough<pcl::PointXYZRGB>(cloud, "z", 0, maxDepth);
}
cloud = rtabmap::util3d::transformPointCloud<pcl::PointXYZRGB>(cloud, pose*data.getLocalTransform());
std::string name = uFormat("%s/node%d.pcd", path.toStdString().c_str(), iter->first);
pcl::io::savePCDFile(name, *cloud);
UINFO("Saved %s (%d points)", name.c_str(), cloud->size());
progressDialog.appendText(QString("Saved %1 (%2 points)").arg(name.c_str()).arg(cloud->size()));
progressDialog.incrementStep();
QApplication::processEvents();
}
}
progressDialog.setValue(progressDialog.maximumSteps());
QMessageBox::information(this, tr("Finished"), tr("%1 clouds generated to %2.").arg(depthGraph.size()).arg(path));
}
else
{
QMessageBox::critical(this, tr("Error"), tr("No neighbors found for node %1.").arg(id));
}
}
}
}
}
}
}
void DatabaseViewer::detectMoreLoopClosures()
{
std::map<int, rtabmap::Transform> optimizedPoses;
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());
const std::map<int, Transform> & optimizedPoses = graphes_.back();
int iterations = ui_->doubleSpinBox_detectMore_iterations->value();
UASSERT(iterations > 0);
@@ -825,7 +797,7 @@ void DatabaseViewer::detectMoreLoopClosures()
if(!findActiveLink(from, to).isValid() && !containsLink(linksRemoved_, from, to) &&
addedLinks.find(from) == addedLinks.end() && addedLinks.find(to) == addedLinks.end())
{
if(addConstraint(from, to, true))
if(addConstraint(from, to, true, false))
{
UINFO("Added new loop closure between %d and %d.", from, to);
++added;
@@ -840,6 +812,10 @@ void DatabaseViewer::detectMoreLoopClosures()
break;
}
}
if(added)
{
this->updateGraphView();
}
UINFO("Total added %d loop closures.", added);
}
@@ -855,12 +831,14 @@ void DatabaseViewer::refineAllNeighborLinks()
{
int from = neighborLinks_[i].from();
int to = neighborLinks_[i].to();
this->refineConstraint(neighborLinks_[i].from(), neighborLinks_[i].to());
this->refineConstraint(neighborLinks_[i].from(), neighborLinks_[i].to(), false);
progressDialog.appendText(tr("Refined link %1->%2 (%3/%4)").arg(from).arg(to).arg(i+1).arg(neighborLinks_.size()));
progressDialog.incrementStep();
QApplication::processEvents();
}
this->updateGraphView();
progressDialog.setValue(progressDialog.maximumSteps());
progressDialog.appendText("Refining links finished!");
}
@@ -878,12 +856,14 @@ void DatabaseViewer::refineAllLoopClosureLinks()
{
int from = loopLinks_[i].from();
int to = loopLinks_[i].to();
this->refineConstraint(loopLinks_[i].from(), loopLinks_[i].to());
this->refineConstraint(loopLinks_[i].from(), loopLinks_[i].to(), false);
progressDialog.appendText(tr("Refined link %1->%2 (%3/%4)").arg(from).arg(to).arg(i+1).arg(loopLinks_.size()));
progressDialog.incrementStep();
QApplication::processEvents();
}
this->updateGraphView();
progressDialog.setValue(progressDialog.maximumSteps());
progressDialog.appendText("Refining links finished!");
}
@@ -901,12 +881,14 @@ void DatabaseViewer::refineVisuallyAllNeighborLinks()
{
int from = neighborLinks_[i].from();
int to = neighborLinks_[i].to();
this->refineConstraintVisually(neighborLinks_[i].from(), neighborLinks_[i].to());
this->refineConstraintVisually(neighborLinks_[i].from(), neighborLinks_[i].to(), false);
progressDialog.appendText(tr("Refined link %1->%2 (%3/%4)").arg(from).arg(to).arg(i+1).arg(neighborLinks_.size()));
progressDialog.incrementStep();
QApplication::processEvents();
}
this->updateGraphView();
progressDialog.setValue(progressDialog.maximumSteps());
progressDialog.appendText("Refining links finished!");
}
@@ -924,12 +906,14 @@ void DatabaseViewer::refineVisuallyAllLoopClosureLinks()
{
int from = loopLinks_[i].from();
int to = loopLinks_[i].to();
this->refineConstraintVisually(loopLinks_[i].from(), loopLinks_[i].to());
this->refineConstraintVisually(loopLinks_[i].from(), loopLinks_[i].to(), false);
progressDialog.appendText(tr("Refined link %1->%2 (%3/%4)").arg(from).arg(to).arg(i+1).arg(loopLinks_.size()));
progressDialog.incrementStep();
QApplication::processEvents();
}
this->updateGraphView();
progressDialog.setValue(progressDialog.maximumSteps());
progressDialog.appendText("Refining links finished!");
}
@@ -1026,26 +1010,24 @@ void DatabaseViewer::update(int value,
}
// loops
std::map<int, rtabmap::Transform> parents;
std::map<int, rtabmap::Transform> children;
memory_->getLoopClosureIds(id, parents, children, true);
if(parents.size())
std::map<int, rtabmap::Link> loopClosures;
loopClosures = memory_->getLoopClosureLinks(id, true);
if(loopClosures.size())
{
QString str;
for(std::map<int, rtabmap::Transform>::iterator iter=parents.begin(); iter!=parents.end(); ++iter)
QString strParents, strChildren;
for(std::map<int, rtabmap::Link>::iterator iter=loopClosures.begin(); iter!=loopClosures.end(); ++iter)
{
str.append(QString("%1 ").arg(iter->first));
if(iter->first < id)
{
strChildren.append(QString("%1 ").arg(iter->first));
}
else
{
strParents.append(QString("%1 ").arg(iter->first));
}
}
labelParents->setText(str);
}
if(children.size())
{
QString str;
for(std::map<int, rtabmap::Transform>::iterator iter=children.begin(); iter!=children.end(); ++iter)
{
str.append(QString("%1 ").arg(iter->first));
}
labelChildren->setText(str);
labelParents->setText(strParents);
labelChildren->setText(strChildren);
}
}
@@ -1396,22 +1378,61 @@ void DatabaseViewer::sliderLoopValueChanged(int value)
this->updateConstraintView(loopLinks_.at(value));
}
void DatabaseViewer::updateConstraintView(const rtabmap::Link & link,
// only called when ui_->checkBox_showOptimized state changed
void DatabaseViewer::updateConstraintView()
{
this->updateConstraintView(neighborLinks_.at(ui_->horizontalSlider_neighbors->value()),
pcl::PointCloud<pcl::PointXYZ>::Ptr(new pcl::PointCloud<pcl::PointXYZ>),
pcl::PointCloud<pcl::PointXYZ>::Ptr(new pcl::PointCloud<pcl::PointXYZ>),
false);
}
void DatabaseViewer::updateConstraintView(const rtabmap::Link & linkIn,
const pcl::PointCloud<pcl::PointXYZ>::Ptr & cloudFrom,
const pcl::PointCloud<pcl::PointXYZ>::Ptr & cloudTo,
bool updateImageSliders)
{
std::multimap<int, Link>::iterator iter = util3d::findLink(linksRefined_, link.from(), link.to());
rtabmap::Transform t = link.transform();
std::multimap<int, Link>::iterator iter = util3d::findLink(linksRefined_, linkIn.from(), linkIn.to());
rtabmap::Link link = linkIn;
if(iter != linksRefined_.end())
{
t = iter->second.transform();
link = iter->second;
}
rtabmap::Transform t = link.transform();
ui_->label_constraint->clear();
ui_->label_constraint_opt->clear();
ui_->checkBox_showOptimized->setEnabled(false);
UASSERT(!t.isNull() && memory_);
ui_->label_constraint->setText(t.prettyPrint().c_str());
ui_->label_constraint->setText(QString("%1 (%2=%3)").arg(t.prettyPrint().c_str()).arg(QChar(0xc3, 0x03)).arg(sqrt(link.variance())));
if(link.type() == Link::kNeighbor &&
graphes_.size() &&
(int)graphes_.size()-1 == ui_->horizontalSlider_iterations->maximum())
{
std::map<int, rtabmap::Transform> & graph = uValueAt(graphes_, ui_->horizontalSlider_iterations->value());
if(link.type() == Link::kNeighbor)
{
std::map<int, rtabmap::Transform>::iterator iterFrom = graph.find(link.from());
std::map<int, rtabmap::Transform>::iterator iterTo = graph.find(link.to());
if(iterFrom != graph.end() && iterTo != graph.end())
{
ui_->checkBox_showOptimized->setEnabled(true);
Transform topt = iterFrom->second.inverse()*iterTo->second;
Transform delta = t.inverse()*topt;
Transform v1 = t.rotation()*Transform(1,0,0,0,0,0);
Transform v2 = topt.rotation()*Transform(1,0,0,0,0,0);
float a = pcl::getAngle3D(Eigen::Vector4f(v1.x(), v1.y(), v1.z(), 0), Eigen::Vector4f(v2.x(), v2.y(), v2.z(), 0));
a = (a *180.0f) / CV_PI;
ui_->label_constraint_opt->setText(QString("%1 (error=%2% a=%3)").arg(topt.prettyPrint().c_str()).arg((delta.getNorm()/t.getNorm())*100.0f).arg(a));
if(ui_->checkBox_showOptimized->isChecked())
{
t = topt;
}
}
}
}
if(updateImageSliders)
{
@@ -1587,8 +1608,8 @@ void DatabaseViewer::updateConstraintView(const rtabmap::Link & link,
//cloud 2d
pcl::PointCloud<pcl::PointXYZ>::Ptr scanA, scanB;
scanA = rtabmap::util3d::depth2DToPointCloud(dataFrom.getDepth2DRaw());
scanB = rtabmap::util3d::depth2DToPointCloud(dataTo.getDepth2DRaw());
scanA = rtabmap::util3d::laserScanToPointCloud(dataFrom.getLaserScanRaw());
scanB = rtabmap::util3d::laserScanToPointCloud(dataTo.getLaserScanRaw());
scanB = rtabmap::util3d::transformPointCloud<pcl::PointXYZ>(scanB, t);
if(scanA->size())
@@ -1611,6 +1632,11 @@ void DatabaseViewer::updateConstraintView(const rtabmap::Link & link,
ui_->constraintsViewer->addOrUpdateCloud("cloud1", cloudTo);
}
}
//update cordinate
ui_->constraintsViewer->updateCameraPosition(t);
ui_->constraintsViewer->clearTrajectory();
ui_->constraintsViewer->render();
}
@@ -1669,18 +1695,18 @@ void DatabaseViewer::sliderIterationsValueChanged(int value)
{
if(memory_ && value >=0 && value < (int)graphes_.size())
{
if(scans_.size() == 0)
if(ui_->dockWidget_graphView->isVisible() && scans_.size() == 0)
{
//update scans
UINFO("Update scans list...");
for(int i=0; i<ids_.size(); ++i)
{
Signature data = memory_->getSignatureData(ids_.at(i), false);
if(!data.getDepth2DCompressed().empty())
if(!data.getLaserScanCompressed().empty())
{
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud;
cv::Mat depth2d = rtabmap::util3d::uncompressData(data.getDepth2DCompressed());
cloud = rtabmap::util3d::depth2DToPointCloud(depth2d);
cv::Mat laserScan = rtabmap::util3d::uncompressData(data.getLaserScanCompressed());
cloud = rtabmap::util3d::laserScanToPointCloud(laserScan);
scans_.insert(std::make_pair(ids_.at(i), cloud));
}
}
@@ -1722,7 +1748,7 @@ void DatabaseViewer::sliderIterationsValueChanged(int value)
}
void DatabaseViewer::updateGraphView()
{
if(ui_->dockWidget_graphView->isVisible() && poses_.size())
if(poses_.size())
{
if(!uContains(poses_, ui_->spinBox_optimizationsFrom->value()))
{
@@ -1740,7 +1766,14 @@ void DatabaseViewer::updateGraphView()
ui_->actionGenerate_TORO_graph_graph->setEnabled(true);
std::multimap<int, rtabmap::Link> links = updateLinksWithModifications(links_);
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_);
util3d::optimizeTOROGraph(
depthGraph,
poses_,
links, finalPoses,
ui_->spinBox_iterations->value(),
ui_->checkBox_initGuess->isChecked(),
ui_->checkBox_ignoreCovariance->isChecked(),
&graphes_);
graphes_.push_back(finalPoses);
}
if(graphes_.size())
@@ -1792,10 +1825,10 @@ void DatabaseViewer::refineConstraint()
{
int from = ids_.at(ui_->horizontalSlider_A->value());
int to = ids_.at(ui_->horizontalSlider_B->value());
refineConstraint(from, to);
refineConstraint(from, to, true);
}
void DatabaseViewer::refineConstraint(int from, int to)
void DatabaseViewer::refineConstraint(int from, int to, bool updateGraph)
{
if(from == to)
{
@@ -1809,10 +1842,29 @@ void DatabaseViewer::refineConstraint(int from, int to)
UERROR("Not found link! (%d->%d)", from, to);
return;
}
Transform t = currentLink.transform();
if(ui_->checkBox_showOptimized->isChecked() &&
currentLink.type() == Link::kNeighbor &&
graphes_.size() &&
(int)graphes_.size()-1 == ui_->horizontalSlider_iterations->maximum())
{
std::map<int, rtabmap::Transform> & graph = uValueAt(graphes_, ui_->horizontalSlider_iterations->value());
if(currentLink.type() == Link::kNeighbor)
{
std::map<int, rtabmap::Transform>::iterator iterFrom = graph.find(currentLink.from());
std::map<int, rtabmap::Transform>::iterator iterTo = graph.find(currentLink.to());
if(iterFrom != graph.end() && iterTo != graph.end())
{
Transform topt = iterFrom->second.inverse()*iterTo->second;
t = topt;
}
}
}
bool hasConverged = false;
double fitness = 0.0f;
double variance = -1.0;
int correspondences = 0;
Transform transform;
Signature dataFrom, dataTo;
@@ -1824,14 +1876,14 @@ void DatabaseViewer::refineConstraint(int from, int to)
if(ui_->checkBox_icp_2d->isChecked())
{
//2D
cv::Mat oldDepth2D = util3d::uncompressData(dataFrom.getDepth2DCompressed());
cv::Mat newDepth2D = util3d::uncompressData(dataTo.getDepth2DCompressed());
cv::Mat oldLaserScan = util3d::uncompressData(dataFrom.getLaserScanCompressed());
cv::Mat newLaserScan = util3d::uncompressData(dataTo.getLaserScanCompressed());
if(!oldDepth2D.empty() && !newDepth2D.empty())
if(!oldLaserScan.empty() && !newLaserScan.empty())
{
// 2D
pcl::PointCloud<pcl::PointXYZ>::Ptr oldCloud = util3d::cvMat2Cloud(oldDepth2D);
pcl::PointCloud<pcl::PointXYZ>::Ptr newCloud = util3d::cvMat2Cloud(newDepth2D, currentLink.transform());
pcl::PointCloud<pcl::PointXYZ>::Ptr oldCloud = util3d::cvMat2Cloud(oldLaserScan);
pcl::PointCloud<pcl::PointXYZ>::Ptr newCloud = util3d::cvMat2Cloud(newLaserScan, t);
//voxelize
if(ui_->doubleSpinBox_icp_voxel->value() > 0.0f)
@@ -1846,8 +1898,9 @@ void DatabaseViewer::refineConstraint(int from, int to)
oldCloud,
ui_->doubleSpinBox_icp_maxCorrespDistance->value(),
ui_->spinBox_icp_iteration->value(),
hasConverged,
fitness);
&hasConverged,
&variance,
&correspondences);
}
}
}
@@ -1911,7 +1964,7 @@ void DatabaseViewer::refineConstraint(int from, int to)
{
cloudB = util3d::voxelize<pcl::PointXYZ>(cloudB, ui_->doubleSpinBox_icp_voxel->value());
}
cloudB = util3d::transformPointCloud<pcl::PointXYZ>(cloudB, currentLink.transform() * dataTo.getLocalTransform());
cloudB = util3d::transformPointCloud<pcl::PointXYZ>(cloudB, t * dataTo.getLocalTransform());
}
else
{
@@ -1921,7 +1974,7 @@ void DatabaseViewer::refineConstraint(int from, int to)
ui_->doubleSpinBox_icp_maxDepth->value(),
ui_->doubleSpinBox_icp_voxel->value(),
0, // no sampling
currentLink.transform() * dataTo.getLocalTransform());
t * dataTo.getLocalTransform());
}
if(ui_->checkBox_icp_p2plane->isChecked())
@@ -1945,8 +1998,9 @@ void DatabaseViewer::refineConstraint(int from, int to)
cloudANormals,
ui_->doubleSpinBox_icp_maxCorrespDistance->value(),
ui_->spinBox_icp_iteration->value(),
hasConverged,
fitness);
&hasConverged,
&variance,
&correspondences);
}
else
{
@@ -1954,15 +2008,15 @@ void DatabaseViewer::refineConstraint(int from, int to)
cloudA,
ui_->doubleSpinBox_icp_maxCorrespDistance->value(),
ui_->spinBox_icp_iteration->value(),
hasConverged,
fitness);
&hasConverged,
&variance,
&correspondences);
}
}
if(hasConverged && !transform.isNull())
{
ui_->label_fitness->setNum(fitness);
Link newLink(currentLink.from(), currentLink.to(), transform*currentLink.transform(), currentLink.type());
Link newLink(currentLink.from(), currentLink.to(), currentLink.type(), transform*t, variance);
bool updated = false;
std::multimap<int, Link>::iterator iter = linksRefined_.find(currentLink.from());
@@ -1980,27 +2034,29 @@ void DatabaseViewer::refineConstraint(int from, int to)
if(!updated)
{
linksRefined_.insert(std::make_pair<int, Link>(newLink.from(), newLink));
if(updateGraph)
{
this->updateGraphView();
}
}
if(ui_->dockWidget_constraints->isVisible())
{
cloudB = util3d::transformPointCloud<pcl::PointXYZ>(cloudB, transform);
this->updateConstraintView(newLink, cloudA, cloudB);
}
}
else
{
ui_->label_fitness->setText("not converged");
}
}
void DatabaseViewer::refineConstraintVisually()
{
int from = ids_.at(ui_->horizontalSlider_A->value());
int to = ids_.at(ui_->horizontalSlider_B->value());
refineConstraintVisually(from, to);
refineConstraintVisually(from, to, true);
}
void DatabaseViewer::refineConstraintVisually(int from, int to)
void DatabaseViewer::refineConstraintVisually(int from, int to, bool updateGraph)
{
if(from == to)
{
@@ -2017,6 +2073,8 @@ void DatabaseViewer::refineConstraintVisually(int from, int to)
Transform t;
std::string rejectedMsg;
double variance = -1.0;
int inliers = -1;
if(ui_->checkBox_visual_recomputeFeatures->isChecked())
{
// create a fake memory to regenerate features
@@ -2048,7 +2106,7 @@ void DatabaseViewer::refineConstraintVisually(int from, int to)
}
t = tmpMemory.computeVisualTransform(to, from, &rejectedMsg);
t = tmpMemory.computeVisualTransform(to, from, &rejectedMsg, &inliers, &variance);
}
else
{
@@ -2058,12 +2116,12 @@ void DatabaseViewer::refineConstraintVisually(int from, int to)
parameters.insert(ParametersPair(Parameters::kLccBowIterations(), uNumber2Str(ui_->spinBox_visual_iteration->value())));
parameters.insert(ParametersPair(Parameters::kLccBowMinInliers(), uNumber2Str(ui_->spinBox_visual_minCorrespondences->value())));
memory_->parseParameters(parameters);
t = memory_->computeVisualTransform(to, from, &rejectedMsg);
t = memory_->computeVisualTransform(to, from, &rejectedMsg, &inliers, &variance);
}
if(!t.isNull())
{
Link newLink(currentLink.from(), currentLink.to(), t, currentLink.type());
Link newLink(currentLink.from(), currentLink.to(), currentLink.type(), t, variance);
bool updated = false;
std::multimap<int, Link>::iterator iter = linksRefined_.find(currentLink.from());
@@ -2081,6 +2139,11 @@ void DatabaseViewer::refineConstraintVisually(int from, int to)
if(!updated)
{
linksRefined_.insert(std::make_pair<int, Link>(newLink.from(), newLink));
if(updateGraph)
{
this->updateGraphView();
}
}
if(ui_->dockWidget_constraints->isVisible())
{
@@ -2093,10 +2156,10 @@ void DatabaseViewer::addConstraint()
{
int from = ids_.at(ui_->horizontalSlider_A->value());
int to = ids_.at(ui_->horizontalSlider_B->value());
addConstraint(from, to, false);
addConstraint(from, to, false, true);
}
bool DatabaseViewer::addConstraint(int from, int to, bool silent)
bool DatabaseViewer::addConstraint(int from, int to, bool silent, bool updateGraph)
{
if(from < to)
{
@@ -2120,6 +2183,8 @@ bool DatabaseViewer::addConstraint(int from, int to, bool silent)
Transform t;
std::string rejectedMsg;
double variance = -1.0;
int inliers = -1;
if(ui_->checkBox_visual_recomputeFeatures->isChecked())
{
// create a fake memory to regenerate features
@@ -2151,7 +2216,7 @@ bool DatabaseViewer::addConstraint(int from, int to, bool silent)
}
t = tmpMemory.computeVisualTransform(to, from, &rejectedMsg);
t = tmpMemory.computeVisualTransform(to, from, &rejectedMsg, &inliers, &variance);
}
else
{
@@ -2161,7 +2226,7 @@ bool DatabaseViewer::addConstraint(int from, int to, bool silent)
parameters.insert(ParametersPair(Parameters::kLccBowIterations(), uNumber2Str(ui_->spinBox_visual_iteration->value())));
parameters.insert(ParametersPair(Parameters::kLccBowMinInliers(), uNumber2Str(ui_->spinBox_visual_minCorrespondences->value())));
memory_->parseParameters(parameters);
t = memory_->computeVisualTransform(to, from, &rejectedMsg);
t = memory_->computeVisualTransform(to, from, &rejectedMsg, &inliers, &variance);
}
if(t.isNull())
@@ -2184,7 +2249,7 @@ bool DatabaseViewer::addConstraint(int from, int to, bool silent)
}
// transform is valid, make a link
linksAdded_.insert(std::make_pair(from, Link(from, to, t, Link::kUserClosure)));
linksAdded_.insert(std::make_pair(from, Link(from, to, Link::kUserClosure, t, variance)));
updateSlider = true;
}
}
@@ -2198,6 +2263,10 @@ bool DatabaseViewer::addConstraint(int from, int to, bool silent)
if(updateSlider)
{
updateLoopClosuresSlider(from, to);
if(updateGraph)
{
this->updateGraphView();
}
}
return updateSlider;
}
@@ -2224,6 +2293,7 @@ void DatabaseViewer::resetConstraint()
if(iter != linksRefined_.end())
{
linksRefined_.erase(iter);
this->updateGraphView();
}
iter = util3d::findLink(links_, from, to);
@@ -2255,6 +2325,8 @@ void DatabaseViewer::rejectConstraint()
return;
}
bool removed = false;
// find the original one
std::multimap<int, Link>::iterator iter;
iter = util3d::findLink(links_, from, to);
@@ -2266,6 +2338,7 @@ void DatabaseViewer::rejectConstraint()
return;
}
linksRemoved_.insert(*iter);
removed = true;
}
// remove from refined and added
@@ -2273,11 +2346,17 @@ void DatabaseViewer::rejectConstraint()
if(iter != linksRefined_.end())
{
linksRefined_.erase(iter);
removed = true;
}
iter = util3d::findLink(linksAdded_, from, to);
if(iter != linksAdded_.end())
{
linksAdded_.erase(iter);
removed = true;
}
if(removed)
{
this->updateGraphView();
}
updateLoopClosuresSlider();
}

4
guilib/src/LoopClosureViewer.cpp
View File

@@ -169,8 +169,8 @@ void LoopClosureViewer::updateView(const Transform & transform)
cloudB = util3d::transformPointCloud<pcl::PointXYZRGB>(cloudB, t*sB_.getLocalTransform());
//cloud 2d
pcl::PointCloud<pcl::PointXYZ>::Ptr scanA, scanB;
scanA = util3d::depth2DToPointCloud(sA_.getDepth2DRaw());
pcl::PointCloud<pcl::PointXYZ>::Ptr scanA, scanB;
scanA = util3d::laserScanToPointCloud(sA_.getLaserScanRaw());
scanB = util3d::laserScanToPointCloud(sB_.getLaserScanRaw());
scanB = util3d::transformPointCloud<pcl::PointXYZ>(scanB, t);

111
guilib/src/MainWindow.cpp
View File

@@ -358,7 +358,8 @@ MainWindow::MainWindow(PreferencesDialog * prefDialog, QWidget * parent) :
connect(this, SIGNAL(statsReceived(rtabmap::Statistics)), this, SLOT(processStats(rtabmap::Statistics)));
qRegisterMetaType<rtabmap::SensorData>("rtabmap::SensorData");
connect(this, SIGNAL(odometryReceived(rtabmap::SensorData, int, float, int, int)), this, SLOT(processOdometry(rtabmap::SensorData, int, float, int, int)));
qRegisterMetaType<rtabmap::OdometryInfo>("rtabmap::OdometryInfo");
connect(this, SIGNAL(odometryReceived(rtabmap::SensorData, rtabmap::OdometryInfo)), this, SLOT(processOdometry(rtabmap::SensorData, rtabmap::OdometryInfo)));
connect(this, SIGNAL(noMoreImagesReceived()), this, SLOT(stopDetection()));
@@ -570,7 +571,7 @@ void MainWindow::handleEvent(UEvent* anEvent)
!_processingStatistics)
{
_lastOdometryProcessed = false; // if we receive too many odometry events!
emit odometryReceived(odomEvent->data(), odomEvent->quality(), odomEvent->time(), odomEvent->features(), odomEvent->localMapSize());
emit odometryReceived(odomEvent->data(), odomEvent->info());
}
}
else if(anEvent->getClassName().compare("ULogEvent") == 0)
@@ -580,7 +581,7 @@ void MainWindow::handleEvent(UEvent* anEvent)
{
QMetaObject::invokeMethod(_ui->dockWidget_console, "show");
// The timer prevents multiple calls to pauseDetection() before the state can be changed
if(_state != kPaused && _logEventTime->elapsed() > 1000)
if(_state != kPaused && _state != kMonitoringPaused && _logEventTime->elapsed() > 1000)
{
_logEventTime->start();
if(_preferencesDialog->beepOnPause())
@@ -593,7 +594,7 @@ void MainWindow::handleEvent(UEvent* anEvent)
}
}
void MainWindow::processOdometry(const rtabmap::SensorData & data, int quality, float time, int features, int localMapSize)
void MainWindow::processOdometry(const rtabmap::SensorData & data, const rtabmap::OdometryInfo & info)
{
Transform pose = data.pose();
bool lost = false;
@@ -607,11 +608,11 @@ void MainWindow::processOdometry(const rtabmap::SensorData & data, int quality,
pose = _lastOdomPose;
lost = true;
}
else if(quality>=0 &&
else if(info.inliers>=0 &&
_preferencesDialog->getOdomQualityWarnThr() &&
quality < _preferencesDialog->getOdomQualityWarnThr())
info.inliers < _preferencesDialog->getOdomQualityWarnThr())
{
UDEBUG("odom warn, quality=%d thr=%d", quality, _preferencesDialog->getOdomQualityWarnThr());
UDEBUG("odom warn, quality(inliers)=%d thr=%d", info.inliers, _preferencesDialog->getOdomQualityWarnThr());
_ui->widget_cloudViewer->setBackgroundColor(Qt::darkYellow);
_ui->imageView_odometry->setBackgroundBrush(QBrush(Qt::darkYellow));
}
@@ -621,22 +622,42 @@ void MainWindow::processOdometry(const rtabmap::SensorData & data, int quality,
_ui->widget_cloudViewer->setBackgroundColor(Qt::black);
_ui->imageView_odometry->setBackgroundBrush(QBrush(Qt::black));
}
if(quality >= 0)
if(info.inliers >= 0)
{
_ui->statsToolBox->updateStat("Odometry/Inliers/", (float)data.id(), (float)quality);
_ui->statsToolBox->updateStat("Odometry/Inliers/", (float)data.id(), (float)info.inliers);
}
if(time > 0)
if(info.matches >= 0)
{
_ui->statsToolBox->updateStat("Odometry/Time/ms", (float)data.id(), (float)time*1000.0f);
_ui->statsToolBox->updateStat("Odometry/Matches/", (float)data.id(), (float)info.matches);
}
if(features >=0)
if(info.variance >= 0)
{
_ui->statsToolBox->updateStat("Odometry/Features/", (float)data.id(), (float)features);
_ui->statsToolBox->updateStat("Odometry/StdDev/", (float)data.id(), sqrt((float)info.variance));
}
if(localMapSize >=0)
if(info.variance >= 0)
{
_ui->statsToolBox->updateStat("Odometry/LocalMapSize/", (float)data.id(), (float)localMapSize);
_ui->statsToolBox->updateStat("Odometry/Variance/", (float)data.id(), (float)info.variance);
}
if(info.time > 0)
{
_ui->statsToolBox->updateStat("Odometry/Time/ms", (float)data.id(), (float)info.time*1000.0f);
}
if(info.features >=0)
{
_ui->statsToolBox->updateStat("Odometry/Features/", (float)data.id(), (float)info.features);
}
if(info.localMapSize >=0)
{
_ui->statsToolBox->updateStat("Odometry/Local_map_size/", (float)data.id(), (float)info.localMapSize);
}
float x,y,z, roll,pitch,yaw;
pose.getTranslationAndEulerAngles(x,y,z,roll,pitch,yaw);
_ui->statsToolBox->updateStat("Odometry/T_x/m", (float)data.id(), x);
_ui->statsToolBox->updateStat("Odometry/T_y/m", (float)data.id(), y);
_ui->statsToolBox->updateStat("Odometry/T_z/m", (float)data.id(), z);
_ui->statsToolBox->updateStat("Odometry/T_roll/deg", (float)data.id(), roll*180.0/CV_PI);
_ui->statsToolBox->updateStat("Odometry/T_pitch/deg", (float)data.id(), pitch*180.0/CV_PI);
_ui->statsToolBox->updateStat("Odometry/T_yaw/deg", (float)data.id(), yaw*180.0/CV_PI);
if(_ui->dockWidget_cloudViewer->isVisible())
{
@@ -677,11 +698,11 @@ void MainWindow::processOdometry(const rtabmap::SensorData & data, int quality,
}
// 2d cloud
if(!data.depth2d().empty() &&
if(!data.laserScan().empty() &&
_preferencesDialog->isScansShown(1))
{
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud;
cloud = util3d::depth2DToPointCloud(data.depth2d());
cloud = util3d::laserScanToPointCloud(data.laserScan());
cloud = util3d::transformPointCloud<pcl::PointXYZ>(cloud, pose);
if(!_ui->widget_cloudViewer->addOrUpdateCloud("scanOdom", cloud, _odometryCorrection))
{
@@ -1039,7 +1060,7 @@ void MainWindow::updateMapCloud(
if(!_ui->actionView_scans->isEnabled() &&
_cachedSignatures.size() &&
!(--_cachedSignatures.end())->getDepth2DCompressed().empty())
!(--_cachedSignatures.end())->getLaserScanCompressed().empty())
{
_ui->actionExport_2D_scans_ply_pcd->setEnabled(true);
_ui->actionExport_2D_Grid_map_bmp_png->setEnabled(true);
@@ -1158,7 +1179,7 @@ void MainWindow::updateMapCloud(
else if(_cachedSignatures.contains(iter->first))
{
QMap<int, Signature>::iterator jter = _cachedSignatures.find(iter->first);
if(!jter->getDepth2DCompressed().empty())
if(!jter->getLaserScanCompressed().empty())
{
this->createAndAddScanToMap(iter->first, iter->second, uValue(mapIds, iter->first, -1));
}
@@ -1441,13 +1462,13 @@ void MainWindow::createAndAddScanToMap(int nodeId, const Transform & pose, int m
return;
}
if(!iter->getDepth2DCompressed().empty())
if(!iter->getLaserScanCompressed().empty())
{
cv::Mat depth2D;
iter->uncompressData(0, 0, &depth2D);
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud;
cloud = util3d::depth2DToPointCloud(depth2D);
cloud = util3d::laserScanToPointCloud(depth2D);
QColor color = Qt::red;
if(mapId >= 0)
{
@@ -2342,6 +2363,7 @@ void MainWindow::startDetection()
}
return;
}
if(_odomThread)
{
UEventsManager::createPipe(_dbReader, _odomThread, "CameraEvent");
@@ -2746,9 +2768,11 @@ void MainWindow::postProcessing()
_initProgressDialog->show();
ParametersMap parameters = _preferencesDialog->getAllParameters();
int toroIterations = 100;
bool toroOptimizeFromGraphEnd = false;
int toroIterations = Parameters::defaultRGBDToroIterations();
bool ignoreVariance = Parameters::defaultRGBDToroIgnoreVariance();
bool toroOptimizeFromGraphEnd = Parameters::defaultRGBDOptimizeFromGraphEnd();
Parameters::parse(parameters, Parameters::kRGBDToroIterations(), toroIterations);
Parameters::parse(parameters, Parameters::kRGBDToroIgnoreVariance(), ignoreVariance);
Parameters::parse(parameters, Parameters::kRGBDOptimizeFromGraphEnd(), toroOptimizeFromGraphEnd);
int loopClosuresAdded = 0;
@@ -2819,7 +2843,8 @@ void MainWindow::postProcessing()
Transform transform;
std::string rejectedMsg;
int inliers;
int inliers = -1;
double variance = -1.0;
if(reextractFeatures)
{
memory.init("", true); // clear previously added signatures
@@ -2846,7 +2871,7 @@ void MainWindow::postProcessing()
memory.update(dataTo);
}
transform = memory.computeVisualTransform(dataTo.id(), dataFrom.id(), &rejectedMsg, &inliers);
transform = memory.computeVisualTransform(dataTo.id(), dataFrom.id(), &rejectedMsg, &inliers, &variance);
}
else
{
@@ -2855,14 +2880,14 @@ void MainWindow::postProcessing()
}
else
{
transform = memory.computeVisualTransform(signatureTo, signatureFrom, &rejectedMsg, &inliers);
transform = memory.computeVisualTransform(signatureTo, signatureFrom, &rejectedMsg, &inliers, &variance);
}
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)));
_currentLinksMap.insert(std::make_pair(from, Link(from, to, Link::kUserClosure, transform, variance)));
++loopClosuresAdded;
_initProgressDialog->appendText(tr("Detected loop closure %1->%2! (%3/%4)").arg(from).arg(to).arg(i+1).arg(clusters.size()));
}
@@ -2882,7 +2907,7 @@ void MainWindow::postProcessing()
.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);
util3d::optimizeTOROGraph(depthGraph, odomPoses, _currentLinksMap, optimizedPoses, toroIterations, true, ignoreVariance);
_currentPosesMap = optimizedPoses;
_initProgressDialog->appendText(tr("Optimizing graph with new links... done!"));
}
@@ -2903,14 +2928,14 @@ void MainWindow::postProcessing()
float maxDepth=2.0f;
float voxelSize=0.01f;
int samples = 0;
float minFitness = 1.0f;
float maxCorrespondences = 0.05f;
float correspondenceRatio = 0.7f;
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::kLccIcp3CorrespondenceRatio(), correspondenceRatio);
Parameters::parse(parameters, Parameters::kLccIcp3MaxCorrespondenceDistance(), maxCorrespondences);
Parameters::parse(parameters, Parameters::kLccIcp3Iterations(), icpIterations);
bool pointToPlane = false;
@@ -2974,7 +2999,8 @@ void MainWindow::postProcessing()
iter->second.transform() * signatureTo.getLocalTransform());
bool hasConverged = false;
double fitness = -1;
double variance = -1;
int correspondences = 0;
Transform transform;
if(pointToPlane)
{
@@ -2997,8 +3023,9 @@ void MainWindow::postProcessing()
cloudANormals,
maxCorrespondences,
icpIterations,
hasConverged,
fitness);
&hasConverged,
&variance,
&correspondences);
}
else
{
@@ -3006,18 +3033,22 @@ void MainWindow::postProcessing()
cloudA,
maxCorrespondences,
icpIterations,
hasConverged,
fitness);
&hasConverged,
&variance,
&correspondences);
}
if(hasConverged && !transform.isNull() && fitness>=0.0f && fitness <= minFitness)
float correspondencesRatio = float(correspondences)/float(cloudB->size()>cloudA->size()?cloudB->size():cloudA->size());
if(!transform.isNull() && hasConverged &&
correspondencesRatio >= correspondenceRatio)
{
Link newLink(from, to, transform*iter->second.transform(), iter->second.type());
Link newLink(from, to, iter->second.type(), transform*iter->second.transform(), variance);
iter->second = newLink;
}
else
{
UWARN("Cannot refine link %d->%d (converged=%s fitness=%f)", from, to, hasConverged?"true":"false", fitness);
UWARN("Cannot refine link %d->%d (converged=%s variance=%f)", from, to, hasConverged?"true":"false", variance);
}
}
}
@@ -3029,7 +3060,7 @@ void MainWindow::postProcessing()
.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);
util3d::optimizeTOROGraph(depthGraph, odomPoses, _currentLinksMap, optimizedPoses, toroIterations, true, ignoreVariance);
_initProgressDialog->appendText(tr("Optimizing graph with updated links... done!"));
_initProgressDialog->incrementStep();
@@ -4698,7 +4729,7 @@ void MainWindow::changeState(MainWindow::State newState)
_ui->statusbar->showMessage(tr("Paused..."));
_ui->actionDump_the_memory->setEnabled(true);
_ui->actionDump_the_prediction_matrix->setEnabled(true);
_ui->actionDelete_memory->setEnabled(true);
_ui->actionDelete_memory->setEnabled(false);
_ui->actionGenerate_map->setEnabled(true);
_ui->actionGenerate_local_map->setEnabled(true);
_ui->actionGenerate_TORO_graph_graph->setEnabled(true);

2
guilib/src/OdometryViewer.cpp
View File

@@ -187,7 +187,7 @@ void OdometryViewer::handleEvent(UEvent * event)
{
data_.back() = odomEvent->data();
}
dataQuality_ = odomEvent->quality();
dataQuality_ = odomEvent->info().inliers;
dataMutex_.unlock();
if(empty)
{

4
guilib/src/PreferencesDialog.cpp
View File

@@ -440,6 +440,7 @@ PreferencesDialog::PreferencesDialog(QWidget * parent) :
_ui->rgdb_newMapOdomChange->setObjectName(Parameters::kRGBDNewMapOdomChangeDistance().c_str());
_ui->odomScanHistory->setObjectName(Parameters::kRGBDPoseScanMatching().c_str());
_ui->globalDetection_toroIterations->setObjectName(Parameters::kRGBDToroIterations().c_str());
_ui->globalDetection_toroIgnoreVariance->setObjectName(Parameters::kRGBDToroIgnoreVariance().c_str());
_ui->globalDetection_optimizeFromGraphEnd->setObjectName(Parameters::kRGBDOptimizeFromGraphEnd().c_str());
_ui->groupBox_localDetection_time->setObjectName(Parameters::kRGBDLocalLoopDetectionTime().c_str());
@@ -469,13 +470,12 @@ PreferencesDialog::PreferencesDialog(QWidget * parent) :
_ui->loopClosure_icpSamples->setObjectName(Parameters::kLccIcp3Samples().c_str());
_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_icpRatio->setObjectName(Parameters::kLccIcp3CorrespondenceRatio().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());
_ui->loopClosure_icp2MaxFitness->setObjectName(Parameters::kLccIcp2MaxFitness().c_str());
_ui->loopClosure_icp2Ratio->setObjectName(Parameters::kLccIcp2CorrespondenceRatio().c_str());
_ui->loopClosure_icp2Voxel->setObjectName(Parameters::kLccIcp2VoxelSize().c_str());

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

@@ -7,7 +7,7 @@
<x>0</x>
<y>0</y>
<width>1187</width>
<height>851</height>
<height>862</height>
</rect>
</property>
<property name="windowTitle">
@@ -354,13 +354,6 @@
</property>
</widget>
</item>
<item row="2" column="1">
<widget class="QLabel" name="label_constraint">
<property name="text">
<string/>
</property>
</widget>
</item>
<item row="2" column="0">
<widget class="QLabel" name="label_16">
<property name="text">
@@ -368,17 +361,24 @@
</property>
</widget>
</item>
<item row="3" column="0">
<widget class="QLabel" name="label_18">
<item row="2" column="1">
<widget class="QLabel" name="label_constraint">
<property name="text">
<string>Fitness</string>
<string/>
</property>
</widget>
</item>
<item row="3" column="0">
<widget class="QCheckBox" name="checkBox_showOptimized">
<property name="text">
<string>Optimized</string>
</property>
</widget>
</item>
<item row="3" column="1">
<widget class="QLabel" name="label_fitness">
<widget class="QLabel" name="label_constraint_opt">
<property name="text">
<string>-</string>
<string/>
</property>
</widget>
</item>
@@ -511,14 +511,14 @@
</property>
</widget>
</item>
<item row="2" column="0">
<item row="3" column="0">
<widget class="QLabel" name="label_8">
<property name="text">
<string>Optimize from:</string>
</property>
</widget>
</item>
<item row="2" column="1">
<item row="3" column="1">
<widget class="QSpinBox" name="spinBox_optimizationsFrom"/>
</item>
<item row="1" column="1">
@@ -538,20 +538,37 @@
</property>
</widget>
</item>
<item row="3" column="0">
<item row="4" column="0">
<widget class="QLabel" name="label_10">
<property name="text">
<string>Total path length (m):</string>
</property>
</widget>
</item>
<item row="3" column="1">
<item row="4" column="1">
<widget class="QLabel" name="label_pathLength">
<property name="text">
<string/>
</property>
</widget>
</item>
<item row="2" column="0">
<widget class="QLabel" name="label_34">
<property name="text">
<string>Ignore covariance:</string>
</property>
</widget>
</item>
<item row="2" column="1">
<widget class="QCheckBox" name="checkBox_ignoreCovariance">
<property name="text">
<string/>
</property>
<property name="checked">
<bool>false</bool>
</property>
</widget>
</item>
</layout>
</item>
</layout>

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

@@ -63,9 +63,9 @@
<property name="geometry">
<rect>
<x>0</x>
<y>0</y>
<y>-305</y>
<width>744</width>
<height>1074</height>
<height>1252</height>
</rect>
</property>
<layout class="QVBoxLayout" name="verticalLayout_16">
@@ -86,7 +86,7 @@
<enum>QFrame::Raised</enum>
</property>
<property name="currentIndex">
<number>1</number>
<number>19</number>
</property>
<widget class="QWidget" name="page_22">
<layout class="QVBoxLayout" name="verticalLayout_29">
@@ -5174,7 +5174,7 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag
</property>
</widget>
</item>
<item row="1" column="1">
<item row="2" column="1">
<widget class="QLabel" name="label_151">
<property name="text">
<string>Optimize graph from the newest node.
@@ -5187,13 +5187,30 @@ Warning when set to false: when some nodes are transferred, the first referentia
</property>
</widget>
</item>
<item row="1" column="0">
<item row="2" column="0">
<widget class="QCheckBox" name="globalDetection_optimizeFromGraphEnd">
<property name="text">
<string/>
</property>
</widget>
</item>
<item row="1" column="0">
<widget class="QCheckBox" name="globalDetection_toroIgnoreVariance">
<property name="text">
<string/>
</property>
</widget>
</item>
<item row="1" column="1">
<widget class="QLabel" name="label_141">
<property name="text">
<string>Ignore constraints' variance. If checked, identity information matrix is used for each constraint in TORO. Otherwise, an information matrix is generated from the variance saved in the links.</string>
</property>
<property name="wordWrap">
<bool>true</bool>
</property>
</widget>
</item>
</layout>
</widget>
</item>
@@ -5209,7 +5226,7 @@ Warning when set to false: when some nodes are transferred, the first referentia
<item>
<widget class="QLabel" name="label_54">
<property name="text">
<string>Activate local detection over all locations in STM. The Bayes filter is not used here, so it may results in more false detections. The same 2-steps technique as for global loop closure constraints is used here.</string>
<string>Activate local detection over all locations in STM. The Bayes filter is not used here: If there are enough correspondences between the current image and others in STM, transformations are computed. The same 2-steps technique as for global loop closure constraints is used here. This generates more constraints in the map's graph, so more time is required to optimize the graph.</string>
</property>
<property name="wordWrap">
<bool>true</bool>
@@ -5837,19 +5854,25 @@ Lower the ratio -&gt; higher the precision. 0 means disabled, matching the neare
</widget>
</item>
<item row="6" column="0">
<widget class="QDoubleSpinBox" name="loopClosure_icpMaxFitness">
<widget class="QDoubleSpinBox" name="loopClosure_icpRatio">
<property name="minimum">
<double>0.000000000000000</double>
</property>
<property name="maximum">
<double>1.000000000000000</double>
</property>
<property name="singleStep">
<double>0.010000000000000</double>
</property>
<property name="value">
<double>30.000000000000000</double>
<double>0.700000000000000</double>
</property>
</widget>
</item>
<item row="6" column="1">
<widget class="QLabel" name="label_133">
<property name="text">
<string>Maximum fitness to accept the computed transform.</string>
<string>Ratio of matching correspondences to accept the transform.</string>
</property>
<property name="wordWrap">
<bool>true</bool>
@@ -5955,26 +5978,6 @@ Lower the ratio -&gt; higher the precision. 0 means disabled, matching the neare
</widget>
</item>
<item row="2" column="0">
<widget class="QDoubleSpinBox" name="loopClosure_icp2MaxFitness">
<property name="minimum">
<double>0.010000000000000</double>
</property>
<property name="value">
<double>30.000000000000000</double>
</property>
</widget>
</item>
<item row="2" column="1">
<widget class="QLabel" name="label_141">
<property name="text">
<string>Maximum fitness to accept the computed transform.</string>
</property>
<property name="wordWrap">
<bool>true</bool>
</property>
</widget>
</item>
<item row="3" column="0">
<widget class="QDoubleSpinBox" name="loopClosure_icp2Ratio">
<property name="minimum">
<double>0.000000000000000</double>
@@ -5986,11 +5989,11 @@ Lower the ratio -&gt; higher the precision. 0 means disabled, matching the neare
<double>0.010000000000000</double>
</property>
<property name="value">
<double>0.900000000000000</double>
<double>0.700000000000000</double>
</property>
</widget>
</item>
<item row="3" column="1">
<item row="2" column="1">
<widget class="QLabel" name="label_148">
<property name="text">
<string>Ratio of matching correspondences to accept the transform.</string>
@@ -6000,17 +6003,7 @@ Lower the ratio -&gt; higher the precision. 0 means disabled, matching the neare
</property>
</widget>
</item>
<item row="4" column="1">
<widget class="QLabel" name="label_150">
<property name="text">
<string>Voxel size. Set to 0 to disable voxel filtering.</string>
</property>
<property name="wordWrap">
<bool>true</bool>
</property>
</widget>
</item>
<item row="4" column="0">
<item row="3" column="0">
<widget class="QDoubleSpinBox" name="loopClosure_icp2Voxel">
<property name="suffix">
<string> m</string>
@@ -6032,6 +6025,16 @@ Lower the ratio -&gt; higher the precision. 0 means disabled, matching the neare
</property>
</widget>
</item>
<item row="3" column="1">
<widget class="QLabel" name="label_150">
<property name="text">
<string>Voxel size. Set to 0 to disable voxel filtering.</string>
</property>
<property name="wordWrap">
<bool>true</bool>
</property>
</widget>
</item>
</layout>
</widget>
</item>

29
guilib/src/utilite/UPlot.cpp
View File

@@ -19,6 +19,7 @@
#include "UPlot.h"
#include "rtabmap/utilite/ULogger.h"
#include "rtabmap/utilite/UMath.h"
#include <QtGui/QGraphicsScene>
#include <QtGui/QGraphicsView>
@@ -1341,6 +1342,8 @@ UPlotLegendItem::UPlotLegendItem(UPlotCurve * curve, QWidget * parent) :
_aResetText = new QAction(tr("Reset text..."), this);
_aChangeColor = new QAction(tr("Change color..."), this);
_aCopyToClipboard = new QAction(tr("Copy curve data to the clipboard"), this);
_aShowStdDev = new QAction(tr("Show std deviation"), this);
_aShowStdDev->setCheckable(true);
_aMoveUp = new QAction(tr("Move up"), this);
_aMoveDown = new QAction(tr("Move down"), this);
_aRemoveCurve = new QAction(tr("Remove this curve"), this);
@@ -1349,6 +1352,7 @@ UPlotLegendItem::UPlotLegendItem(UPlotCurve * curve, QWidget * parent) :
_menu->addAction(_aResetText);
_menu->addAction(_aChangeColor);
_menu->addAction(_aCopyToClipboard);
_menu->addAction(_aShowStdDev);
_menu->addSeparator();
_menu->addAction(_aMoveUp);
_menu->addAction(_aMoveDown);
@@ -1416,6 +1420,20 @@ void UPlotLegendItem::contextMenuEvent(QContextMenuEvent * event)
clipboard->setText((textX+"\n")+textY);
}
}
else if(action == _aShowStdDev)
{
if(_aShowStdDev->isChecked())
{
connect(_curve, SIGNAL(dataChanged(const UPlotCurve *)), this, SLOT(updateStdDev()));
}
else
{
disconnect(_curve, SIGNAL(dataChanged(const UPlotCurve *)), this, SLOT(updateStdDev()));
QString nameSpaced = _curve->name();
nameSpaced.replace('_', ' ');
this->setText(nameSpaced);
}
}
else if(action == _aRemoveCurve)
{
emit legendItemRemoved(_curve);
@@ -1442,6 +1460,17 @@ QPixmap UPlotLegendItem::createSymbol(const QPen & pen, const QBrush & brush)
return pixmap;
}
void UPlotLegendItem::updateStdDev()
{
QVector<float> x, y;
_curve->getData(x, y);
float stdDev = std::sqrt(uVariance(y.data(), y.size()));
QString nameSpaced = _curve->name();
nameSpaced.replace('_', ' ');
nameSpaced += QString(" (%1=%2)").arg(QChar(0xc3, 0x03)).arg(stdDev);
this->setText(nameSpaced);
}

4
guilib/src/utilite/UPlot.h
View File

@@ -356,6 +356,9 @@ signals:
void moveUpRequest(UPlotLegendItem *);
void moveDownRequest(UPlotLegendItem *);
private slots:
void updateStdDev();
protected:
virtual void contextMenuEvent(QContextMenuEvent * event);
@@ -366,6 +369,7 @@ private:
QAction * _aResetText;
QAction * _aChangeColor;
QAction * _aCopyToClipboard;
QAction * _aShowStdDev;
QAction * _aRemoveCurve;
QAction * _aMoveUp;
QAction * _aMoveDown;