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

DatabaseViewer: added exporting of a specific session with or without user_data. MainWindow: added action to send goal to rtabmap. Rtabmap: Saving goals in user_data of the current signature, modified parameter Mem/RehearsedNodesKept to Mem/NotLinkedNodesKept. DBReader: added sending goals when detected while playing back a database.

Browse Source
This commit is contained in:
Mathieu Labbe
2015-04-17 18:30:12 -04:00
parent b48bd7cf70
commit 5700c14bcb
25 changed files with 325 additions and 49 deletions
Download Patch File Download Diff File Expand all files Collapse all files

9
guilib/src/DataRecorder.cpp
View File

@@ -75,7 +75,7 @@ bool DataRecorder::init(const QString & path, bool recordInRAM)
customParameters.insert(ParametersPair(Parameters::kMemBinDataKept(), "true")); // to keep images
if(!recordInRAM)
{
customParameters.insert(ParametersPair(Parameters::kDbSqlite3InMemory(), "false")); // to keep images
customParameters.insert(ParametersPair(Parameters::kDbSqlite3InMemory(), "false"));
}
memory_ = new Memory();
if(!memory_->init(path.toStdString(), true, customParameters))
@@ -125,6 +125,13 @@ void DataRecorder::addData(const rtabmap::SensorData & data)
memoryMutex_.lock();
if(memory_)
{
if(memory_->getStMem().size() == 0 && data.id() > 0)
{
ParametersMap customParameters;
customParameters.insert(ParametersPair(Parameters::kMemGenerateIds(), "false")); // use id from data
memory_->parseParameters(customParameters);
}
//save to database
UTimer time;
memory_->update(data);

4
guilib/src/DatabaseViewer.cpp
View File

@@ -706,6 +706,10 @@ void DatabaseViewer::exportDatabase()
Signature data = memory_->getSignatureData(id, true);
rtabmap::SensorData sensorData = data.toSensorData();
if(!dialog.isUserDataExported())
{
sensorData.setUserData(std::vector<unsigned char>());
}
recorder.addData(sensorData);
progressDialog.appendText(tr("Exported node %1").arg(id));

6
guilib/src/ExportDialog.cpp
View File

@@ -46,6 +46,7 @@ ExportDialog::ExportDialog(QWidget * parent) :
connect(_ui->checkBox_depth, SIGNAL(stateChanged(int)), this, SIGNAL(configChanged()));
connect(_ui->checkBox_depth2d, SIGNAL(stateChanged(int)), this, SIGNAL(configChanged()));
connect(_ui->checkBox_odom, SIGNAL(stateChanged(int)), this, SIGNAL(configChanged()));
connect(_ui->checkBox_userData, SIGNAL(stateChanged(int)), this, SIGNAL(configChanged()));
_ui->lineEdit_path->setText(QDir::homePath()+QDir::separator()+"output.db");
}
@@ -99,4 +100,9 @@ bool ExportDialog::isOdomExported() const
return _ui->checkBox_odom->isChecked();
}
bool ExportDialog::isUserDataExported() const
{
return _ui->checkBox_userData->isChecked();
}
}

1
guilib/src/ExportDialog.h
View File

@@ -50,6 +50,7 @@ public:
bool isDepthExported() const;
bool isDepth2dExported() const;
bool isOdomExported() const;
bool isUserDataExported() const;
signals:
void configChanged();

57
guilib/src/GraphViewer.cpp
View File

