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 a quality odometry threshold, turning the background yellow when the mapping area has too low features

Browse Source 
git-svn-id: http://rtabmap.googlecode.com/svn/trunk/rtabmap@1334 f169173b-cf89-36c8-b27e-44dbe73f0c83
This commit is contained in:
matlabbe
2014-06-06 18:06:49 +00:00
parent 363901cfe4
commit df0f9d60d5
12 changed files with 191 additions and 55 deletions
Download Patch File Download Diff File Expand all files Collapse all files

4
guilib/include/rtabmap/gui/MainWindow.h
View File

@@ -126,7 +126,7 @@ private slots:
void selectScreenCaptureFormat(bool checked);
void takeScreenshot();
void updateElapsedTime();
void processOdometry(const rtabmap::Image & data);
void processOdometry(const rtabmap::Image & data, int quality);
void applyAllPrefSettings();
void applyPrefSettings(PreferencesDialog::PANEL_FLAGS flags);
void applyPrefSettings(const rtabmap::ParametersMap & parameters);
@@ -156,7 +156,7 @@ private slots:
signals:
void statsReceived(const rtabmap::Statistics &);
void odometryReceived(const rtabmap::Image &);
void odometryReceived(const rtabmap::Image &, int);
void thresholdsChanged(int, int);
void stateChanged(MainWindow::State);
void rtabmapEventInitReceived(int status, const QString & info);

6
guilib/include/rtabmap/gui/OdometryViewer.h
View File

@@ -23,7 +23,7 @@ class RTABMAPGUI_EXP OdometryViewer : public CloudViewer, public UEventsHandler
Q_OBJECT
public:
OdometryViewer(int maxClouds = 10, int decimation = 2, float voxelSize = 0.0f, QWidget * parent = 0);
OdometryViewer(int maxClouds = 10, int decimation = 2, float voxelSize = 0.0f, int qualityWarningThr=0, QWidget * parent = 0);
virtual ~OdometryViewer() {}
protected:
@@ -35,11 +35,13 @@ private slots:
private:
UMutex dataMutex_;
std::list<rtabmap::Image> buffer_;
std::list<rtabmap::Image> data_;
int dataQuality_;
UTimer timer_;
int maxClouds_;
float voxelSize_;
int decimation_;
int qualityWarningThr_;
int id_;
std::map<int, pcl::PointCloud<pcl::PointXYZRGB>::Ptr > clouds_;
QAction * _aSetVoxelSize;

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

@@ -113,6 +113,7 @@ public:
bool imageHighestHypShown() const;
bool beepOnPause() const;
int getKeypointsOpacity() const;
int getOdomQualityWarnThr() const;
bool isCloudMeshing(int index) const; // 0=map
bool isCloudsShown(int index) const; // 0=map, 1=odom, 2=save

14
guilib/src/MainWindow.cpp
View File

