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

Rtabmap::detectMoreLoopClosures: added clusterRadiusMin parameter and update optimized poses after each accepted loop closure (also in MainWindow) like in DbViewer. Added graph::computeMinMax(poses). OdometryInfo: added guess transform. Export: added min/max axis ranges to filter nodes before expoting clouds.

Browse Source
This commit is contained in:
matlabbe
2021-03-13 18:42:13 -05:00
parent f6e17be2b4
commit 752509fb15
14 changed files with 815 additions and 326 deletions
Download Patch File Download Diff File Expand all files Collapse all files

1
guilib/include/rtabmap/gui/ExportCloudsDialog.h
View File

@@ -131,6 +131,7 @@ private Q_SLOTS:
void cancel();
private:
std::map<int, Transform> filterNodes(const std::map<int, Transform> & poses);
std::map<int, std::pair<pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr, pcl::IndicesPtr> > getClouds(
const std::map<int, Transform> & poses,
const QMap<int, Signature> & cachedSignatures,

76
guilib/src/ExportCloudsDialog.cpp
View File

@@ -110,6 +110,15 @@ ExportCloudsDialog::ExportCloudsDialog(QWidget *parent) :
connect(_ui->comboBox_meshingApproach, SIGNAL(currentIndexChanged(int)), this, SIGNAL(configChanged()));
connect(_ui->comboBox_meshingApproach, SIGNAL(currentIndexChanged(int)), this, SLOT(updateReconstructionFlavor()));
connect(_ui->checkBox_nodes_filtering, SIGNAL(stateChanged(int)), this, SIGNAL(configChanged()));
connect(_ui->checkBox_nodes_filtering, SIGNAL(stateChanged(int)), this, SLOT(updateReconstructionFlavor()));
connect(_ui->doubleSpinBox_nodes_filtering_xmin, SIGNAL(valueChanged(double)), this, SIGNAL(configChanged()));
connect(_ui->doubleSpinBox_nodes_filtering_xmax, SIGNAL(valueChanged(double)), this, SIGNAL(configChanged()));
connect(_ui->doubleSpinBox_nodes_filtering_ymin, SIGNAL(valueChanged(double)), this, SIGNAL(configChanged()));
connect(_ui->doubleSpinBox_nodes_filtering_ymax, SIGNAL(valueChanged(double)), this, SIGNAL(configChanged()));
connect(_ui->doubleSpinBox_nodes_filtering_zmin, SIGNAL(valueChanged(double)), this, SIGNAL(configChanged()));
connect(_ui->doubleSpinBox_nodes_filtering_zmax, SIGNAL(valueChanged(double)), this, SIGNAL(configChanged()));
connect(_ui->checkBox_regenerate, SIGNAL(stateChanged(int)), this, SIGNAL(configChanged()));
connect(_ui->checkBox_regenerate, SIGNAL(stateChanged(int)), this, SLOT(updateReconstructionFlavor()));
connect(_ui->spinBox_decimation, SIGNAL(valueChanged(int)), this, SIGNAL(configChanged()));
@@ -325,6 +334,14 @@ void ExportCloudsDialog::saveSettings(QSettings & settings, const QString & grou
settings.setValue("normals_radius", _ui->doubleSpinBox_normalRadiusSearch->value());
settings.setValue("intensity_colormap", _ui->comboBox_intensityColormap->currentIndex());
settings.setValue("nodes_filtering", _ui->checkBox_nodes_filtering->isChecked());
settings.setValue("nodes_filtering_xmin", _ui->doubleSpinBox_nodes_filtering_xmin->value());
settings.setValue("nodes_filtering_xmax", _ui->doubleSpinBox_nodes_filtering_xmax->value());
settings.setValue("nodes_filtering_ymin", _ui->doubleSpinBox_nodes_filtering_ymin->value());
settings.setValue("nodes_filtering_ymax", _ui->doubleSpinBox_nodes_filtering_ymax->value());
settings.setValue("nodes_filtering_zmin", _ui->doubleSpinBox_nodes_filtering_zmin->value());
settings.setValue("nodes_filtering_zmax", _ui->doubleSpinBox_nodes_filtering_zmax->value());
settings.setValue("regenerate", _ui->checkBox_regenerate->isChecked());
settings.setValue("regenerate_decimation", _ui->spinBox_decimation->value());
settings.setValue("regenerate_max_depth", _ui->doubleSpinBox_maxDepth->value());
@@ -474,6 +491,14 @@ void ExportCloudsDialog::loadSettings(QSettings & settings, const QString & grou
_ui->doubleSpinBox_normalRadiusSearch->setValue(settings.value("normals_radius", _ui->doubleSpinBox_normalRadiusSearch->value()).toDouble());
_ui->comboBox_intensityColormap->setCurrentIndex(settings.value("intensity_colormap", _ui->comboBox_intensityColormap->currentIndex()).toInt());
_ui->checkBox_nodes_filtering->setChecked(settings.value("nodes_filtering", _ui->checkBox_nodes_filtering->isChecked()).toBool());
_ui->doubleSpinBox_nodes_filtering_xmin->setValue(settings.value("nodes_filtering_xmin", _ui->doubleSpinBox_nodes_filtering_xmin->value()).toInt());
_ui->doubleSpinBox_nodes_filtering_xmax->setValue(settings.value("nodes_filtering_xmax", _ui->doubleSpinBox_nodes_filtering_xmax->value()).toInt());
_ui->doubleSpinBox_nodes_filtering_ymin->setValue(settings.value("nodes_filtering_ymin", _ui->doubleSpinBox_nodes_filtering_ymin->value()).toInt());
_ui->doubleSpinBox_nodes_filtering_ymax->setValue(settings.value("nodes_filtering_ymax", _ui->doubleSpinBox_nodes_filtering_ymax->value()).toInt());
_ui->doubleSpinBox_nodes_filtering_zmin->setValue(settings.value("nodes_filtering_zmin", _ui->doubleSpinBox_nodes_filtering_zmin->value()).toInt());
_ui->doubleSpinBox_nodes_filtering_zmax->setValue(settings.value("nodes_filtering_zmax", _ui->doubleSpinBox_nodes_filtering_zmax->value()).toInt());
_ui->checkBox_regenerate->setChecked(settings.value("regenerate", _ui->checkBox_regenerate->isChecked()).toBool());
_ui->spinBox_decimation->setValue(settings.value("regenerate_decimation", _ui->spinBox_decimation->value()).toInt());
_ui->doubleSpinBox_maxDepth->setValue(settings.value("regenerate_max_depth", _ui->doubleSpinBox_maxDepth->value()).toDouble());
@@ -626,6 +651,14 @@ void ExportCloudsDialog::restoreDefaults()
_ui->doubleSpinBox_normalRadiusSearch->setValue(0.0);
_ui->comboBox_intensityColormap->setCurrentIndex(0);
_ui->checkBox_nodes_filtering->setChecked(false);
_ui->doubleSpinBox_nodes_filtering_xmin->setValue(0);
_ui->doubleSpinBox_nodes_filtering_xmax->setValue(0);
_ui->doubleSpinBox_nodes_filtering_ymin->setValue(0);
_ui->doubleSpinBox_nodes_filtering_ymax->setValue(0);
_ui->doubleSpinBox_nodes_filtering_zmin->setValue(0);
_ui->doubleSpinBox_nodes_filtering_zmax->setValue(0);
_ui->checkBox_regenerate->setChecked(_dbDriver!=0?true:false);
_ui->spinBox_decimation->setValue(1);
_ui->doubleSpinBox_maxDepth->setValue(4);
@@ -830,6 +863,7 @@ void ExportCloudsDialog::updateReconstructionFlavor()
_ui->checkBox_cameraProjection->setEnabled(_ui->checkBox_assemble->isChecked() && !_ui->checkBox_meshing->isChecked());
_ui->label_cameraProjection->setEnabled(_ui->checkBox_cameraProjection->isEnabled());
_ui->groupBox_nodes_filtering->setVisible(_ui->checkBox_nodes_filtering->isChecked());
_ui->groupBox_regenerate->setVisible(_ui->checkBox_regenerate->isChecked() && _ui->checkBox_fromDepth->isChecked());
_ui->groupBox_regenerateScans->setVisible(_ui->checkBox_regenerate->isChecked() && !_ui->checkBox_fromDepth->isChecked());
_ui->groupBox_bilateral->setVisible(_ui->checkBox_bilateral->isChecked());
@@ -953,6 +987,42 @@ void ExportCloudsDialog::setOkButton()
updateReconstructionFlavor();
}
std::map<int, Transform> ExportCloudsDialog::filterNodes(const std::map<int, Transform> & poses)
{
if(_ui->checkBox_nodes_filtering->isChecked())
{
std::map<int, Transform> posesFiltered;
for(std::map<int, Transform>::const_iterator iter=poses.begin(); iter!=poses.end(); ++iter)
{
bool ignore = false;
if(_ui->doubleSpinBox_nodes_filtering_xmin->value() != _ui->doubleSpinBox_nodes_filtering_xmax->value() &&
(iter->second.x() < _ui->doubleSpinBox_nodes_filtering_xmin->value() ||
iter->second.x() > _ui->doubleSpinBox_nodes_filtering_xmax->value()))
{
ignore = true;
}
if(_ui->doubleSpinBox_nodes_filtering_ymin->value() != _ui->doubleSpinBox_nodes_filtering_ymax->value() &&
(iter->second.y() < _ui->doubleSpinBox_nodes_filtering_ymin->value() ||
iter->second.y() > _ui->doubleSpinBox_nodes_filtering_ymax->value()))
{
ignore = true;
}
if(_ui->doubleSpinBox_nodes_filtering_zmin->value() != _ui->doubleSpinBox_nodes_filtering_zmax->value() &&
(iter->second.z() < _ui->doubleSpinBox_nodes_filtering_zmin->value() ||
iter->second.z() > _ui->doubleSpinBox_nodes_filtering_zmax->value()))
{
ignore = true;
}
if(!ignore)
{
posesFiltered.insert(*iter);
}
}
return posesFiltered;
}
return poses;
}
void ExportCloudsDialog::exportClouds(
const std::map<int, Transform> & poses,
const std::multimap<int, Link> & links,
@@ -1432,7 +1502,7 @@ bool ExportCloudsDialog::removeDirRecursively(const QString & dirName)
}
bool ExportCloudsDialog::getExportedClouds(
const std::map<int, Transform> & poses,
const std::map<int, Transform> & posesIn,
const std::multimap<int, Link> & links,
const std::map<int, int> & mapIds,
const QMap<int, Signature> & cachedSignatures,
@@ -1460,9 +1530,11 @@ bool ExportCloudsDialog::getExportedClouds(
}
if(this->exec() == QDialog::Accepted)
{
std::map<int, Transform> poses = filterNodes(posesIn);
if(poses.empty())
{
QMessageBox::critical(this, tr("Creating clouds..."), tr("Poses are null! Cannot export/view clouds."));
QMessageBox::critical(this, tr("Creating clouds..."), tr("Poses are empty! Cannot export/view clouds."));
return false;
}
_progressDialog->resetProgress();

357
guilib/src/MainWindow.cpp
View File

@@ -1671,15 +1671,15 @@ void MainWindow::processOdometry(const rtabmap::OdometryEvent & odom, bool dataI
_ui->statsToolBox->updateStat("Odometry/Speed/kph", _preferencesDialog->isTimeUsedInFigures()?odom.data().stamp()-_firstStamp:(float)odom.data().id(), dist/odom.info().interval*3.6f, _preferencesDialog->isCacheSavedInFigures());
_ui->statsToolBox->updateStat("Odometry/Speed/mph", _preferencesDialog->isTimeUsedInFigures()?odom.data().stamp()-_firstStamp:(float)odom.data().id(), dist/odom.info().interval*2.237f, _preferencesDialog->isCacheSavedInFigures());
_ui->statsToolBox->updateStat("Odometry/Speed/mps", _preferencesDialog->isTimeUsedInFigures()?odom.data().stamp()-_firstStamp:(float)odom.data().id(), dist/odom.info().interval, _preferencesDialog->isCacheSavedInFigures());
}
if(!odom.info().guessVelocity.isNull())
{
odom.info().guessVelocity.getTranslationAndEulerAngles(x,y,z,roll,pitch,yaw);
dist = odom.info().guessVelocity.getNorm();
_ui->statsToolBox->updateStat("Odometry/SpeedGuess/kph", _preferencesDialog->isTimeUsedInFigures()?odom.data().stamp()-_firstStamp:(float)odom.data().id(), dist*3.6f, _preferencesDialog->isCacheSavedInFigures());
_ui->statsToolBox->updateStat("Odometry/SpeedGuess/mph", _preferencesDialog->isTimeUsedInFigures()?odom.data().stamp()-_firstStamp:(float)odom.data().id(), dist*2.237f, _preferencesDialog->isCacheSavedInFigures());
_ui->statsToolBox->updateStat("Odometry/SpeedGuess/mps", _preferencesDialog->isTimeUsedInFigures()?odom.data().stamp()-_firstStamp:(float)odom.data().id(), dist, _preferencesDialog->isCacheSavedInFigures());
if(!odom.info().guess.isNull())
{
odom.info().guess.getTranslationAndEulerAngles(x,y,z,roll,pitch,yaw);
dist = odom.info().guess.getNorm();
_ui->statsToolBox->updateStat("Odometry/SpeedGuess/kph", _preferencesDialog->isTimeUsedInFigures()?odom.data().stamp()-_firstStamp:(float)odom.data().id(), dist/odom.info().interval*3.6f, _preferencesDialog->isCacheSavedInFigures());
_ui->statsToolBox->updateStat("Odometry/SpeedGuess/mph", _preferencesDialog->isTimeUsedInFigures()?odom.data().stamp()-_firstStamp:(float)odom.data().id(), dist/odom.info().interval*2.237f, _preferencesDialog->isCacheSavedInFigures());
_ui->statsToolBox->updateStat("Odometry/SpeedGuess/mps", _preferencesDialog->isTimeUsedInFigures()?odom.data().stamp()-_firstStamp:(float)odom.data().id(), dist/odom.info().interval, _preferencesDialog->isCacheSavedInFigures());
}
}
if(!odom.info().transformGroundTruth.isNull())
@@ -6029,186 +6029,195 @@ void MainWindow::postProcessing()
addedLinks.find(to) == addedLinks.end() &&
rtabmap::graph::findLink(_currentLinksMap, from, to) == _currentLinksMap.end())
{
checkedLoopClosures.insert(std::make_pair(from, to));
// Reverify if in the bounds with the current optimized graph
Transform delta = _currentPosesMap.at(from).inverse() * _currentPosesMap.at(to);
if(delta.getNorm() < clusterRadius)
{
checkedLoopClosures.insert(std::make_pair(from, to));
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];
if(signatureFrom.getWeight() >= 0 &&
signatureTo.getWeight() >= 0) // ignore intermediate nodes
if(!_cachedSignatures.contains(from))
{
Transform transform;
RegistrationInfo info;
if(parameters.find(Parameters::kRegStrategy()) != parameters.end() &&
parameters.at(Parameters::kRegStrategy()).compare("1") == 0)
{
uInsert(parameters, ParametersPair(Parameters::kRegStrategy(), "2"));
}
Registration * registration = Registration::create(parameters);
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];
if(reextractFeatures)
if(signatureFrom.getWeight() >= 0 &&
signatureTo.getWeight() >= 0) // ignore intermediate nodes
{
signatureFrom.sensorData().uncompressData();
signatureTo.sensorData().uncompressData();
Transform transform;
RegistrationInfo info;
if(parameters.find(Parameters::kRegStrategy()) != parameters.end() &&
parameters.at(Parameters::kRegStrategy()).compare("1") == 0)
{
uInsert(parameters, ParametersPair(Parameters::kRegStrategy(), "2"));
}
Registration * registration = Registration::create(parameters);
if(signatureFrom.sensorData().imageRaw().empty() &&
signatureTo.sensorData().imageRaw().empty())
if(reextractFeatures)
{
UWARN("\"%s\" is false and signatures (%d and %d) don't have raw "
"images. Update the cache.",
Parameters::kRGBDLoopClosureReextractFeatures().c_str());
}
else
{
signatureFrom.removeAllWords();
signatureFrom.sensorData().setFeatures(std::vector<cv::KeyPoint>(), std::vector<cv::Point3f>(), cv::Mat());
signatureTo.removeAllWords();
signatureTo.sensorData().setFeatures(std::vector<cv::KeyPoint>(), std::vector<cv::Point3f>(), cv::Mat());
}
}
else if(!reextractFeatures && signatureFrom.getWords().empty() && signatureTo.getWords().empty())
{
UWARN("\"%s\" is false and signatures (%d and %d) don't have words, "
"registration will not be possible. Set \"%s\" to true.",
Parameters::kRGBDLoopClosureReextractFeatures().c_str(),
signatureFrom.id(),
signatureTo.id(),
Parameters::kRGBDLoopClosureReextractFeatures().c_str());
}
transform = registration->computeTransformation(signatureFrom, signatureTo, Transform(), &info);
delete registration;
if(!transform.isNull())
{
//optimize the graph to see if the new constraint is globally valid
bool updateConstraint = true;
cv::Mat information = info.covariance.inv();
if(odomMaxInf.size() == 6 && information.cols==6 && information.rows==6)
{
for(int i=0; i<6; ++i)
signatureFrom.sensorData().uncompressData();
signatureTo.sensorData().uncompressData();
if(signatureFrom.sensorData().imageRaw().empty() &&
signatureTo.sensorData().imageRaw().empty())
{
if(information.at<double>(i,i) > odomMaxInf[i])
{
information.at<double>(i,i) = odomMaxInf[i];
}
}
}
if(optimizeMaxError > 0.0f && optimizeIterations > 0)
{
int fromId = from;
int mapId = _currentMapIds.at(from);
// use first node of the map containing from
for(std::map<int, int>::iterator iter=_currentMapIds.begin(); iter!=_currentMapIds.end(); ++iter)
{
if(iter->second == mapId && _currentPosesMap.find(iter->first)!=_currentPosesMap.end())
{
fromId = iter->first;
break;
}
}
std::multimap<int, Link> linksIn = _currentLinksMap;
linksIn.insert(std::make_pair(from, Link(from, to, Link::kUserClosure, transform, information)));
const Link * maxLinearLink = 0;
const Link * maxAngularLink = 0;
float maxLinearError = 0.0f;
float maxAngularError = 0.0f;
std::map<int, Transform> poses;
std::multimap<int, Link> links;
UASSERT(_currentPosesMap.find(fromId) != _currentPosesMap.end());
UASSERT_MSG(_currentPosesMap.find(from) != _currentPosesMap.end(), uFormat("id=%d poses=%d links=%d", from, (int)poses.size(), (int)links.size()).c_str());
UASSERT_MSG(_currentPosesMap.find(to) != _currentPosesMap.end(), uFormat("id=%d poses=%d links=%d", to, (int)poses.size(), (int)links.size()).c_str());
optimizer->getConnectedGraph(fromId, _currentPosesMap, linksIn, poses, links);
UASSERT(poses.find(fromId) != poses.end());
UASSERT_MSG(poses.find(from) != poses.end(), uFormat("id=%d poses=%d links=%d", from, (int)poses.size(), (int)links.size()).c_str());
UASSERT_MSG(poses.find(to) != poses.end(), uFormat("id=%d poses=%d links=%d", to, (int)poses.size(), (int)links.size()).c_str());
UASSERT(graph::findLink(links, from, to) != links.end());
poses = optimizer->optimize(fromId, poses, links);
std::string msg;
if(poses.size())
{
float maxLinearErrorRatio = 0.0f;
float maxAngularErrorRatio = 0.0f;
graph::computeMaxGraphErrors(
poses,
links,
maxLinearErrorRatio,
maxAngularErrorRatio,
maxLinearError,
maxAngularError,
&maxLinearLink,
&maxAngularLink);
if(maxLinearLink)
{
UINFO("Max optimization linear error = %f m (link %d->%d)", maxLinearError, maxLinearLink->from(), maxLinearLink->to());
if(maxLinearErrorRatio > optimizeMaxError)
{
msg = uFormat("Rejecting edge %d->%d because "
"graph error is too large after optimization (%f m for edge %d->%d with ratio %f > std=%f m). "
"\"%s\" is %f.",
from,
to,
maxLinearError,
maxLinearLink->from(),
maxLinearLink->to(),
maxLinearErrorRatio,
sqrt(maxLinearLink->transVariance()),
Parameters::kRGBDOptimizeMaxError().c_str(),
optimizeMaxError);
}
}
else if(maxAngularLink)
{
UINFO("Max optimization angular error = %f deg (link %d->%d)", maxAngularError*180.0f/M_PI, maxAngularLink->from(), maxAngularLink->to());
if(maxAngularErrorRatio > optimizeMaxError)
{
msg = uFormat("Rejecting edge %d->%d because "
"graph error is too large after optimization (%f deg for edge %d->%d with ratio %f > std=%f deg). "
"\"%s\" is %f m.",
from,
to,
maxAngularError*180.0f/M_PI,
maxAngularLink->from(),
maxAngularLink->to(),
maxAngularErrorRatio,
sqrt(maxAngularLink->rotVariance()),
Parameters::kRGBDOptimizeMaxError().c_str(),
optimizeMaxError);
}
}
UWARN("\"%s\" is false and signatures (%d and %d) don't have raw "
"images. Update the cache.",
Parameters::kRGBDLoopClosureReextractFeatures().c_str());
}
else
{
msg = uFormat("Rejecting edge %d->%d because graph optimization has failed!",
from,
to);
}
if(!msg.empty())
{
UWARN("%s", msg.c_str());
_progressDialog->appendText(tr("%1").arg(msg.c_str()));
QApplication::processEvents();
updateConstraint = false;
signatureFrom.removeAllWords();
signatureFrom.sensorData().setFeatures(std::vector<cv::KeyPoint>(), std::vector<cv::Point3f>(), cv::Mat());
signatureTo.removeAllWords();
signatureTo.sensorData().setFeatures(std::vector<cv::KeyPoint>(), std::vector<cv::Point3f>(), cv::Mat());
}
}
if(updateConstraint)
else if(!reextractFeatures && signatureFrom.getWords().empty() && signatureTo.getWords().empty())
{
UINFO("Added new loop closure between %d and %d.", from, to);
addedLinks.insert(from);
addedLinks.insert(to);
UWARN("\"%s\" is false and signatures (%d and %d) don't have words, "
"registration will not be possible. Set \"%s\" to true.",
Parameters::kRGBDLoopClosureReextractFeatures().c_str(),
signatureFrom.id(),
signatureTo.id(),
Parameters::kRGBDLoopClosureReextractFeatures().c_str());
}
transform = registration->computeTransformation(signatureFrom, signatureTo, Transform(), &info);
delete registration;
if(!transform.isNull())
{
//optimize the graph to see if the new constraint is globally valid
bool updateConstraint = true;
cv::Mat information = info.covariance.inv();
if(odomMaxInf.size() == 6 && information.cols==6 && information.rows==6)
{
for(int i=0; i<6; ++i)
{
if(information.at<double>(i,i) > odomMaxInf[i])
{
information.at<double>(i,i) = odomMaxInf[i];
}
}
}
if(optimizeMaxError > 0.0f && optimizeIterations > 0)
{
int fromId = from;
int mapId = _currentMapIds.at(from);
// use first node of the map containing from
for(std::map<int, int>::iterator iter=_currentMapIds.begin(); iter!=_currentMapIds.end(); ++iter)
{
if(iter->second == mapId && _currentPosesMap.find(iter->first)!=_currentPosesMap.end())
{
fromId = iter->first;
break;
}
}
std::multimap<int, Link> linksIn = _currentLinksMap;
linksIn.insert(std::make_pair(from, Link(from, to, Link::kUserClosure, transform, information)));
const Link * maxLinearLink = 0;
const Link * maxAngularLink = 0;
float maxLinearError = 0.0f;
float maxAngularError = 0.0f;
std::map<int, Transform> poses;
std::multimap<int, Link> links;
UASSERT(_currentPosesMap.find(fromId) != _currentPosesMap.end());
UASSERT_MSG(_currentPosesMap.find(from) != _currentPosesMap.end(), uFormat("id=%d poses=%d links=%d", from, (int)poses.size(), (int)links.size()).c_str());
UASSERT_MSG(_currentPosesMap.find(to) != _currentPosesMap.end(), uFormat("id=%d poses=%d links=%d", to, (int)poses.size(), (int)links.size()).c_str());
optimizer->getConnectedGraph(fromId, _currentPosesMap, linksIn, poses, links);
UASSERT(poses.find(fromId) != poses.end());
UASSERT_MSG(poses.find(from) != poses.end(), uFormat("id=%d poses=%d links=%d", from, (int)poses.size(), (int)links.size()).c_str());
UASSERT_MSG(poses.find(to) != poses.end(), uFormat("id=%d poses=%d links=%d", to, (int)poses.size(), (int)links.size()).c_str());
UASSERT(graph::findLink(links, from, to) != links.end());
poses = optimizer->optimize(fromId, poses, links);
std::string msg;
if(poses.size())
{
float maxLinearErrorRatio = 0.0f;
float maxAngularErrorRatio = 0.0f;
graph::computeMaxGraphErrors(
poses,
links,
maxLinearErrorRatio,
maxAngularErrorRatio,
maxLinearError,
maxAngularError,
&maxLinearLink,
&maxAngularLink);
if(maxLinearLink)
{
UINFO("Max optimization linear error = %f m (link %d->%d)", maxLinearError, maxLinearLink->from(), maxLinearLink->to());
if(maxLinearErrorRatio > optimizeMaxError)
{
msg = uFormat("Rejecting edge %d->%d because "
"graph error is too large after optimization (%f m for edge %d->%d with ratio %f > std=%f m). "
"\"%s\" is %f.",
from,
to,
maxLinearError,
maxLinearLink->from(),
maxLinearLink->to(),
maxLinearErrorRatio,
sqrt(maxLinearLink->transVariance()),
Parameters::kRGBDOptimizeMaxError().c_str(),
optimizeMaxError);
}
}
else if(maxAngularLink)
{
UINFO("Max optimization angular error = %f deg (link %d->%d)", maxAngularError*180.0f/M_PI, maxAngularLink->from(), maxAngularLink->to());
if(maxAngularErrorRatio > optimizeMaxError)
{
msg = uFormat("Rejecting edge %d->%d because "
"graph error is too large after optimization (%f deg for edge %d->%d with ratio %f > std=%f deg). "
"\"%s\" is %f m.",
from,
to,
maxAngularError*180.0f/M_PI,
maxAngularLink->from(),
maxAngularLink->to(),
maxAngularErrorRatio,
sqrt(maxAngularLink->rotVariance()),
Parameters::kRGBDOptimizeMaxError().c_str(),
optimizeMaxError);
}
}
}
else
{
msg = uFormat("Rejecting edge %d->%d because graph optimization has failed!",
from,
to);
}
if(!msg.empty())
{
UWARN("%s", msg.c_str());
_progressDialog->appendText(tr("%1").arg(msg.c_str()));
QApplication::processEvents();
updateConstraint = false;
}
else
{
_currentPosesMap = poses;
}
}
_currentLinksMap.insert(std::make_pair(from, Link(from, to, Link::kUserClosure, transform, information)));
++loopClosuresAdded;
_progressDialog->appendText(tr("Detected loop closure %1->%2! (%3/%4)").arg(from).arg(to).arg(i+1).arg(clusters.size()));
if(updateConstraint)
{
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, Link::kUserClosure, transform, information)));
++loopClosuresAdded;
_progressDialog->appendText(tr("Detected loop closure %1->%2! (%3/%4)").arg(from).arg(to).arg(i+1).arg(clusters.size()));
}
}
}
}