@@ -157,7 +157,8 @@ GraphViewer::GraphViewer(QWidget * parent) :
_loopClosureLocalColor(Qt::yellow),
_loopClosureUserColor(Qt::red),
_loopClosureVirtualColor(Qt::magenta),
_pathColor(Qt::cyan),
_localPathColor(Qt::cyan),
_globalPathColor(Qt::darkMagenta),
_root(0),
_nodeRadius(0.01),
_linkWidth(0),
@@ -430,6 +431,32 @@ void GraphViewer::updatePosterior(const std::map<int, float> & posterior)
}
}
void GraphViewer::setGlobalPath(const std::vector<std::pair<int, Transform> > & globalPath)
{
UDEBUG("Set global path size=%d", (int)globalPath.size());
qDeleteAll(_globalPathLinkItems);
_globalPathLinkItems.clear();
if(globalPath.size() >= 2)
{
for(unsigned int i=0; i<globalPath.size()-1; ++i)
{
//create a link item
int idFrom = globalPath[i].first;
int idTo = globalPath[i+1].first;
LinkItem * item = new LinkItem(idFrom, idTo, globalPath[i].second, globalPath[i+1].second, Link::kUndef);
QPen p = item->pen();
p.setWidthF(_linkWidth);
item->setPen(p);
item->setColor(_globalPathColor);
this->scene()->addItem(item);
item->setZValue(1);
item->setParentItem(_root);
_globalPathLinkItems.insert(idFrom, item);
}
}
}
void GraphViewer::updateLocalPath(const std::vector<int> & localPath)
{
if(localPath.size() > 1)
@@ -445,7 +472,7 @@ void GraphViewer::updateLocalPath(const std::vector<int> & localPath)
{
if(itemIter.value()->to() == idTo)
{
itemIter.value()->setColor(_pathColor);
itemIter.value()->setColor(_localPathColor);
break;
}
++itemIter;
@@ -461,6 +488,8 @@ void GraphViewer::clearGraph()
_nodeItems.clear();
qDeleteAll(_linkItems);
_linkItems.clear();
qDeleteAll(_globalPathLinkItems);
_globalPathLinkItems.clear();
_referential->resetTransform();
this->scene()->setSceneRect(this->scene()->itemsBoundingRect()); // Re-shrink the scene to it's bounding contents
}
@@ -501,6 +530,7 @@ void GraphViewer::saveSettings(QSettings & settings, const QString & group) cons
settings.setValue("user_color", this->getUserLoopClosureColor());
settings.setValue("virtual_color", this->getVirtualLoopClosureColor());
settings.setValue("local_path_color", this->getLocalPathColor());
settings.setValue("global_path_color", this->getGlobalPathColor());
settings.setValue("grid_visible", this->isGridMapVisible());
settings.setValue("origin_visible", this->isOriginVisible());
settings.setValue("referential_visible", this->isReferentialVisible());
@@ -525,6 +555,7 @@ void GraphViewer::loadSettings(QSettings & settings, const QString & group)
this->setUserLoopClosureColor(settings.value("user_color", this->getUserLoopClosureColor()).value<QColor>());
this->setVirtualLoopClosureColor(settings.value("virtual_color", this->getVirtualLoopClosureColor()).value<QColor>());
this->setLocalPathColor(settings.value("local_path_color", this->getLocalPathColor()).value<QColor>());
this->setGlobalPathColor(settings.value("global_path_color", this->getGlobalPathColor()).value<QColor>());
this->setGridMapVisible(settings.value("grid_visible", this->isGridMapVisible()).toBool());
this->setOriginVisible(settings.value("origin_visible", this->isOriginVisible()).toBool());
this->setReferentialVisible(settings.value("referential_visible", this->isReferentialVisible()).toBool());
@@ -644,7 +675,11 @@ void GraphViewer::setVirtualLoopClosureColor(const QColor & color)
}
void GraphViewer::setLocalPathColor(const QColor & color)
{
_pathColor = color;
_localPathColor = color;
}
void GraphViewer::setGlobalPathColor(const QColor & color)
{
_globalPathColor = color;
}
void GraphViewer::setGridMapVisible(bool visible)
{
@@ -710,18 +745,21 @@ void GraphViewer::contextMenuEvent(QContextMenuEvent * event)
QAction * aChangeUserLoopColor = menuLink->addAction(tr("User loop closure"));
QAction * aChangeVirtualLoopColor = menuLink->addAction(tr("Virtual loop closure"));
QAction * aChangeLocalPathColor = menuLink->addAction(tr("Local path"));
QAction * aChangeGlobalPathColor = menuLink->addAction(tr("Global path"));
aChangeNeighborColor->setIcon(createIcon(_neighborColor));
aChangeGlobalLoopColor->setIcon(createIcon(_loopClosureColor));
aChangeLocalLoopColor->setIcon(createIcon(_loopClosureLocalColor));
aChangeUserLoopColor->setIcon(createIcon(_loopClosureUserColor));
aChangeVirtualLoopColor->setIcon(createIcon(_loopClosureVirtualColor));
aChangeLocalPathColor->setIcon(createIcon(_pathColor));
aChangeLocalPathColor->setIcon(createIcon(_localPathColor));
aChangeGlobalPathColor->setIcon(createIcon(_globalPathColor));
aChangeNeighborColor->setIconVisibleInMenu(true);
aChangeGlobalLoopColor->setIconVisibleInMenu(true);
aChangeLocalLoopColor->setIconVisibleInMenu(true);
aChangeUserLoopColor->setIconVisibleInMenu(true);
aChangeVirtualLoopColor->setIconVisibleInMenu(true);
aChangeLocalPathColor->setIconVisibleInMenu(true);
aChangeGlobalPathColor->setIconVisibleInMenu(true);
menu.addSeparator();
QAction * aSetNodeSize = menu.addAction(tr("Set node radius..."));
@@ -830,7 +868,8 @@ void GraphViewer::contextMenuEvent(QContextMenuEvent * event)
r == aChangeLocalLoopColor ||
r == aChangeUserLoopColor ||
r == aChangeVirtualLoopColor ||
r == aChangeLocalPathColor)
r == aChangeLocalPathColor ||
r == aChangeGlobalPathColor)
{
QColor color;
if(r == aChangeNodeColor)
@@ -855,7 +894,11 @@ void GraphViewer::contextMenuEvent(QContextMenuEvent * event)
}
else if(r == aChangeLocalPathColor)
{
color = _pathColor;
color = _localPathColor;
}
else if(r == aChangeGlobalPathColor)
{
color = _globalPathColor;
}
else //if(r == aChangeNeighborColor)
{
@@ -902,7 +945,7 @@ void GraphViewer::contextMenuEvent(QContextMenuEvent * event)
else if(r == aSetNodeSize)
{
bool ok;
double value = QInputDialog::getDouble(this, tr("Node radius"), tr("Radius (m)"), _nodeRadius, 0.01, 100, 2, &ok);
double value = QInputDialog::getDouble(this, tr("Node radius"), tr("Radius (m)"), _nodeRadius, 0.001, 100, 3, &ok);
if(ok)
{
setNodeRadius(value);

9
guilib/src/GraphViewer.h
View File

@@ -58,6 +58,7 @@ public:
void updateMap(const cv::Mat & map8U, float resolution, float xMin, float yMin);
void updatePosterior(const std::map<int, float> & posterior);
void updateLocalPath(const std::vector<int> & localPath);
void setGlobalPath(const std::vector<std::pair<int, Transform> > & globalPath);
void clearGraph();
void clearMap();
void clearPosterior();
@@ -76,7 +77,8 @@ public:
const QColor & getLocalLoopClosureColor() const {return _loopClosureLocalColor;}
const QColor & getUserLoopClosureColor() const {return _loopClosureUserColor;}
const QColor & getVirtualLoopClosureColor() const {return _loopClosureVirtualColor;}
const QColor & getLocalPathColor() const {return _pathColor;}
const QColor & getLocalPathColor() const {return _localPathColor;}
const QColor & getGlobalPathColor() const {return _globalPathColor;}
bool isGridMapVisible() const;
bool isOriginVisible() const;
bool isReferentialVisible() const;
@@ -92,6 +94,7 @@ public:
void setUserLoopClosureColor(const QColor & color);
void setVirtualLoopClosureColor(const QColor & color);
void setLocalPathColor(const QColor & color);
void setGlobalPathColor(const QColor & color);
void setGridMapVisible(bool visible);
void setOriginVisible(bool visible);
void setReferentialVisible(bool visible);
@@ -114,10 +117,12 @@ private:
QColor _loopClosureLocalColor;
QColor _loopClosureUserColor;
QColor _loopClosureVirtualColor;
QColor _pathColor;
QColor _localPathColor;
QColor _globalPathColor;
QGraphicsItem * _root;
QMap<int, NodeItem*> _nodeItems;
QMultiMap<int, LinkItem*> _linkItems;
QMultiMap<int, LinkItem*> _globalPathLinkItems;
float _nodeRadius;
float _linkWidth;
QGraphicsPixmapItem * _gridMap;

44
guilib/src/MainWindow.cpp
View File

@@ -253,6 +253,8 @@ MainWindow::MainWindow(PreferencesDialog * prefDialog, QWidget * parent) :
connect(this, SIGNAL(rtabmapEventInitReceived(int, const QString &)), this, SLOT(processRtabmapEventInit(int, const QString &)));
qRegisterMetaType<rtabmap::RtabmapEvent3DMap>("rtabmap::RtabmapEvent3DMap");
connect(this, SIGNAL(rtabmapEvent3DMapReceived(const rtabmap::RtabmapEvent3DMap &)), this, SLOT(processRtabmapEvent3DMap(const rtabmap::RtabmapEvent3DMap &)));
qRegisterMetaType<rtabmap::RtabmapGlobalPathEvent>("rtabmap::RtabmapGlobalPathEvent");
connect(this, SIGNAL(rtabmapGlobalPathEventReceived(const rtabmap::RtabmapGlobalPathEvent &)), this, SLOT(processRtabmapGlobalPathEvent(const rtabmap::RtabmapGlobalPathEvent &)));
// Dock Widget view actions (Menu->Window)
_ui->menuShow_view->addAction(_ui->dockWidget_imageView->toggleViewAction());
@@ -285,6 +287,7 @@ MainWindow::MainWindow(PreferencesDialog * prefDialog, QWidget * parent) :
connect(_ui->actionStop, SIGNAL(triggered()), this, SLOT(stopDetection()));
connect(_ui->actionDump_the_memory, SIGNAL(triggered()), this, SLOT(dumpTheMemory()));
connect(_ui->actionDump_the_prediction_matrix, SIGNAL(triggered()), this, SLOT(dumpThePrediction()));
connect(_ui->actionSend_goal, SIGNAL(triggered()), this, SLOT(sendGoal()));
connect(_ui->actionClear_cache, SIGNAL(triggered()), this, SLOT(clearTheCache()));
connect(_ui->actionAbout, SIGNAL(triggered()), _aboutDialog , SLOT(exec()));
connect(_ui->actionPrint_loop_closure_IDs_to_console, SIGNAL(triggered()), this, SLOT(printLoopClosureIds()));
@@ -635,6 +638,11 @@ void MainWindow::handleEvent(UEvent* anEvent)
RtabmapEvent3DMap * rtabmapEvent3DMap = (RtabmapEvent3DMap*)anEvent;
emit rtabmapEvent3DMapReceived(*rtabmapEvent3DMap);
}
else if(anEvent->getClassName().compare("RtabmapGlobalPathEvent") == 0)
{
RtabmapGlobalPathEvent * rtabmapGlobalPathEvent = (RtabmapGlobalPathEvent*)anEvent;
emit rtabmapGlobalPathEventReceived(*rtabmapGlobalPathEvent);
}
else if(anEvent->getClassName().compare("CameraEvent") == 0)
{
CameraEvent * cameraEvent = (CameraEvent*)anEvent;
@@ -744,6 +752,8 @@ void MainWindow::processOdometry(const rtabmap::SensorData & data, const rtabmap
{
_ui->statsToolBox->updateStat("Odometry/Local_map_size/", (float)data.id(), (float)info.localMapSize);
}
_ui->statsToolBox->updateStat("Odometry/ID/", (float)data.id(), (float)data.id());
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);
@@ -1183,6 +1193,11 @@ void MainWindow::processStats(const rtabmap::Statistics & stat)
}
// update local path on the graph view
_ui->graphicsView_graphView->updateLocalPath(stat.localPath());
if(stat.localPath().size() == 0)
{
// clear the global path if set (goal reached)
_ui->graphicsView_graphView->setGlobalPath(std::vector<std::pair<int, Transform> >());
}
UDEBUG("");
}
@@ -1921,6 +1936,22 @@ void MainWindow::processRtabmapEvent3DMap(const rtabmap::RtabmapEvent3DMap & eve
_initProgressDialog->setValue(_initProgressDialog->maximumSteps());
}
void MainWindow::processRtabmapGlobalPathEvent(const rtabmap::RtabmapGlobalPathEvent & event)
{
if(event.getPoses().empty())
{
QMessageBox::warning(this, tr("Setting goal failed"), tr("Setting goal to location %1 failed. "
"Some reasons: \n"
"1) the location doesn't exist in the graph,\n"
"2) the location is not linked to the global graph or\n"
"3) the location is too near of the current location (goal already reached).").arg(event.getGoal()));
}
else
{
_ui->graphicsView_graphView->setGlobalPath(event.getPoses());
}
}
void MainWindow::applyPrefSettings(PreferencesDialog::PANEL_FLAGS flags)
{
ULOGGER_DEBUG("");
@@ -3594,6 +3625,19 @@ void MainWindow::dumpThePrediction()
this->post(new RtabmapEventCmd(RtabmapEventCmd::kCmdDumpPrediction));
}
void MainWindow::sendGoal()
{
UINFO("Sending a goal...");
bool ok = false;
int id = QInputDialog::getInt(this, tr("Send a goal"), tr("Goal location ID: "), 1, 1, 99999, 1, &ok);
if(ok)
{
_ui->graphicsView_graphView->setGlobalPath(std::vector<std::pair<int, Transform> >()); // clear
UINFO("Posting event with goal %d", id);
this->post(new RtabmapEventCmd(RtabmapEventCmd::kCmdGoal, "", id));
}
}
void MainWindow::downloadAllClouds()
{
QStringList items;

3
guilib/src/PreferencesDialog.cpp
View File

@@ -349,7 +349,7 @@ PreferencesDialog::PreferencesDialog(QWidget * parent) :
// Memory
_ui->general_checkBox_keepRawData->setObjectName(Parameters::kMemImageKept().c_str());
_ui->general_checkBox_keepBinaryData->setObjectName(Parameters::kMemBinDataKept().c_str());
_ui->general_checkBox_keepRehearsedNodes->setObjectName(Parameters::kMemRehearsedNodesKept().c_str());
_ui->general_checkBox_keepNotLinkedNodes->setObjectName(Parameters::kMemNotLinkedNodesKept().c_str());
_ui->general_spinBox_maxStMemSize->setObjectName(Parameters::kMemSTMSize().c_str());
_ui->doubleSpinBox_similarityThreshold->setObjectName(Parameters::kMemRehearsalSimilarity().c_str());
_ui->general_checkBox_SLAM_mode->setObjectName(Parameters::kMemIncrementalMemory().c_str());
@@ -476,6 +476,7 @@ PreferencesDialog::PreferencesDialog(QWidget * parent) :
_ui->graphPlan_goalReachedRadius->setObjectName(Parameters::kRGBDGoalReachedRadius().c_str());
_ui->graphPlan_planWithNearNodesLinked->setObjectName(Parameters::kRGBDPlanWithNearNodesLinked().c_str());
_ui->graphPlan_goalsSavedInUserData->setObjectName(Parameters::kRGBDGoalsSavedInUserData().c_str());
_ui->groupBox_localDetection_time->setObjectName(Parameters::kRGBDLocalLoopDetectionTime().c_str());
_ui->groupBox_localDetection_space->setObjectName(Parameters::kRGBDLocalLoopDetectionSpace().c_str());

14
guilib/src/ui/exportDialog.ui
View File

@@ -6,8 +6,8 @@
<rect>
<x>0</x>
<y>0</y>
<width>371</width>
<height>236</height>
<width>382</width>
<height>291</height>
</rect>
</property>
<property name="windowTitle">
@@ -125,6 +125,16 @@
</property>
</widget>
</item>
<item>
<widget class="QCheckBox" name="checkBox_userData">
<property name="text">
<string>User data (planned goals)</string>
</property>
<property name="checked">
<bool>false</bool>
</property>
</widget>
</item>
<item>
<layout class="QHBoxLayout" name="horizontalLayout">
<item>

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

@@ -27,7 +27,7 @@
<x>0</x>
<y>0</y>
<width>1012</width>
<height>21</height>
<height>25</height>
</rect>
</property>
<widget class="QMenu" name="menuFile">
@@ -61,6 +61,7 @@
<addaction name="actionGenerate_local_map"/>
<addaction name="actionGenerate_TORO_graph_graph"/>
<addaction name="actionPrint_loop_closure_IDs_to_console"/>
<addaction name="actionSend_goal"/>
</widget>
<addaction name="actionDownload_all_clouds"/>
<addaction name="actionDownload_graph"/>
@@ -1215,6 +1216,11 @@
<string>StereoFlyCapture2</string>
</property>
</action>
<action name="actionSend_goal">
<property name="text">
<string>Send a goal...</string>
</property>
</action>
</widget>
<customwidgets>
<customwidget>

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

@@ -7,7 +7,7 @@
<x>0</x>
<y>0</y>
<width>1035</width>
<height>716</height>
<height>702</height>
</rect>
</property>
<property name="sizePolicy">
@@ -64,8 +64,8 @@
<rect>
<x>0</x>
<y>0</y>
<width>736</width>
<height>781</height>
<width>737</width>
<height>948</height>
</rect>
</property>
<layout class="QVBoxLayout" name="verticalLayout_16">
@@ -86,7 +86,7 @@
<enum>QFrame::Raised</enum>
</property>
<property name="currentIndex">
<number>0</number>
<number>8</number>
</property>
<widget class="QWidget" name="page_22">
<layout class="QVBoxLayout" name="verticalLayout_29">
@@ -3140,7 +3140,7 @@ see Sqlite3 doc 'PRAGMA temp_store'.</string>
<item row="1" column="1">
<widget class="QLabel" name="label_keepRehearsed">
<property name="text">
<string>Keep rehearsed locations.</string>
<string>Keep not linked nodes in db (rehearsed nodes and deleted nodes are saved).</string>
</property>
<property name="wordWrap">
<bool>true</bool>
@@ -3148,7 +3148,7 @@ see Sqlite3 doc 'PRAGMA temp_store'.</string>
</widget>
</item>
<item row="1" column="0">
<widget class="QCheckBox" name="general_checkBox_keepRehearsedNodes">
<widget class="QCheckBox" name="general_checkBox_keepNotLinkedNodes">
<property name="text">
<string/>
</property>
@@ -3280,7 +3280,7 @@ generate the number of words requested.</string>
<item row="2" column="0">
<widget class="QSpinBox" name="surf_spinBox_wordsPerImageTarget">
<property name="minimum">
<number>0</number>
<number>-1</number>
</property>
<property name="maximum">
<number>2000</number>
@@ -3299,7 +3299,7 @@ Otherwise, the threshold is modified to
generate the number of words requested.</string>
</property>
<property name="text">
<string>Maximum words per image.</string>
<string>Maximum words per image (0=no maximum). Setting to -1 will disable features extraction, so disabling loop closure detection indirectly.</string>
</property>
<property name="wordWrap">
<bool>true</bool>
@@ -5290,6 +5290,23 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag
</property>
</widget>
</item>
<item row="2" column="1">
<widget class="QLabel" name="label_space3_5">
<property name="text">
<string>When a goal is received and processed with success, it is saved in user data of the location with this format: &quot;GOAL:#&quot;.</string>
</property>
<property name="wordWrap">
<bool>true</bool>
</property>
</widget>
</item>
<item row="2" column="0">
<widget class="QCheckBox" name="graphPlan_goalsSavedInUserData">
<property name="text">
<string/>
</property>
</widget>
</item>
</layout>
</item>
</layout>