@@ -328,7 +328,7 @@ MainWindow::MainWindow(PreferencesDialog * prefDialog, QWidget * parent) :
connect(this, SIGNAL(statsReceived(rtabmap::Statistics)), this, SLOT(processStats(rtabmap::Statistics)));
qRegisterMetaType<rtabmap::Image>("rtabmap::Image");
connect(this, SIGNAL(odometryReceived(rtabmap::Image)), this, SLOT(processOdometry(rtabmap::Image)));
connect(this, SIGNAL(odometryReceived(rtabmap::Image, int)), this, SLOT(processOdometry(rtabmap::Image, int)));
connect(this, SIGNAL(noMoreImagesReceived()), this, SLOT(stopDetection()));
@@ -527,7 +527,7 @@ void MainWindow::handleEvent(UEvent* anEvent)
!_processingStatistics)
{
_lastOdometryProcessed = false; // if we receive too many odometry events!
emit odometryReceived(odomEvent->data());
emit odometryReceived(odomEvent->data(), odomEvent->quality());
}
}
else if(anEvent->getClassName().compare("ULogEvent") == 0)
@@ -550,7 +550,7 @@ void MainWindow::handleEvent(UEvent* anEvent)
}
}
void MainWindow::processOdometry(const rtabmap::Image & data)
void MainWindow::processOdometry(const rtabmap::Image & data, int quality)
{
Transform pose = data.pose();
if(pose.isNull())
@@ -560,11 +560,19 @@ void MainWindow::processOdometry(const rtabmap::Image & data)
pose = _lastOdomPose;
}
else if(quality &&
_preferencesDialog->getOdomQualityWarnThr() &&
quality < _preferencesDialog->getOdomQualityWarnThr())
{
UDEBUG("odom warn, quality=%d thr=%d", quality, _preferencesDialog->getOdomQualityWarnThr());
_ui->widget_cloudViewer->setBackgroundColor(Qt::darkYellow);
}
else
{
UDEBUG("odom ok");
_ui->widget_cloudViewer->setBackgroundColor(Qt::black);
}
_ui->statsToolBox->updateStat("/Odom inliers/", (float)data.id(), (float)quality);
if(!pose.isNull())
{
_lastOdomPose = pose;

28
guilib/src/OdometryViewer.cpp
View File

@@ -21,11 +21,12 @@
namespace rtabmap {
OdometryViewer::OdometryViewer(int maxClouds, int decimation, float voxelSize, QWidget * parent) :
OdometryViewer::OdometryViewer(int maxClouds, int decimation, float voxelSize, int qualityWarningThr, QWidget * parent) :
CloudViewer(parent),
maxClouds_(maxClouds),
voxelSize_(voxelSize),
decimation_(decimation),
qualityWarningThr_(qualityWarningThr),
id_(0),
_aSetVoxelSize(0),
_aSetDecimation(0),
@@ -48,11 +49,14 @@ OdometryViewer::OdometryViewer(int maxClouds, int decimation, float voxelSize, Q
void OdometryViewer::processData()
{
rtabmap::Image data;
int quality;
dataMutex_.lock();
if(buffer_.size())
if(data_.size())
{
data = buffer_.back();
buffer_.clear();
data = data_.back();
data_.clear();
quality = dataQuality_;
dataQuality_ = 0;
}
dataMutex_.unlock();
@@ -96,7 +100,14 @@ void OdometryViewer::processData()
this->updateCameraPosition(data.pose());
this->setBackgroundColor(Qt::black);
if(qualityWarningThr_ && quality && quality < qualityWarningThr_)
{
this->setBackgroundColor(Qt::darkYellow);
}
else
{
this->setBackgroundColor(Qt::black);
}
this->render();
}
@@ -114,15 +125,16 @@ void OdometryViewer::handleEvent(UEvent * event)
{
bool empty = false;
dataMutex_.lock();
if(buffer_.empty())
if(data_.empty())
{
buffer_.push_back(odomEvent->data());
data_.push_back(odomEvent->data());
empty= true;
}
else
{
buffer_.back() = odomEvent->data();
data_.back() = odomEvent->data();
}
dataQuality_ = odomEvent->quality();
dataMutex_.unlock();
if(empty)
{

10
guilib/src/PreferencesDialog.cpp
View File

@@ -394,6 +394,7 @@ PreferencesDialog::PreferencesDialog(QWidget * parent) :
_ui->odom_iterations->setObjectName(Parameters::kOdomIterations().c_str());
_ui->odom_maxDepth->setObjectName(Parameters::kOdomMaxDepth().c_str());
_ui->odom_minInliers->setObjectName(Parameters::kOdomMinInliers().c_str());
_ui->odom_ratio->setObjectName(Parameters::kOdomWordsRatio().c_str());
_ui->stackedWidget_odom->setCurrentIndex(_ui->odom_type->currentIndex());
connect(_ui->odom_type, SIGNAL(currentIndexChanged(int)), _ui->stackedWidget_odom, SLOT(setCurrentIndex(int)));
@@ -705,6 +706,7 @@ void PreferencesDialog::resetSettings(QGroupBox * groupBox)
_ui->checkBox_imageRejectedShown->setChecked(true);
_ui->checkBox_imageHighestHypShown->setChecked(false);
_ui->horizontalSlider_keypointsOpacity->setSliderPosition(20);
_ui->spinBox_odomQualityWarnThr->setValue(50);
}
else if(groupBox->objectName() == _ui->groupBox_cloudRendering1->objectName())
{
@@ -718,7 +720,7 @@ void PreferencesDialog::resetSettings(QGroupBox * groupBox)
if(i<2)
{
_3dRenderingOpacity[i]->setValue(i==0?0.6:1.0);
_3dRenderingOpacity[i]->setValue(1.0);
_3dRenderingPtSize[i]->setValue(i==0?1:2);
_3dRenderingOpacityScan[i]->setValue(1.0);
_3dRenderingPtSizeScan[i]->setValue(1);
@@ -2330,6 +2332,10 @@ int PreferencesDialog::getKeypointsOpacity() const
{
return _ui->horizontalSlider_keypointsOpacity->value();
}
int PreferencesDialog::getOdomQualityWarnThr() const
{
return _ui->spinBox_odomQualityWarnThr->value();
}
bool PreferencesDialog::isCloudsShown(int index) const
{
@@ -2767,7 +2773,7 @@ void PreferencesDialog::testOdometry(OdomType type)
window->setMinimumHeight(600);
connect( window, SIGNAL(destroyed(QObject*)), this, SLOT(cleanOdometryTest()) );
OdometryViewer * odomViewer = new OdometryViewer(10, 2, 0.0, window);
OdometryViewer * odomViewer = new OdometryViewer(10, 2, 0.0, this->getOdomQualityWarnThr(), window);
QVBoxLayout *layout = new QVBoxLayout();
layout->addWidget(odomViewer);

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

@@ -7,7 +7,7 @@
<x>0</x>
<y>0</y>
<width>1027</width>
<height>494</height>
<height>659</height>
</rect>
</property>
<property name="sizePolicy">
@@ -64,8 +64,8 @@
<rect>
<x>0</x>
<y>0</y>
<width>724</width>
<height>1076</height>
<width>729</width>
<height>823</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>15</number>
</property>
<widget class="QWidget" name="page_22">
<layout class="QVBoxLayout" name="verticalLayout_29">
@@ -143,12 +143,9 @@
<item>
<widget class="QGroupBox" name="groupBox_5">
<property name="title">
<string>Image view</string>
<string>Loop closure detection view</string>
</property>
<layout class="QFormLayout" name="formLayout_12">
<property name="fieldGrowthPolicy">
<enum>QFormLayout::AllNonFixedFieldsGrow</enum>
</property>
<item row="1" column="0">
<widget class="QCheckBox" name="checkBox_imageFlipped">
<property name="text">
@@ -235,7 +232,7 @@
<item row="0" column="1">
<widget class="QLabel" name="label_70">
<property name="text">
<string>Vertical layout used for the loop closure detection window.</string>
<string>Vertical layout.</string>
</property>
<property name="wordWrap">
<bool>false</bool>
@@ -255,6 +252,36 @@
</layout>
</widget>
</item>
<item>
<widget class="QGroupBox" name="groupBox">
<property name="title">
<string>3D Map view</string>
</property>
<layout class="QFormLayout" name="formLayout_23">
<item row="0" column="0">
<widget class="QSpinBox" name="spinBox_odomQualityWarnThr">
<property name="maximum">
<number>9999</number>
</property>
<property name="value">
<number>50</number>
</property>
</widget>
</item>
<item row="0" column="1">
<widget class="QLabel" name="label_86">
<property name="text">
<string>Odometry warning theshold:
Show a yellow background when the number of odometry inliers goes under this threshold. If 0, it is ignored. You can see the current odometry inliers count under Statistics view -&gt; General -&gt; Odom inliers.</string>
</property>
<property name="wordWrap">
<bool>true</bool>
</property>
</widget>
</item>
</layout>
</widget>
</item>
<item>
<widget class="QGroupBox" name="groupBox_2">
<property name="title">
@@ -598,7 +625,7 @@ High-res view</string>
<double>0.100000000000000</double>
</property>
<property name="value">
<double>0.600000000000000</double>
<double>1.000000000000000</double>
</property>
</widget>
</item>
@@ -4963,7 +4990,7 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag
</property>
</widget>
</item>
<item row="9" column="1">
<item row="10" column="1">
<widget class="QLabel" name="label_104">
<property name="text">
<string>Max feature depth. For ICP, it is the max cloud depth.</string>
@@ -4973,7 +5000,7 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag
</property>
</widget>
</item>
<item row="9" column="0">
<item row="10" column="0">
<widget class="QDoubleSpinBox" name="odom_maxDepth">
<property name="suffix">
<string> m</string>
@@ -5009,6 +5036,38 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag
</property>
</widget>
</item>
<item row="9" column="1">
<widget class="QLabel" name="label_90">
<property name="text">
<string>Minmum ratio of keypoints between the current image and the last image to compute odometry.</string>
</property>
<property name="wordWrap">
<bool>true</bool>
</property>
</widget>
</item>
<item row="9" column="0">
<widget class="QDoubleSpinBox" name="odom_ratio">
<property name="suffix">
<string/>
</property>
<property name="decimals">
<number>2</number>
</property>
<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>0.500000000000000</double>
</property>
</widget>
</item>
</layout>
</item>
<item>