249
guilib/src/ui/exportCloudsDialog.ui
View File

@@ -23,9 +23,9 @@
<property name="geometry">
<rect>
<x>0</x>
<y>-584</y>
<y>-148</y>
<width>780</width>
<height>5347</height>
<height>5569</height>
</rect>
</property>
<layout class="QVBoxLayout" name="verticalLayout_13">
@@ -52,21 +52,21 @@
</property>
</widget>
</item>
<item row="10" column="0">
<item row="11" column="0">
<widget class="QCheckBox" name="checkBox_regenerate">
<property name="text">
<string/>
</property>
</widget>
</item>
<item row="11" column="0">
<item row="12" column="0">
<widget class="QCheckBox" name="checkBox_filtering">
<property name="text">
<string/>
</property>
</widget>
</item>
<item row="10" column="1">
<item row="11" column="1">
<widget class="QLabel" name="label_regenerate">
<property name="text">
<string>Regenerate clouds. This can be used to regenerate the point clouds at higher density than those used for online visualization.</string>
@@ -116,14 +116,14 @@
</property>
</widget>
</item>
<item row="12" column="0">
<item row="13" column="0">
<widget class="QCheckBox" name="checkBox_smoothing">
<property name="text">
<string/>
</property>
</widget>
</item>
<item row="11" column="1">
<item row="12" column="1">
<widget class="QLabel" name="label_binaryFile_9">
<property name="text">
<string>Cloud filtering. Remove sparse points that are far from surfaces.</string>
@@ -143,7 +143,7 @@
</property>
</widget>
</item>
<item row="12" column="1">
<item row="13" column="1">
<widget class="QLabel" name="label_smoothing">
<property name="text">
<string>Cloud smoothing using Moving Least Squares algorithm (MLS).</string>
@@ -289,7 +289,7 @@
</item>
</widget>
</item>
<item row="13" column="1">
<item row="14" column="1">
<widget class="QLabel" name="label_gainCompensation">
<property name="text">
<string>Gain compensation. Normalize brightness of images.</string>
@@ -299,7 +299,7 @@
</property>
</widget>
</item>
<item row="14" column="1">
<item row="15" column="1">
<widget class="QLabel" name="label_cameraProjection">
<property name="text">
<string>Camera projection. This can be used to colorize point cloud created from scans and/or export camera IDs for each point of the cloud.</string>
@@ -309,21 +309,21 @@
</property>
</widget>
</item>
<item row="13" column="0">
<item row="14" column="0">
<widget class="QCheckBox" name="checkBox_gainCompensation">
<property name="text">
<string/>
</property>
</widget>
</item>
<item row="14" column="0">
<item row="15" column="0">
<widget class="QCheckBox" name="checkBox_cameraProjection">
<property name="text">
<string/>
</property>
</widget>
</item>
<item row="15" column="1">
<item row="16" column="1">
<widget class="QLabel" name="label_binaryFile_12">
<property name="text">
<string>Meshing.</string>
@@ -333,15 +333,236 @@
</property>
</widget>
</item>
<item row="15" column="0">
<item row="16" column="0">
<widget class="QCheckBox" name="checkBox_meshing">
<property name="text">
<string/>
</property>
</widget>
</item>
<item row="10" column="1">
<widget class="QLabel" name="label_binaryFile_10">
<property name="text">
<string>Nodes filtering. Filter nodes to be exported in a specified region .</string>
</property>
<property name="wordWrap">
<bool>true</bool>
</property>
</widget>
</item>
<item row="10" column="0">
<widget class="QCheckBox" name="checkBox_nodes_filtering">
<property name="text">
<string/>
</property>
</widget>
</item>
</layout>
</item>
<item>
<widget class="QGroupBox" name="groupBox_nodes_filtering">
<property name="title">
<string>Nodes Filtering</string>
</property>
<layout class="QVBoxLayout" name="verticalLayout_19">
<item>
<widget class="QLabel" name="label_114">
<property name="text">
<string>Set min and max range values on each axis. Nodes inside those ranges will be exported. If the min and max are equals, there is no filtering on that axis. For example for 2D map, set Z min and Z max to 0. To filter just in altitude, set only Z ranges.</string>
</property>
<property name="wordWrap">
<bool>true</bool>
</property>
</widget>
</item>
<item>
<layout class="QGridLayout" name="gridLayout_21">
<item row="0" column="2">
<widget class="QLabel" name="label_113">
<property name="text">
<string>X max</string>
</property>
<property name="wordWrap">
<bool>true</bool>
</property>
</widget>
</item>
<item row="1" column="0">
<widget class="QLabel" name="label_142">
<property name="toolTip">
<string>Length along X-Axis</string>
</property>
<property name="text">
<string>Y min</string>
</property>
<property name="wordWrap">
<bool>true</bool>
</property>
</widget>
</item>
<item row="1" column="1">
<widget class="QDoubleSpinBox" name="doubleSpinBox_nodes_filtering_ymin">
<property name="suffix">
<string> m</string>
</property>
<property name="decimals">
<number>0</number>
</property>
<property name="minimum">
<double>-9999999.000000000000000</double>
</property>
<property name="maximum">
<double>9999999.000000000000000</double>
</property>
</widget>
</item>
<item row="1" column="2">
<widget class="QLabel" name="label_115">
<property name="toolTip">
<string>Width along Y-Axis</string>
</property>
<property name="text">
<string>Y max</string>
</property>
<property name="wordWrap">
<bool>true</bool>
</property>
</widget>
</item>
<item row="2" column="0">
<widget class="QLabel" name="label_116">
<property name="toolTip">
<string>Height along Z-Axis</string>
</property>
<property name="text">
<string>Z min</string>
</property>
<property name="wordWrap">
<bool>true</bool>
</property>
</widget>
</item>
<item row="2" column="2">
<widget class="QLabel" name="label_119">
<property name="toolTip">
<string>Width along Y-Axis</string>
</property>
<property name="text">
<string>Z max</string>
</property>
<property name="wordWrap">
<bool>true</bool>
</property>
</widget>
</item>
<item row="0" column="0">
<widget class="QLabel" name="label_111">
<property name="text">
<string>X min</string>
</property>
<property name="wordWrap">
<bool>true</bool>
</property>
</widget>
</item>
<item row="1" column="4">
<spacer name="horizontalSpacer">
<property name="orientation">
<enum>Qt::Horizontal</enum>
</property>
<property name="sizeHint" stdset="0">
<size>
<width>40</width>
<height>20</height>
</size>
</property>
</spacer>
</item>
<item row="0" column="1">
<widget class="QDoubleSpinBox" name="doubleSpinBox_nodes_filtering_xmin">
<property name="suffix">
<string> m</string>
</property>
<property name="decimals">
<number>0</number>
</property>
<property name="minimum">
<double>-9999999.000000000000000</double>
</property>
<property name="maximum">
<double>9999999.000000000000000</double>
</property>
</widget>
</item>
<item row="0" column="3">
<widget class="QDoubleSpinBox" name="doubleSpinBox_nodes_filtering_xmax">
<property name="suffix">
<string> m</string>
</property>
<property name="decimals">
<number>0</number>
</property>
<property name="minimum">
<double>-9999999.000000000000000</double>
</property>
<property name="maximum">
<double>9999999.000000000000000</double>
</property>
</widget>
</item>
<item row="2" column="1">
<widget class="QDoubleSpinBox" name="doubleSpinBox_nodes_filtering_zmin">
<property name="suffix">
<string> m</string>
</property>
<property name="decimals">
<number>0</number>
</property>
<property name="minimum">
<double>-9999999.000000000000000</double>
</property>
<property name="maximum">
<double>9999999.000000000000000</double>
</property>
</widget>
</item>
<item row="1" column="3">
<widget class="QDoubleSpinBox" name="doubleSpinBox_nodes_filtering_ymax">
<property name="suffix">
<string> m</string>
</property>
<property name="decimals">
<number>0</number>
</property>
<property name="minimum">
<double>-9999999.000000000000000</double>
</property>
<property name="maximum">
<double>9999999.000000000000000</double>
</property>
</widget>
</item>
<item row="2" column="3">
<widget class="QDoubleSpinBox" name="doubleSpinBox_nodes_filtering_zmax">
<property name="suffix">
<string> m</string>
</property>
<property name="decimals">
<number>0</number>
</property>
<property name="minimum">
<double>-9999999.000000000000000</double>
</property>
<property name="maximum">
<double>9999999.000000000000000</double>
</property>
</widget>
</item>
</layout>
</item>
</layout>
</widget>
</item>
<item>
<widget class="QGroupBox" name="groupBox_regenerateScans">
<property name="title">