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

Added PnP Pose estimation based Odometry option. Added sensor icons in menu.

Browse Source
This commit is contained in:
Mathieu Labbe
2015-03-31 15:45:38 -04:00
parent 7e216bf842
commit 6089a44589
19 changed files with 1088 additions and 453 deletions
Download Patch File Download Diff File Expand all files Collapse all files

4
guilib/src/GuiLib.qrc
View File

@@ -23,5 +23,9 @@
<file>images/document-save.png</file>
<file>images/document-properties.png</file>
<file>images/system-log-out.png</file>
<file>images/kinect_xbox_360.png</file>
<file>images/kinect_xbox_one.png</file>
<file>images/sense.png</file>
<file>images/xtion_pro_live.png</file>
</qresource>
</RCC>

6
guilib/src/ImageView.cpp
View File

@@ -148,6 +148,12 @@ bool ImageView::isGraphicsViewScaled() const
return _graphicsViewScaled->isChecked();
}
const QColor & ImageView::getBackgroundColor() const
{
return _graphicsView->backgroundBrush().color();
}
void ImageView::setFeaturesShown(bool shown)
{
_showFeatures->setChecked(shown);

134
guilib/src/MainWindow.cpp
View File

@@ -76,6 +76,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
#include <QtCore/QProcess>
#include <QSplashScreen>
#include <QInputDialog>
#include <QToolButton>
//RGB-D stuff
#include "rtabmap/core/CameraRGBD.h"
@@ -127,6 +128,7 @@ MainWindow::MainWindow(PreferencesDialog * prefDialog, QWidget * parent) :
_odomImageDepthShow(false),
_odometryCorrection(Transform::getIdentity()),
_processingOdometry(false),
_lastOdomInfoUpdateTime(0),
_oneSecondTimer(0),
_elapsedTime(0),
_posteriorCurve(0),
@@ -265,7 +267,9 @@ MainWindow::MainWindow(PreferencesDialog * prefDialog, QWidget * parent) :
_ui->menuShow_view->addAction(_ui->dockWidget_graphViewer->toggleViewAction());
_ui->menuShow_view->addAction(_ui->dockWidget_odometry->toggleViewAction());
_ui->menuShow_view->addAction(_ui->toolBar->toggleViewAction());
_ui->toolBar->setWindowTitle(tr("Control toolbar"));
_ui->toolBar->setWindowTitle(tr("File toolbar"));
_ui->menuShow_view->addAction(_ui->toolBar_2->toggleViewAction());
_ui->toolBar_2->setWindowTitle(tr("Control toolbar"));
QAction * a = _ui->menuShow_view->addAction("Progress dialog");
a->setCheckable(false);
connect(a, SIGNAL(triggered(bool)), _initProgressDialog, SLOT(show()));
@@ -321,6 +325,13 @@ MainWindow::MainWindow(PreferencesDialog * prefDialog, QWidget * parent) :
_ui->actionReset_Odometry->setEnabled(false);
_ui->actionPost_processing->setEnabled(false);
QToolButton* toolButton = new QToolButton(this);
toolButton->setMenu(_ui->menuRGB_D_camera);
toolButton->setPopupMode(QToolButton::InstantPopup);
toolButton->setIcon(QIcon(":images/kinect_xbox_360.png"));
toolButton->setToolTip("Select sensor driver");
_ui->toolBar->addWidget(toolButton)->setObjectName("toolbar_source");
#if defined(Q_WS_MAC) || defined(Q_WS_WIN)
connect(_ui->actionOpen_working_directory, SIGNAL(triggered()), SLOT(openWorkingDirectory()));
#else
@@ -632,11 +643,16 @@ void MainWindow::handleEvent(UEvent* anEvent)
}
else if(anEvent->getClassName().compare("OdometryEvent") == 0)
{
OdometryEvent * odomEvent = (OdometryEvent*)anEvent;
if(!_processingOdometry && !_processingStatistics)
// limit 10 Hz max
if(UTimer::now() - _lastOdomInfoUpdateTime > 0.1)
{
_processingOdometry = true; // if we receive too many odometry events!
emit odometryReceived(odomEvent->data(), odomEvent->info());
_lastOdomInfoUpdateTime = UTimer::now();
OdometryEvent * odomEvent = (OdometryEvent*)anEvent;
if(!_processingOdometry && !_processingStatistics)
{
_processingOdometry = true; // if we receive too many odometry events!
emit odometryReceived(odomEvent->data(), odomEvent->info());
}
}
}
else if(anEvent->getClassName().compare("ULogEvent") == 0)
@@ -4960,7 +4976,75 @@ void MainWindow::setMonitoringState(bool pauseChecked)
// Must be called by the GUI thread, use signal StateChanged()
void MainWindow::changeState(MainWindow::State newState)
{
// TODO : To protect with mutex ?
bool monitoring = newState==kMonitoring || newState == kMonitoringPaused;
_ui->actionNew_database->setVisible(!monitoring);
_ui->actionOpen_database->setVisible(!monitoring);
_ui->actionClose_database->setVisible(!monitoring);
_ui->actionEdit_database->setVisible(!monitoring);
_ui->actionStart->setVisible(!monitoring);
_ui->actionStop->setVisible(!monitoring);
_ui->actionDump_the_memory->setVisible(!monitoring);
_ui->actionDump_the_prediction_matrix->setVisible(!monitoring);
_ui->actionGenerate_map->setVisible(!monitoring);
_ui->actionGenerate_local_map->setVisible(!monitoring);
_ui->actionGenerate_TORO_graph_graph->setVisible(!monitoring);
_ui->actionOpen_working_directory->setVisible(!monitoring);
_ui->actionData_recorder->setVisible(!monitoring);
_ui->menuSelect_source->menuAction()->setVisible(!monitoring);
_ui->doubleSpinBox_stats_imgRate->setVisible(!monitoring);
_ui->doubleSpinBox_stats_imgRate_label->setVisible(!monitoring);
_ui->toolBar->setVisible(!monitoring);
_ui->toolBar->toggleViewAction()->setVisible(!monitoring);
QList<QAction*> actions = _ui->menuTools->actions();
for(int i=0; i<actions.size(); ++i)
{
if(actions.at(i)->isSeparator())
{
actions.at(i)->setVisible(!monitoring);
}
}
actions = _ui->menuFile->actions();
if(actions.size()>=9)
{
if(actions.at(2)->isSeparator())
{
actions.at(2)->setVisible(!monitoring);
}
else
{
UWARN("Menu File separators have not the same order.");
}
if(actions.at(8)->isSeparator())
{
actions.at(8)->setVisible(!monitoring);
}
else
{
UWARN("Menu File separators have not the same order.");
}
}
else
{
UWARN("Menu File separators have not the same order.");
}
actions = _ui->menuProcess->actions();
if(actions.size()>=2)
{
if(actions.at(1)->isSeparator())
{
actions.at(1)->setVisible(!monitoring);
}
else
{
UWARN("Menu File separators have not the same order.");
}
}
else
{
UWARN("Menu File separators have not the same order.");
}
switch (newState)
{
case kIdle: // RTAB-Map is not initialized yet
@@ -4983,10 +5067,10 @@ void MainWindow::changeState(MainWindow::State newState)
_ui->actionGenerate_map->setEnabled(false);
_ui->actionGenerate_local_map->setEnabled(false);
_ui->actionGenerate_TORO_graph_graph->setEnabled(false);
_ui->actionOpen_working_directory->setEnabled(true);
_ui->actionDownload_all_clouds->setEnabled(false);
_ui->actionDownload_graph->setEnabled(false);
_ui->menuSelect_source->setEnabled(false);
_ui->toolBar->findChild<QAction*>("toolbar_source")->setEnabled(false);
_ui->actionTrigger_a_new_map->setEnabled(false);
_ui->doubleSpinBox_stats_imgRate->setEnabled(true);
_ui->statusbar->clearMessage();
@@ -5030,10 +5114,10 @@ void MainWindow::changeState(MainWindow::State newState)
_ui->actionGenerate_map->setEnabled(true);
_ui->actionGenerate_local_map->setEnabled(true);
_ui->actionGenerate_TORO_graph_graph->setEnabled(true);
_ui->actionOpen_working_directory->setEnabled(true);
_ui->actionDownload_all_clouds->setEnabled(true);
_ui->actionDownload_graph->setEnabled(true);
_ui->menuSelect_source->setEnabled(true);
_ui->toolBar->findChild<QAction*>("toolbar_source")->setEnabled(true);
_ui->actionTrigger_a_new_map->setEnabled(true);
_ui->doubleSpinBox_stats_imgRate->setEnabled(true);
_ui->statusbar->clearMessage();
@@ -5066,10 +5150,10 @@ void MainWindow::changeState(MainWindow::State newState)
_ui->actionGenerate_map->setEnabled(false);
_ui->actionGenerate_local_map->setEnabled(false);
_ui->actionGenerate_TORO_graph_graph->setEnabled(false);
_ui->actionOpen_working_directory->setEnabled(true);
_ui->actionDownload_all_clouds->setEnabled(false);
_ui->actionDownload_graph->setEnabled(false);
_ui->menuSelect_source->setEnabled(false);
_ui->toolBar->findChild<QAction*>("toolbar_source")->setEnabled(false);
_ui->actionTrigger_a_new_map->setEnabled(true);
_ui->doubleSpinBox_stats_imgRate->setEnabled(true);
_ui->statusbar->showMessage(tr("Detecting..."));
@@ -5150,34 +5234,18 @@ void MainWindow::changeState(MainWindow::State newState)
}
break;
case kMonitoring:
_ui->actionNew_database->setVisible(false);
_ui->actionOpen_database->setVisible(false);
_ui->actionClose_database->setVisible(false);
_ui->actionEdit_database->setVisible(false);
_ui->actionStart->setVisible(false);
_ui->actionPause->setEnabled(true);
_ui->actionPause->setChecked(false);
_ui->actionPause->setToolTip(tr("Pause"));
_ui->actionStop->setVisible(false);
_ui->actionPause_on_match->setEnabled(true);
_ui->actionPause_on_local_loop_detection->setEnabled(true);
_ui->actionPause_when_a_loop_hypothesis_is_rejected->setEnabled(true);
_ui->actionReset_Odometry->setEnabled(true);
_ui->actionPost_processing->setEnabled(false);
_ui->actionDump_the_memory->setVisible(false);
_ui->actionDump_the_prediction_matrix->setVisible(false);
_ui->actionDelete_memory->setEnabled(true);
_ui->actionGenerate_map->setVisible(false);
_ui->actionGenerate_local_map->setVisible(false);
_ui->actionGenerate_TORO_graph_graph->setVisible(false);
_ui->actionData_recorder->setVisible(false);
_ui->actionOpen_working_directory->setEnabled(false);
_ui->actionDownload_all_clouds->setEnabled(true);
_ui->actionDownload_graph->setEnabled(true);
_ui->menuSelect_source->setVisible(false);
_ui->actionTrigger_a_new_map->setEnabled(true);
_ui->doubleSpinBox_stats_imgRate->setVisible(false);
_ui->doubleSpinBox_stats_imgRate_label->setVisible(false);
_ui->statusbar->showMessage(tr("Monitoring..."));
_state = newState;
_elapsedTime->start();
@@ -5185,34 +5253,18 @@ void MainWindow::changeState(MainWindow::State newState)
this->post(new RtabmapEventCmd(RtabmapEventCmd::kCmdPause, "", 0));
break;
case kMonitoringPaused:
_ui->actionNew_database->setVisible(false);
_ui->actionOpen_database->setVisible(false);
_ui->actionClose_database->setVisible(false);
_ui->actionEdit_database->setVisible(false);
_ui->actionStart->setVisible(false);
_ui->actionPause->setToolTip(tr("Continue"));
_ui->actionPause->setChecked(true);
_ui->actionPause->setEnabled(true);
_ui->actionStop->setVisible(false);
_ui->actionPause_on_match->setEnabled(true);
_ui->actionPause_on_local_loop_detection->setEnabled(true);
_ui->actionPause_when_a_loop_hypothesis_is_rejected->setEnabled(true);
_ui->actionReset_Odometry->setEnabled(true);
_ui->actionPost_processing->setEnabled(_cachedSignatures.size() >= 2 && _currentPosesMap.size() >= 2 && _currentLinksMap.size() >= 1);
_ui->actionDump_the_memory->setVisible(false);
_ui->actionDump_the_prediction_matrix->setVisible(false);
_ui->actionDelete_memory->setEnabled(true);
_ui->actionGenerate_map->setVisible(false);
_ui->actionGenerate_local_map->setVisible(false);
_ui->actionGenerate_TORO_graph_graph->setVisible(false);
_ui->actionData_recorder->setVisible(false);
_ui->actionOpen_working_directory->setEnabled(false);
_ui->actionDownload_all_clouds->setEnabled(true);
_ui->actionDownload_graph->setEnabled(true);
_ui->menuSelect_source->setVisible(false);
_ui->actionTrigger_a_new_map->setEnabled(true);
_ui->doubleSpinBox_stats_imgRate->setVisible(false);
_ui->doubleSpinBox_stats_imgRate_label->setVisible(false);
_ui->statusbar->showMessage(tr("Monitoring paused..."));
_state = newState;
_oneSecondTimer->stop();

3
guilib/src/PreferencesDialog.cpp
View File

@@ -522,6 +522,9 @@ PreferencesDialog::PreferencesDialog(QWidget * parent) :
_ui->odom_force2D->setObjectName(Parameters::kOdomForce2D().c_str());
_ui->odom_fillInfoData->setObjectName(Parameters::kOdomFillInfoData().c_str());
_ui->lineEdit_odom_roi->setObjectName(Parameters::kOdomRoiRatios().c_str());
_ui->odom_pnpEstimation->setObjectName(Parameters::kOdomPnPEstimation().c_str());
_ui->odom_pnpReprojError->setObjectName(Parameters::kOdomPnPReprojError().c_str());
_ui->odom_pnpFlags->setObjectName(Parameters::kOdomPnPFlags().c_str());
//Odometry BOW
_ui->odom_localHistory->setObjectName(Parameters::kOdomBowLocalHistorySize().c_str());

BIN
guilib/src/images/kinect_xbox_360.png Normal file
View File

Binary file not shown.
Side by Side

After

Width:  |  Height:  |  Size: 4.1 KiB

BIN
guilib/src/images/kinect_xbox_one.png Normal file
View File

Binary file not shown.
Side by Side

After

Width:  |  Height:  |  Size: 5.0 KiB

BIN
guilib/src/images/sense.png Normal file
View File

Binary file not shown.
Side by Side

After

Width:  |  Height:  |  Size: 9.0 KiB

BIN
guilib/src/images/xtion_pro_live.png Normal file
View File

Binary file not shown.
Side by Side

After

Width:  |  Height:  |  Size: 4.3 KiB

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

@@ -27,7 +27,7 @@
<x>0</x>
<y>0</y>
<width>1012</width>
<height>22</height>
<height>25</height>
</rect>
</property>
<widget class="QMenu" name="menuFile">
@@ -106,6 +106,10 @@
<property name="title">
<string>Kinect</string>
</property>
<property name="icon">
<iconset resource="../GuiLib.qrc">
<normaloff>:/images/kinect_xbox_360.png</normaloff>:/images/kinect_xbox_360.png</iconset>
</property>
<addaction name="actionFreenect"/>
<addaction name="actionOpenNI2_kinect"/>
<addaction name="actionOpenNI_PCL"/>
@@ -115,6 +119,10 @@
<property name="title">
<string>Xtion PRO LIVE</string>
</property>
<property name="icon">
<iconset resource="../GuiLib.qrc">
<normaloff>:/images/xtion_pro_live.png</normaloff>:/images/xtion_pro_live.png</iconset>
</property>
<addaction name="actionOpenNI2"/>
<addaction name="actionOpenNI_PCL_ASUS"/>
<addaction name="actionOpenNI_CV_ASUS"/>
@@ -123,6 +131,10 @@
<property name="title">
<string>Sense 3D scanner</string>
</property>
<property name="icon">
<iconset resource="../GuiLib.qrc">
<normaloff>:/images/sense.png</normaloff>:/images/sense.png</iconset>
</property>
<addaction name="actionOpenNI2_Sense"/>
</widget>
<addaction name="menuKinect_for_Xbox_360"/>
@@ -221,16 +233,7 @@
<property name="spacing">
<number>0</number>
</property>
<property name="leftMargin">
<number>0</number>
</property>
<property name="topMargin">
<number>0</number>
</property>
<property name="rightMargin">
<number>0</number>
</property>
<property name="bottomMargin">
<property name="margin">
<number>0</number>
</property>
<item>
@@ -249,13 +252,9 @@
<attribute name="toolBarBreak">
<bool>false</bool>
</attribute>
<addaction name="actionClose_database"/>
<addaction name="actionNew_database"/>
<addaction name="actionOpen_database"/>
<addaction name="actionClose_database"/>
<addaction name="separator"/>
<addaction name="actionStart"/>
<addaction name="actionPause"/>
<addaction name="actionStop"/>
</widget>
<widget class="QDockWidget" name="dockWidget_statsV2">
<property name="floating">
@@ -272,16 +271,7 @@
<property name="spacing">
<number>0</number>
</property>
<property name="leftMargin">
<number>0</number>
</property>
<property name="topMargin">
<number>0</number>
</property>
<property name="rightMargin">
<number>0</number>
</property>
<property name="bottomMargin">
<property name="margin">
<number>0</number>
</property>
<item>
@@ -524,16 +514,7 @@
<property name="spacing">
<number>0</number>
</property>
<property name="leftMargin">
<number>0</number>
</property>
<property name="topMargin">
<number>0</number>
</property>
<property name="rightMargin">
<number>0</number>
</property>
<property name="bottomMargin">
<property name="margin">
<number>0</number>
</property>
<item>
@@ -554,16 +535,7 @@
<property name="spacing">
<number>0</number>
</property>
<property name="leftMargin">
<number>0</number>
</property>
<property name="topMargin">
<number>0</number>
</property>
<property name="rightMargin">
<number>0</number>
</property>
<property name="bottomMargin">
<property name="margin">
<number>0</number>
</property>
<item>
@@ -584,16 +556,7 @@
<property name="spacing">
<number>0</number>
</property>
<property name="leftMargin">
<number>0</number>
</property>
<property name="topMargin">
<number>0</number>
</property>
<property name="rightMargin">
<number>0</number>
</property>
<property name="bottomMargin">
<property name="margin">
<number>0</number>
</property>
<item>
@@ -614,16 +577,7 @@
<property name="spacing">
<number>0</number>
</property>
<property name="leftMargin">
<number>0</number>
</property>
<property name="topMargin">
<number>0</number>
</property>
<property name="rightMargin">
<number>0</number>
</property>
<property name="bottomMargin">
<property name="margin">
<number>0</number>
</property>
<item>
@@ -644,16 +598,7 @@
<property name="spacing">
<number>0</number>
</property>
<property name="leftMargin">
<number>0</number>
</property>
<property name="topMargin">
<number>0</number>
</property>
<property name="rightMargin">
<number>0</number>
</property>
<property name="bottomMargin">
<property name="margin">
<number>0</number>
</property>
<item>
@@ -677,16 +622,7 @@
<property name="spacing">
<number>0</number>
</property>
<property name="leftMargin">
<number>0</number>
</property>
<property name="topMargin">
<number>0</number>
</property>
<property name="rightMargin">
<number>0</number>
</property>
<property name="bottomMargin">
<property name="margin">
<number>0</number>
</property>
<item>
@@ -707,16 +643,7 @@
<property name="spacing">
<number>0</number>
</property>
<property name="leftMargin">
<number>0</number>
</property>
<property name="topMargin">
<number>0</number>
</property>
<property name="rightMargin">
<number>0</number>
</property>
<property name="bottomMargin">
<property name="margin">
<number>0</number>
</property>
<item>
@@ -740,16 +667,7 @@
<property name="spacing">
<number>0</number>
</property>
<property name="leftMargin">
<number>0</number>
</property>
<property name="topMargin">
<number>0</number>
</property>
<property name="rightMargin">
<number>0</number>
</property>
<property name="bottomMargin">
<property name="margin">
<number>0</number>
</property>
<item>
@@ -819,16 +737,7 @@
<property name="spacing">
<number>0</number>
</property>
<property name="leftMargin">
<number>0</number>
</property>
<property name="topMargin">
<number>0</number>
</property>
<property name="rightMargin">
<number>0</number>
</property>
<property name="bottomMargin">
<property name="margin">
<number>0</number>
</property>
<item>
@@ -837,6 +746,20 @@
</layout>
</widget>
</widget>
<widget class="QToolBar" name="toolBar_2">
<property name="windowTitle">
<string>toolBar_2</string>
</property>
<attribute name="toolBarArea">
<enum>TopToolBarArea</enum>
</attribute>
<attribute name="toolBarBreak">
<bool>false</bool>
</attribute>
<addaction name="actionStart"/>
<addaction name="actionPause"/>
<addaction name="actionStop"/>
</widget>
<action name="actionExit">
<property name="icon">
<iconset resource="../GuiLib.qrc">

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

@@ -86,7 +86,7 @@
<enum>QFrame::Raised</enum>
</property>
<property name="currentIndex">
<number>1</number>
<number>23</number>
</property>
<widget class="QWidget" name="page_22">
<layout class="QVBoxLayout" name="verticalLayout_29">
@@ -6610,6 +6610,81 @@ Lower the ratio -&gt; higher the precision. 0 means disabled, matching the neare
</property>
</widget>
</item>
<item row="5" column="1">
<widget class="QLabel" name="label_225">
<property name="text">
<string>PnP reprojection error.</string>
</property>
<property name="wordWrap">
<bool>true</bool>
</property>
</widget>
</item>
<item row="5" column="0">
<widget class="QDoubleSpinBox" name="odom_pnpReprojError">
<property name="suffix">
<string> pix</string>
</property>
<property name="decimals">
<number>1</number>
</property>
<property name="minimum">
<double>0.100000000000000</double>
</property>
<property name="singleStep">
<double>1.000000000000000</double>
</property>
<property name="value">
<double>8.000000000000000</double>
</property>
</widget>
</item>
<item row="4" column="0">
<widget class="QCheckBox" name="odom_pnpEstimation">
<property name="text">
<string/>
</property>
</widget>
</item>
<item row="4" column="1">
<widget class="QLabel" name="label_226">
<property name="text">
<string>PnP RANSAC: Pose estimation from 2D to 3D correspondences instead of 3D to 3D correspondences. PnP uses &quot;Minimum feature correspondences&quot; and &quot;Maximum iterations&quot; above.</string>
</property>
<property name="wordWrap">
<bool>true</bool>
</property>
</widget>
</item>
<item row="6" column="1">
<widget class="QLabel" name="label_227">
<property name="text">
<string>PnP flags.</string>
</property>
<property name="wordWrap">
<bool>true</bool>
</property>
</widget>
</item>
<item row="6" column="0">
<widget class="QComboBox" name="odom_pnpFlags">
<item>
<property name="text">
<string>Iterative</string>
</property>
</item>
<item>
<property name="text">
<string>EPNP</string>
</property>
</item>
<item>
<property name="text">
<string>P3P</string>
</property>
</item>
</widget>
</item>
</layout>
</widget>
</item>