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

DbViewer: added convenient option to rotate the occupancy grid (affect only optimized graph for localization mode).

Browse Source
This commit is contained in:
matlabbe
2023-02-22 17:20:09 -08:00
parent 23b0b37493
commit 70c3d5d97f
4 changed files with 246 additions and 54 deletions
Download Patch File Download Diff File Expand all files Collapse all files

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

@@ -137,6 +137,7 @@ private Q_SLOTS:
void editConstraint();
void updateGrid();
void updateOctomapView();
void updateGraphRotation();
void updateGraphView();
void refineConstraint();
void addConstraint();

202
guilib/src/DatabaseViewer.cpp
View File

@@ -373,7 +373,7 @@ DatabaseViewer::DatabaseViewer(const QString & ini, QWidget * parent) :
connect(ui_->horizontalSlider_iterations, SIGNAL(valueChanged(int)), this, SLOT(sliderIterationsValueChanged(int)));
connect(ui_->horizontalSlider_iterations, SIGNAL(sliderMoved(int)), this, SLOT(sliderIterationsValueChanged(int)));
connect(ui_->spinBox_optimizationsFrom, SIGNAL(editingFinished()), this, SLOT(updateGraphView()));
connect(ui_->checkBox_iterativeOptimization, SIGNAL(stateChanged(int)), this, SLOT(updateGraphView()));
connect(ui_->comboBox_optimizationFlavor, SIGNAL(activated(int)), this, SLOT(updateGraphView()));
connect(ui_->checkBox_spanAllMaps, SIGNAL(stateChanged(int)), this, SLOT(updateGraphView()));
connect(ui_->checkBox_wmState, SIGNAL(stateChanged(int)), this, SLOT(updateGraphView()));
connect(ui_->graphViewer, SIGNAL(mapShownRequested()), this, SLOT(updateGraphView()));
@@ -428,6 +428,8 @@ DatabaseViewer::DatabaseViewer(const QString & ini, QWidget * parent) :
connect(ui_->groupBox_posefiltering, SIGNAL(clicked(bool)), this, SLOT(configModified()));
connect(ui_->doubleSpinBox_posefilteringRadius, SIGNAL(valueChanged(double)), this, SLOT(configModified()));
connect(ui_->doubleSpinBox_posefilteringAngle, SIGNAL(valueChanged(double)), this, SLOT(configModified()));
connect(ui_->horizontalSlider_rotation, SIGNAL(valueChanged(int)), this, SLOT(updateGraphRotation()));
connect(ui_->pushButton_applyRotation, SIGNAL(clicked()), this, SLOT(updateGraphView()));
connect(ui_->spinBox_icp_decimation, SIGNAL(valueChanged(int)), this, SLOT(configModified()));
connect(ui_->doubleSpinBox_icp_maxDepth, SIGNAL(valueChanged(double)), this, SLOT(configModified()));
@@ -721,7 +723,7 @@ void DatabaseViewer::restoreDefaultSettings()
ui_->checkBox_ignoreIntermediateNodes->setChecked(false);
ui_->checkBox_timeStats->setChecked(true);
ui_->checkBox_iterativeOptimization->setChecked(true);
ui_->comboBox_optimizationFlavor->setCurrentIndex(0);
ui_->checkBox_spanAllMaps->setChecked(true);
ui_->checkBox_wmState->setChecked(false);
ui_->checkBox_ignorePoseCorrection->setChecked(false);
@@ -7117,6 +7119,21 @@ void DatabaseViewer::sliderIterationsValueChanged(int value)
ui_->label_pathLength->setNum(length);
}
}
void DatabaseViewer::updateGraphRotation()
{
if(ui_->horizontalSlider_rotation->isEnabled())
{
float theta = float(ui_->horizontalSlider_rotation->value())*M_PI/1800.0f;
ui_->graphViewer->setWorldMapRotation(theta);
ui_->label_rotation->setText(QString::number(float(-ui_->horizontalSlider_rotation->value())/10.0f, 'f', 1) + " deg");
}
else
{
ui_->graphViewer->setWorldMapRotation(0);
}
}
void DatabaseViewer::updateGraphView()
{
ui_->label_loopClosures->clear();
@@ -7412,41 +7429,160 @@ void DatabaseViewer::updateGraphView()
}
}
graphes_.push_back(poses);
Optimizer * optimizer = Optimizer::create(parameters);
std::map<int, rtabmap::Transform> posesOut;
std::multimap<int, rtabmap::Link> linksOut;
UINFO("Get connected graph from %d (%d poses, %d links)", fromId, (int)poses.size(), (int)links.size());
optimizer->getConnectedGraph(
fromId,
poses,
links,
posesOut,
linksOut);
UINFO("Connected graph of %d poses and %d links", (int)posesOut.size(), (int)linksOut.size());
QTime time;
time.start();
std::map<int, rtabmap::Transform> finalPoses = optimizer->optimize(fromId, posesOut, linksOut, ui_->checkBox_iterativeOptimization->isChecked()?&graphes_:0);
ui_->label_timeOptimization->setNum(double(time.elapsed())/1000.0);
graphLinks_ = linksOut;
if(posesOut.size() && finalPoses.empty())
bool applyRotation = sender() == ui_->pushButton_applyRotation;
if(applyRotation)
{
UWARN("Optimization failed... (poses=%d, links=%d).", (int)posesOut.size(), (int)linksOut.size());
if(!optimizer->isCovarianceIgnored() || optimizer->type() != Optimizer::kTypeTORO)
float xMin, yMin, cellSize;
bool hasMap = !dbDriver_->load2DMap(xMin, yMin, cellSize).empty();
if(hasMap || !dbDriver_->loadOptimizedMesh().empty())
{
QMessageBox::warning(this, tr("Graph optimization error!"), tr("Graph optimization has failed. See the terminal for potential errors. "
"Give it a try with %1=0 and %2=true.").arg(Parameters::kOptimizerStrategy().c_str()).arg(Parameters::kOptimizerVarianceIgnored().c_str()));
}
else
{
QMessageBox::warning(this, tr("Graph optimization error!"), tr("Graph optimization has failed. See the terminal for potential errors."));
QMessageBox::StandardButton r = QMessageBox::question(this,
tr("Rotate Optimized Graph"),
tr("There is a 2D occupancy grid or mesh already saved in "
"database. Applying rotation will clear them (they can be "
"regenerated later from File menu options). "
"Do you want to continue?"),
QMessageBox::Cancel | QMessageBox::Yes,
QMessageBox::Cancel);
if(r != QMessageBox::Yes)
{
applyRotation = false;
}
}
}
ui_->label_poses->setNum((int)finalPoses.size());
graphes_.push_back(finalPoses);
delete optimizer;
std::map<int, Transform> optPoses;
Transform lastLocalizationPose;
if(applyRotation ||
ui_->comboBox_optimizationFlavor->currentIndex() == 2)
{
optPoses = dbDriver_->loadOptimizedPoses(&lastLocalizationPose);
if(optPoses.empty())
{
ui_->comboBox_optimizationFlavor->setCurrentIndex(0);
QMessageBox::warning(this, tr("Optimization Flavor"),
tr("There is no local optimized graph in the database, "
"falling back to global iterative optimization."));
}
}
if(applyRotation ||
ui_->comboBox_optimizationFlavor->currentIndex() != 2)
{
if(ui_->horizontalSlider_rotation->value()!=0 && applyRotation)
{
float theta = float(-ui_->horizontalSlider_rotation->value())*M_PI/1800.0f;
Transform rotT(0,0,theta);
poses.at(fromId) = rotT * poses.at(fromId);
}
graphes_.push_back(poses);
Optimizer * optimizer = Optimizer::create(parameters);
std::map<int, rtabmap::Transform> posesOut;
std::multimap<int, rtabmap::Link> linksOut;
UINFO("Get connected graph from %d (%d poses, %d links)", fromId, (int)poses.size(), (int)links.size());
optimizer->getConnectedGraph(
fromId,
poses,
links,
posesOut,
linksOut);
UINFO("Connected graph of %d poses and %d links", (int)posesOut.size(), (int)linksOut.size());
QTime time;
time.start();
std::map<int, rtabmap::Transform> finalPoses = optimizer->optimize(fromId, posesOut, linksOut, ui_->comboBox_optimizationFlavor->currentIndex()==0?&graphes_:0);
ui_->label_timeOptimization->setNum(double(time.elapsed())/1000.0);
graphLinks_ = linksOut;
if(posesOut.size() && finalPoses.empty())
{
UWARN("Optimization failed... (poses=%d, links=%d).", (int)posesOut.size(), (int)linksOut.size());
if(!optimizer->isCovarianceIgnored() || optimizer->type() != Optimizer::kTypeTORO)
{
QMessageBox::warning(this, tr("Graph optimization error!"), tr("Graph optimization has failed. See the terminal for potential errors. "
"Give it a try with %1=0 and %2=true.").arg(Parameters::kOptimizerStrategy().c_str()).arg(Parameters::kOptimizerVarianceIgnored().c_str()));
}
else
{
QMessageBox::warning(this, tr("Graph optimization error!"), tr("Graph optimization has failed. See the terminal for potential errors."));
}
}
ui_->label_poses->setNum((int)finalPoses.size());
graphes_.push_back(finalPoses);
delete optimizer;
if(applyRotation && !finalPoses.empty())
{
ui_->comboBox_optimizationFlavor->setCurrentIndex(2);
graphes_.clear();
graphes_.push_back(finalPoses);
if(lastLocalizationPose.isNull())
{
// use last pose by default
lastLocalizationPose = finalPoses.rbegin()->second;
}
dbDriver_->saveOptimizedPoses(finalPoses, lastLocalizationPose);
// reset optimized mesh and map as poses have changed
float xMin, yMin, cellSize;
bool hasMap = !dbDriver_->load2DMap(xMin, yMin, cellSize).empty();
if(hasMap || !dbDriver_->loadOptimizedMesh().empty())
{
dbDriver_->saveOptimizedMesh(cv::Mat());
dbDriver_->save2DMap(cv::Mat(), 0, 0, 0);
QMessageBox::StandardButton r = QMessageBox::question(this,
tr("Rotate Optimized Graph"),
tr("Optimized graph has been rotated and saved back to database. "
"Note that 2D occupancy grid and mesh have been cleared (if set). "
"Do you want to regenerate the 2D occupancy grid now "
"(can be done later from File menu)?"),
QMessageBox::Ignore | QMessageBox::Yes,
QMessageBox::Yes);
ui_->actionEdit_optimized_2D_map->setEnabled(false);
ui_->actionExport_saved_2D_map->setEnabled(false);
ui_->actionImport_2D_map->setEnabled(false);
ui_->actionView_optimized_mesh->setEnabled(false);
ui_->actionExport_optimized_mesh->setEnabled(false);
if(r == QMessageBox::Yes)
{
regenerateSavedMap();
}
}
}
}
// Update buttons state
if(ui_->comboBox_optimizationFlavor->currentIndex() == 2)
{
// Local optimized graph
if(graphes_.empty())
{
ui_->label_timeOptimization->setNum(0);
ui_->label_poses->setNum((int)optPoses.size());
graphes_.push_back(optPoses);
}
ui_->horizontalSlider_rotation->setEnabled(false);
ui_->pushButton_applyRotation->setEnabled(false);
ui_->spinBox_optimizationsFrom->setEnabled(false);
ui_->checkBox_spanAllMaps->setEnabled(false);
ui_->checkBox_wmState->setEnabled(false);
ui_->checkBox_alignPosesWithGroundTruth->setEnabled(false);
ui_->checkBox_alignScansCloudsWithGroundTruth->setEnabled(false);
ui_->checkBox_ignoreIntermediateNodes->setEnabled(false);
}
else
{
// Global map re-optimized
ui_->pushButton_applyRotation->setEnabled(true);
ui_->horizontalSlider_rotation->setEnabled(true);
ui_->spinBox_optimizationsFrom->setEnabled(true);
ui_->checkBox_spanAllMaps->setEnabled(true);
ui_->checkBox_wmState->setEnabled(true);
ui_->checkBox_alignPosesWithGroundTruth->setEnabled(true);
ui_->checkBox_alignScansCloudsWithGroundTruth->setEnabled(true);
ui_->checkBox_ignoreIntermediateNodes->setEnabled(true);
}
updateGraphRotation();
}
if(graphes_.size())
{

20
guilib/src/GraphViewer.cpp
View File

@@ -129,6 +129,13 @@ public:
break;
}
_pose=pose;
float r,p,yaw;
_pose.getEulerAngles(r, p, yaw);
if(_line)
{
float radius = this->rect().width()/2.0f;
_line->setLine(0,0,-radius*sin(yaw),-radius*cos(yaw));
}
}
protected:
@@ -1704,16 +1711,9 @@ void GraphViewer::setOrientationENU(bool enabled)
_orientationENU = enabled;
this->rotate(_orientationENU?90:270);
}
if(_orientationENU)
{
QTransform t;
t.rotateRadians(_worldMapRotation);
_root->setTransform(t);
}
else
{
_root->resetTransform();
}
QTransform t;
t.rotateRadians(_worldMapRotation);
_root->setTransform(t);
if(_nodeItems.size() || _linkItems.size())
{
this->scene()->setSceneRect(this->scene()->itemsBoundingRect()); // Re-shrink the scene to it's bounding contents

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

@@ -61,7 +61,7 @@
<rect>
<x>0</x>
<y>0</y>
<width>293</width>
<width>307</width>
<height>380</height>
</rect>
</property>
@@ -355,7 +355,7 @@
<rect>
<x>0</x>
<y>0</y>
<width>292</width>
<width>306</width>
<height>380</height>
</rect>
</property>
@@ -1175,6 +1175,52 @@
<item>
<widget class="rtabmap::GraphViewer" name="graphViewer"/>
</item>
<item>
<layout class="QHBoxLayout" name="horizontalLayout_15">
<item>
<widget class="QLabel" name="label_rotation">
<property name="text">
<string>0.0 deg</string>
</property>
</widget>
</item>
<item>
<widget class="QSlider" name="horizontalSlider_rotation">
<property name="focusPolicy">
<enum>Qt::ClickFocus</enum>
</property>
<property name="minimum">
<number>-1799</number>
</property>
<property name="maximum">
<number>1800</number>
</property>
<property name="sliderPosition">
<number>0</number>
</property>
<property name="orientation">
<enum>Qt::Horizontal</enum>
</property>
<property name="tickPosition">
<enum>QSlider::TicksAbove</enum>
</property>
<property name="tickInterval">
<number>100</number>
</property>
</widget>
</item>
<item>
<widget class="QPushButton" name="pushButton_applyRotation">
<property name="toolTip">
<string>&lt;html&gt;&lt;head/&gt;&lt;body&gt;&lt;p&gt;The rotation will be applied temporary to optimized global graph. To save it to database, do File-&amp;gt;&amp;quot;Regenerate optimized 2D map...&amp;quot;.&lt;/p&gt;&lt;/body&gt;&lt;/html&gt;</string>
</property>
<property name="text">
<string>Apply Rotation</string>
</property>
</widget>
</item>
</layout>
</item>
<item>
<layout class="QHBoxLayout" name="horizontalLayout_4">
<item>
@@ -1198,16 +1244,25 @@
</widget>
</item>
<item>
<widget class="QCheckBox" name="checkBox_iterativeOptimization">
<property name="toolTip">
<string>Iterative optimization. Intermediate optimization graphes will be computed. Depending on the graph optimizer used, final optimization may not give the same results.</string>
</property>
<property name="text">
<string/>
</property>
<property name="checked">
<bool>true</bool>
<widget class="QComboBox" name="comboBox_optimizationFlavor">
<property name="sizeAdjustPolicy">
<enum>QComboBox::AdjustToContents</enum>
</property>
<item>
<property name="text">
<string>Global Iterative</string>
</property>
</item>
<item>
<property name="text">
<string>Global Full</string>
</property>
</item>
<item>
<property name="text">
<string>Local Optimized</string>
</property>
</item>
</widget>
</item>
</layout>