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:

Browse Source 
-Fixed map id not incremented on Odometry reset (identity transform was missed)
-Keeping maximum odometry variance between two loop closure updates
Odometry:
-removed parameters Odom/FeaturesRatio, Odom/LinearUpdate, Odom/AngularUpdate
-added parameter Odom/FillInfoData
-expended OdometryInfo class with features stuff
Gui:
-show inliers/outliers features in Odometry view
This commit is contained in:
Mathieu Labbe
2014-12-18 16:54:46 -05:00
parent 744e2fb3c7
commit 86a6fe202c
17 changed files with 325 additions and 382 deletions
Download Patch File Download Diff File Expand all files Collapse all files

44
guilib/src/ImageView.cpp
View File

@@ -201,6 +201,14 @@ void ImageView::updateOpacity()
_imageDepth->setGraphicsEffect(0);
}
}
else if(_image)
{
_image->setGraphicsEffect(0);
}
else if(_imageDepth)
{
_imageDepth->setGraphicsEffect(0);
}
}
void ImageView::updateZoom()
@@ -242,13 +250,12 @@ void ImageView::wheelEvent(QWheelEvent * e)
this->setMatrix(matrix);
}
void ImageView::setFeatures(const std::multimap<int, cv::KeyPoint> & refWords)
void ImageView::setFeatures(const std::multimap<int, cv::KeyPoint> & refWords, const QColor & color)
{
qDeleteAll(_features);
_features.clear();
rtabmap::KeypointItem * item = 0;
int alpha = 70;
for(std::multimap<int, cv::KeyPoint>::const_iterator i = refWords.begin(); i != refWords.end(); ++i )
{
const cv::KeyPoint & r = (*i).second;
@@ -260,9 +267,35 @@ void ImageView::setFeatures(const std::multimap<int, cv::KeyPoint> & refWords)
"X = %5\n"
"Y = %6\n"
"Size = %7").arg(id).arg(1).arg(r.angle).arg(r.response).arg(r.pt.x).arg(r.pt.y).arg(r.size);
float radius = r.size*1.2/9.*2;
float radius = r.size/2.0f;
item = new rtabmap::KeypointItem(r.pt.x-radius, r.pt.y-radius, radius*2, info, color);
item = new rtabmap::KeypointItem(r.pt.x-radius, r.pt.y-radius, radius*2, info, QColor(255, 255, 0, alpha));
scene()->addItem(item);
_features.insert(id, item);
item->setVisible(_showFeatures->isChecked());
item->setZValue(1);
}
}
void ImageView::setFeatures(const std::vector<cv::KeyPoint> & features, const QColor & color)
{
qDeleteAll(_features);
_features.clear();
rtabmap::KeypointItem * item = 0;
for(unsigned int i = 0; i< features.size(); ++i )
{
const cv::KeyPoint & r = features[i];
int id = i;
QString info = QString( "WordRef = %1\n"
"Laplacian = %2\n"
"Dir = %3\n"
"Hessian = %4\n"
"X = %5\n"
"Y = %6\n"
"Size = %7").arg(id).arg(1).arg(r.angle).arg(r.response).arg(r.pt.x).arg(r.pt.y).arg(r.size);
float radius = (r.size==0?3:r.size)/2.0f;
item = new rtabmap::KeypointItem(r.pt.x-radius, r.pt.y-radius, radius*2, info, color);
scene()->addItem(item);
_features.insert(id, item);
@@ -308,8 +341,7 @@ void ImageView::setFeatureColor(int id, const QColor & color)
{
for(int i=0; i<items.size(); ++i)
{
items[i]->setPen(QPen(color));
items[i]->setBrush(QBrush(color));
items[i]->setColor(color);
}
}
else

13
guilib/src/KeypointItem.cpp
View File

@@ -39,8 +39,7 @@ KeypointItem::KeypointItem(qreal x, qreal y, int r, const QString & info, const
_info(info),
_placeHolder(0)
{
this->setPen(QPen(color));
this->setBrush(QBrush(color));
this->setColor(color);
this->setAcceptsHoverEvents(true);
this->setFlag(QGraphicsItem::ItemIsFocusable, true);
_width = pen().width();
@@ -48,10 +47,16 @@ KeypointItem::KeypointItem(qreal x, qreal y, int r, const QString & info, const
KeypointItem::~KeypointItem()
{
/*if(_placeHolder)
if(_placeHolder)
{
delete _placeHolder;
}*/
}
}
void KeypointItem::setColor(const QColor & color)
{
this->setPen(QPen(color));
this->setBrush(QBrush(color));
}
void KeypointItem::showDescription()

52
guilib/src/MainWindow.cpp
View File

@@ -566,9 +566,7 @@ void MainWindow::handleEvent(UEvent* anEvent)
else if(anEvent->getClassName().compare("OdometryEvent") == 0)
{
OdometryEvent * odomEvent = (OdometryEvent*)anEvent;
if((_ui->dockWidget_cloudViewer->isVisible() || _ui->dockWidget_odometry->isVisible()) &&
_lastOdometryProcessed &&
!_processingStatistics)
if(_lastOdometryProcessed && !_processingStatistics)
{
_lastOdometryProcessed = false; // if we receive too many odometry events!
emit odometryReceived(odomEvent->data(), odomEvent->info());
@@ -596,6 +594,7 @@ void MainWindow::handleEvent(UEvent* anEvent)
void MainWindow::processOdometry(const rtabmap::SensorData & data, const rtabmap::OdometryInfo & info)
{
UTimer time;
Transform pose = data.pose();
bool lost = false;
_ui->imageView_odometry->resetTransform();
@@ -725,6 +724,16 @@ void MainWindow::processOdometry(const rtabmap::SensorData & data, const rtabmap
if(_ui->dockWidget_odometry->isVisible() &&
!data.image().empty())
{
int alpha = _preferencesDialog->getKeypointsOpacity()*255/100;
if(info.type == 0)
{
_ui->imageView_odometry->setFeatures(info.words, QColor(255,255,0, alpha));
}
else if(info.type == 1)
{
_ui->imageView_odometry->setFeatures(info.refCorners, QColor(255,0,0, alpha));
}
_ui->imageView_odometry->clearLines();
if(lost)
{
_ui->imageView_odometry->setImageDepth(uCvMat2QImage(data.image()));
@@ -736,6 +745,35 @@ void MainWindow::processOdometry(const rtabmap::SensorData & data, const rtabmap
_ui->imageView_odometry->setImage(uCvMat2QImage(data.image()));
_ui->imageView_odometry->setImageShown(true);
_ui->imageView_odometry->setImageDepthShown(false);
if(info.type == 0)
{
for(unsigned int i=0; i<info.wordMatches.size(); ++i)
{
_ui->imageView_odometry->setFeatureColor(info.wordMatches[i], QColor(255,0,0, alpha)); // outliers
}
for(unsigned int i=0; i<info.wordInliers.size(); ++i)
{
_ui->imageView_odometry->setFeatureColor(info.wordInliers[i], QColor(0,255,0, alpha)); // inliers
}
}
else if(info.type == 1)
{
//draw lines
UASSERT(info.refCorners.size() == info.newCorners.size());
for(unsigned int i=0; i<info.cornerInliers.size(); ++i)
{
_ui->imageView_odometry->setFeatureColor(info.cornerInliers[i], QColor(0,255,0, alpha)); // inliers
QGraphicsLineItem * item = _ui->imageView_odometry->scene()->addLine(
info.refCorners[info.cornerInliers[i]].pt.x,
info.refCorners[info.cornerInliers[i]].pt.y,
info.newCorners[info.cornerInliers[i]].pt.x,
info.newCorners[info.cornerInliers[i]].pt.y,
QPen(QColor(0, 0, 255, alpha)));
item->setVisible(_ui->imageView_odometry->isLinesShown());
item->setZValue(1);
}
}
}
_ui->imageView_odometry->resetZoom();
@@ -753,6 +791,8 @@ void MainWindow::processOdometry(const rtabmap::SensorData & data, const rtabmap
{
this->captureScreen();
}
_ui->statsToolBox->updateStat("/Gui refresh odom/ms", (float)data.id(), time.elapsed()*1000.0);
}
void MainWindow::processStats(const rtabmap::Statistics & stat)
@@ -1815,7 +1855,7 @@ void MainWindow::drawKeypoints(const std::multimap<int, cv::KeyPoint> & refWords
"X = %5\n"
"Y = %6\n"
"Size = %7").arg(id).arg(1).arg(r.angle).arg(r.response).arg(r.pt.x).arg(r.pt.y).arg(r.size);
float radius = r.size*1.2/9.*2;
float radius = r.size/2.0f;
if(uContains(loopWords, id))
{
// PINK = FOUND IN LOOP SIGNATURE
@@ -1864,7 +1904,7 @@ void MainWindow::drawKeypoints(const std::multimap<int, cv::KeyPoint> & refWords
"X = %5\n"
"Y = %6\n"
"Size = %7").arg(id).arg(1).arg(r.angle).arg(r.response).arg(r.pt.x).arg(r.pt.y).arg(r.size);
float radius = r.size*1.2/9.*2;
float radius = r.size/2.0f;
if(uContains(refWords, id))
{
// PINK = FOUND IN LOOP SIGNATURE
@@ -1945,7 +1985,7 @@ void MainWindow::resizeEvent(QResizeEvent* anEvent)
{
_ui->imageView_source->fitInView(_ui->imageView_source->sceneRect(), Qt::KeepAspectRatio);
_ui->imageView_loopClosure->fitInView(_ui->imageView_source->sceneRect(), Qt::KeepAspectRatio);
_ui->imageView_source->fitInView(_ui->imageView_odometry->sceneRect(), Qt::KeepAspectRatio);
_ui->imageView_odometry->fitInView(_ui->imageView_odometry->sceneRect(), Qt::KeepAspectRatio);
_ui->imageView_source->resetZoom();
_ui->imageView_loopClosure->resetZoom();
_ui->imageView_odometry->resetZoom();

4
guilib/src/PreferencesDialog.cpp
View File

@@ -482,17 +482,15 @@ PreferencesDialog::PreferencesDialog(QWidget * parent) :
//Odometry
_ui->odom_strategy->setObjectName(Parameters::kOdomStrategy().c_str());
_ui->odom_type->setObjectName(Parameters::kOdomFeatureType().c_str());
_ui->odom_linearUpdate->setObjectName(Parameters::kOdomLinearUpdate().c_str());
_ui->odom_angularUpdate->setObjectName(Parameters::kOdomAngularUpdate().c_str());
_ui->odom_countdown->setObjectName(Parameters::kOdomResetCountdown().c_str());
_ui->odom_maxFeatures->setObjectName(Parameters::kOdomMaxFeatures().c_str());
_ui->odom_ratio->setObjectName(Parameters::kOdomFeaturesRatio().c_str());
_ui->odom_inlierDistance->setObjectName(Parameters::kOdomInlierDistance().c_str());
_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_refine_iterations->setObjectName(Parameters::kOdomRefineIterations().c_str());
_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());
//Odometry BOW

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

@@ -792,6 +792,9 @@
<property name="text">
<string/>
</property>
<property name="checked">
<bool>true</bool>
</property>
</widget>
</item>
<item row="2" column="0">

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

@@ -63,9 +63,9 @@
<property name="geometry">
<rect>
<x>0</x>
<y>-305</y>
<y>0</y>
<width>744</width>
<height>1252</height>
<height>1056</height>
</rect>
</property>
<layout class="QVBoxLayout" name="verticalLayout_16">
@@ -86,7 +86,7 @@
<enum>QFrame::Raised</enum>
</property>
<property name="currentIndex">
<number>19</number>
<number>23</number>
</property>
<widget class="QWidget" name="page_22">
<layout class="QVBoxLayout" name="verticalLayout_29">
@@ -6261,7 +6261,7 @@ Lower the ratio -&gt; higher the precision. 0 means disabled, matching the neare
</item>
<item>
<layout class="QGridLayout" name="gridLayout_27" columnstretch="0,0">
<item row="4" column="1">
<item row="2" column="1">
<widget class="QLabel" name="label_47">
<property name="text">
<string>Automatically reset odometry after X consecutive images on which odometry cannot be computed (value=0 disables auto-reset).</string>
@@ -6288,55 +6288,13 @@ Lower the ratio -&gt; higher the precision. 0 means disabled, matching the neare
</item>
</widget>
</item>
<item row="3" column="1">
<widget class="QLabel" name="label_46">
<property name="text">
<string>Angular update: minimum angular distance to update the odometry.</string>
</property>
<property name="wordWrap">
<bool>true</bool>
</property>
</widget>
</item>
<item row="3" column="0">
<widget class="QDoubleSpinBox" name="odom_angularUpdate">
<property name="suffix">
<string> m</string>
</property>
<property name="decimals">
<number>3</number>
</property>
<property name="singleStep">
<double>0.100000000000000</double>
</property>
<property name="value">
<double>0.015000000000000</double>
</property>
</widget>
</item>
<item row="4" column="0">
<item row="2" column="0">
<widget class="QSpinBox" name="odom_countdown">
<property name="maximum">
<number>999999</number>
</property>
</widget>
</item>
<item row="2" column="0">
<widget class="QDoubleSpinBox" name="odom_linearUpdate">
<property name="suffix">
<string> m</string>
</property>
<property name="decimals">
<number>3</number>
</property>
<property name="singleStep">
<double>0.100000000000000</double>
</property>
<property name="value">
<double>0.015000000000000</double>
</property>
</widget>
</item>
<item row="1" column="0">
<widget class="QComboBox" name="odom_type">
<property name="sizeAdjustPolicy">
@@ -6384,16 +6342,6 @@ Lower the ratio -&gt; higher the precision. 0 means disabled, matching the neare
</item>
</widget>
</item>
<item row="2" column="1">
<widget class="QLabel" name="label_45">
<property name="text">
<string>Linear update: minimum linear distance to update the odometry.</string>
</property>
<property name="wordWrap">
<bool>true</bool>
</property>
</widget>
</item>
<item row="1" column="1">
<widget class="QLabel" name="label_44">
<property name="text">
@@ -6416,14 +6364,14 @@ Lower the ratio -&gt; higher the precision. 0 means disabled, matching the neare
</property>
</widget>
</item>
<item row="5" column="0">
<item row="3" column="0">
<widget class="QCheckBox" name="odom_force2D">
<property name="text">
<string/>
</property>
</widget>
</item>
<item row="5" column="1">
<item row="3" column="1">
<widget class="QLabel" name="label_196">
<property name="text">
<string>Force 2D transform (3DoF: x,y and yaw).</string>
@@ -6433,6 +6381,23 @@ Lower the ratio -&gt; higher the precision. 0 means disabled, matching the neare
</property>
</widget>
</item>
<item row="4" column="1">
<widget class="QLabel" name="label_221">
<property name="text">
<string>Fill info with data (inliers/outliers features to be shown in Odometry view).</string>
</property>
<property name="wordWrap">
<bool>true</bool>
</property>
</widget>
</item>
<item row="4" column="0">
<widget class="QCheckBox" name="odom_fillInfoData">
<property name="text">
<string/>
</property>
</widget>
</item>
</layout>
</item>
<item>
@@ -6545,38 +6510,6 @@ Lower the ratio -&gt; higher the precision. 0 means disabled, matching the neare
</property>
</widget>
</item>
<item row="4" 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>
<item row="4" column="1">
<widget class="QLabel" name="label_90">
<property name="text">
<string>Minimum 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>
</layout>
</widget>
</item>