i/rtabmap
1
0
Fork 0
mirror of https://github.com/introlab/rtabmap.git synced 2026-09-02 17:40:23 +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

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();