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

Odometry and stereo: added parameter OdomStereo/MaxSlope to filter bad stereo pair matches from cvOpticalFlow

Browse Source 
git-svn-id: http://rtabmap.googlecode.com/svn/trunk/rtabmap@1967 f169173b-cf89-36c8-b27e-44dbe73f0c83
This commit is contained in:
matlabbe
2014-11-04 18:58:40 +00:00
parent 54084fc5e1
commit cbb0185adf
12 changed files with 948 additions and 533 deletions
Download Patch File Download Diff File Expand all files Collapse all files

172
guilib/src/DatabaseViewer.cpp
View File

@@ -33,6 +33,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
#include <QtGui/QInputDialog>
#include <QtGui/QGraphicsLineItem>
#include <QtGui/QCloseEvent>
#include <QtGui/QGraphicsOpacityEffect>
#include <QtCore/QBuffer>
#include <QtCore/QTextStream>
#include <rtabmap/utilite/ULogger.h>
@@ -47,6 +48,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
#include "rtabmap/gui/UCv2Qt.h"
#include "rtabmap/core/util3d.h"
#include "rtabmap/core/Signature.h"
#include "rtabmap/core/Features2d.h"
#include "rtabmap/gui/DataRecorder.h"
#include "rtabmap/core/SensorData.h"
#include "ExportDialog.h"
@@ -76,12 +78,14 @@ DatabaseViewer::DatabaseViewer(QWidget * parent) :
ui_->dockWidget_graphView->setVisible(false);
ui_->dockWidget_icp->setVisible(false);
ui_->dockWidget_visual->setVisible(false);
ui_->dockWidget_stereoView->setVisible(false);
ui_->dockWidget_icp->setFloating(true);
ui_->dockWidget_visual->setFloating(true);
ui_->menuView->addAction(ui_->dockWidget_constraints->toggleViewAction());
ui_->menuView->addAction(ui_->dockWidget_graphView->toggleViewAction());
ui_->menuView->addAction(ui_->dockWidget_icp->toggleViewAction());
ui_->menuView->addAction(ui_->dockWidget_visual->toggleViewAction());
ui_->menuView->addAction(ui_->dockWidget_stereoView->toggleViewAction());
connect(ui_->dockWidget_graphView->toggleViewAction(), SIGNAL(triggered()), this, SLOT(updateGraphView()));
connect(ui_->actionQuit, SIGNAL(triggered()), this, SLOT(close()));
@@ -994,6 +998,12 @@ void DatabaseViewer::update(int value,
}
mapId = memory_->getMapId(id);
//stereo
if(!data.getDepthRaw().empty() && data.getDepthRaw().type() == CV_8UC1)
{
this->updateStereo(&data);
}
}
if(!imgDepth.isNull())
@@ -1121,6 +1131,168 @@ void DatabaseViewer::update(int value,
view->fitInView(view->sceneRect(), Qt::KeepAspectRatio);
}
void DatabaseViewer::updateStereo(const Signature * data)
{
if(ui_->dockWidget_stereoView->isVisible() && !data->getImageRaw().empty() && !data->getDepthRaw().empty() && data->getDepthRaw().type() == CV_8UC1)
{
cv::Mat leftMono;
if(data->getImageRaw().channels() == 3)
{
cv::cvtColor(data->getImageRaw(), leftMono, CV_BGR2GRAY);
}
else
{
leftMono = data->getImageRaw();
}
UTimer timer;
// generate kpts
std::vector<cv::KeyPoint> kpts;
cv::Rect roi = Feature2D::computeRoi(leftMono, "0.03 0.03 0.04 0.04");
ParametersMap parameters;
parameters.insert(ParametersPair(Parameters::kGFTTMaxCorners(), "1000"));
parameters.insert(ParametersPair(Parameters::kGFTTMinDistance(), "10"));
Feature2D::Type type = Feature2D::kFeatureGfttBrief;
Feature2D * kptDetector = Feature2D::create(type, parameters);
kpts = kptDetector->generateKeypoints(leftMono, 0, roi);
delete kptDetector;
float timeKpt = timer.ticks();
std::vector<cv::Point2f> leftCorners;
cv::KeyPoint::convert(kpts, leftCorners);
// Find features in the new left image
std::vector<unsigned char> status;
std::vector<float> err;
std::vector<cv::Point2f> rightCorners;
cv::calcOpticalFlowPyrLK(
leftMono,
data->getDepthRaw(),
leftCorners,
rightCorners,
status,
err,
cv::Size(21, 21), 4,
cv::TermCriteria(cv::TermCriteria::COUNT+cv::TermCriteria::EPS, 20, 0.02));
float timeFlow = timer.ticks();
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>);
cloud->resize(kpts.size());
float bad_point = std::numeric_limits<float>::quiet_NaN ();
UASSERT(status.size() == kpts.size());
int oi = 0;
for(unsigned int i=0; i<status.size(); ++i)
{
pcl::PointXYZ pt(bad_point, bad_point, bad_point);
if(status[i])
{
float disparity = leftCorners[i].x - rightCorners[i].x;
if(disparity > 0.0f)
{
if(fabs((leftCorners[i].y-rightCorners[i].y) / (leftCorners[i].x-rightCorners[i].x)) < 0.1)
{
pcl::PointXYZ tmpPt = util3d::projectDisparityTo3D(
leftCorners[i],
disparity,
data->getDepthCx(), data->getDepthCy(), data->getDepthFx(), data->getDepthFy());
if(pcl::isFinite(tmpPt))
{
pt = pcl::transformPoint(tmpPt, util3d::transformToEigen3f(data->getLocalTransform()));
if(fabs(pt.x) > 2 || fabs(pt.y) > 2 || fabs(pt.z) > 2)
{
status[i] = 100; //blue
}
cloud->at(oi++) = pt;
}
}
else
{
status[i] = 101; //yellow
}
}
else
{
status[i] = 102; //magenta
}
}
}
cloud->resize(oi);
UINFO("correspondences = %d/%d (%f) (time kpt=%fs flow=%fs)",
(int)cloud->size(), (int)leftCorners.size(), float(cloud->size())/float(leftCorners.size()), timeKpt, timeFlow);
ui_->stereoViewer->updateCameraPosition(Transform::getIdentity());
ui_->stereoViewer->addOrUpdateCloud("stereo", cloud);
ui_->stereoViewer->render();
std::vector<cv::KeyPoint> rightKpts;
cv::KeyPoint::convert(rightCorners, rightKpts);
std::vector<cv::DMatch> good_matches(kpts.size());
for(unsigned int i=0; i<good_matches.size(); ++i)
{
good_matches[i].trainIdx = i;
good_matches[i].queryIdx = i;
}
//
//cv::Mat imageMatches;
//cv::drawMatches( leftMono, kpts, data->getDepthRaw(), rightKpts,
// good_matches, imageMatches, cv::Scalar::all(-1), cv::Scalar::all(-1),
// std::vector<char>(), cv::DrawMatchesFlags::NOT_DRAW_SINGLE_POINTS );
//ui_->graphicsView_stereo->setImage(uCvMat2QImage(imageMatches));
ui_->graphicsView_stereo->scene()->clear();
ui_->graphicsView_stereo->setSceneRect(0,0,(float)leftMono.cols, (float)leftMono.rows);
ui_->graphicsView_stereo->setLinesShown(true);
ui_->graphicsView_stereo->setFeaturesShown(false);
QGraphicsPixmapItem * item1 = ui_->graphicsView_stereo->scene()->addPixmap(QPixmap::fromImage(uCvMat2QImage(data->getImageRaw())));
QGraphicsPixmapItem * item2 = ui_->graphicsView_stereo->scene()->addPixmap(QPixmap::fromImage(uCvMat2QImage(data->getDepthRaw())));
QGraphicsOpacityEffect * effect1 = new QGraphicsOpacityEffect();
QGraphicsOpacityEffect * effect2 = new QGraphicsOpacityEffect();
effect1->setOpacity(0.5);
effect2->setOpacity(0.5);
item1->setGraphicsEffect(effect1);
item2->setGraphicsEffect(effect2);
// Draw lines between corresponding features...
for(unsigned int i=0; i<kpts.size(); ++i)
{
QColor c = Qt::green;
if(status[i] == 0)
{
c = Qt::red;
}
else if(status[i] == 100)
{
c = Qt::blue;
}
else if(status[i] == 101)
{
c = Qt::yellow;
}
else if(status[i] == 102)
{
c = Qt::magenta;
}
QGraphicsLineItem * item = ui_->graphicsView_stereo->scene()->addLine(
kpts[i].pt.x,
kpts[i].pt.y,
rightKpts[i].pt.x,
rightKpts[i].pt.y,
QPen(c));
item->setZValue(1);
}
}
}
void DatabaseViewer::updateWordsMatching()
{
int from = ids_.at(ui_->horizontalSlider_A->value());

28
guilib/src/PreferencesDialog.cpp
View File

@@ -346,6 +346,10 @@ PreferencesDialog::PreferencesDialog(QWidget * parent) :
connect(_ui->toolButton_dictionaryPath, SIGNAL(clicked()), this, SLOT(changeDictionaryPath()));
_ui->checkBox_kp_newWordsComparedTogether->setObjectName(Parameters::kKpNewWordsComparedTogether().c_str());
_ui->subpix_winSize->setObjectName(Parameters::kKpSubPixWinSize().c_str());
_ui->subpix_iterations->setObjectName(Parameters::kKpSubPixIterations().c_str());
_ui->subpix_eps->setObjectName(Parameters::kKpSubPixEps().c_str());
//SURF detector
_ui->surf_doubleSpinBox_hessianThr->setObjectName(Parameters::kSURFHessianThreshold().c_str());
_ui->surf_spinBox_octaves->setObjectName(Parameters::kSURFOctaves().c_str());
@@ -478,18 +482,18 @@ PreferencesDialog::PreferencesDialog(QWidget * parent) :
_ui->odom_flow_maxLevel->setObjectName(Parameters::kOdomFlowMaxLevel().c_str());
_ui->odom_flow_iterations->setObjectName(Parameters::kOdomFlowIterations().c_str());
_ui->odom_flow_eps->setObjectName(Parameters::kOdomFlowEps().c_str());
_ui->odom_flow_subpix_winSize->setObjectName(Parameters::kOdomFlowSubPixWinSize().c_str());
_ui->odom_flow_subpix_iterations->setObjectName(Parameters::kOdomFlowSubPixIterations().c_str());
_ui->odom_flow_subpix_eps->setObjectName(Parameters::kOdomFlowSubPixEps().c_str());
_ui->odom_stereo_maxSlope->setObjectName(Parameters::kOdomStereoMaxSlope().c_str());
_ui->odom_subpix_winSize->setObjectName(Parameters::kOdomSubPixWinSize().c_str());
_ui->odom_subpix_iterations->setObjectName(Parameters::kOdomSubPixIterations().c_str());
_ui->odom_subpix_eps->setObjectName(Parameters::kOdomSubPixEps().c_str());
//Stereo
_ui->stereo_winSize->setObjectName(Parameters::kStereoWinSize().c_str());
_ui->stereo_maxLevel->setObjectName(Parameters::kStereoMaxLevel().c_str());
_ui->stereo_iterations->setObjectName(Parameters::kStereoIterations().c_str());
_ui->stereo_eps->setObjectName(Parameters::kStereoEps().c_str());
_ui->stereo_subpix_winSize->setObjectName(Parameters::kStereoSubPixWinSize().c_str());
_ui->stereo_subpix_iterations->setObjectName(Parameters::kStereoSubPixIterations().c_str());
_ui->stereo_subpix_eps->setObjectName(Parameters::kStereoSubPixEps().c_str());
_ui->stereo_flow_winSize->setObjectName(Parameters::kStereoWinSize().c_str());
_ui->stereo_flow_maxLevel->setObjectName(Parameters::kStereoMaxLevel().c_str());
_ui->stereo_flow_iterations->setObjectName(Parameters::kStereoIterations().c_str());
_ui->stereo_flow_eps->setObjectName(Parameters::kStereoEps().c_str());
_ui->stereo_maxSlope->setObjectName(Parameters::kStereoMaxSlope().c_str());
setupSignals();
// custom signals
@@ -813,8 +817,8 @@ void PreferencesDialog::resetSettings(QGroupBox * groupBox)
_ui->checkBox_mls->setChecked(false);
_ui->doubleSpinBox_mlsRadius->setValue(0.04);
_ui->groupBox_poseFiltering->setChecked(false);
_ui->doubleSpinBox_cloudFilterRadius->setValue(0.1);
_ui->groupBox_poseFiltering->setChecked(true);
_ui->doubleSpinBox_cloudFilterRadius->setValue(0.3);
_ui->doubleSpinBox_cloudFilterAngle->setValue(30);
_ui->checkBox_map_shown->setChecked(false);

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

@@ -7,7 +7,7 @@
<x>0</x>
<y>0</y>
<width>1187</width>
<height>711</height>
<height>851</height>
</rect>
</property>
<property name="windowTitle">
@@ -1011,6 +1011,24 @@
</layout>
</widget>
</widget>
<widget class="QDockWidget" name="dockWidget_stereoView">
<property name="windowTitle">
<string>Stereo view</string>
</property>
<attribute name="dockWidgetArea">
<number>4</number>
</attribute>
<widget class="QWidget" name="dockWidgetContents_5">
<layout class="QHBoxLayout" name="horizontalLayout_6">
<item>
<widget class="rtabmap::ImageView" name="graphicsView_stereo"/>
</item>
<item>
<widget class="rtabmap::CloudViewer" name="stereoViewer" native="true"/>
</item>
</layout>
</widget>
</widget>
<action name="actionOpen_database">
<property name="text">
<string>Open database</string>
@@ -1103,17 +1121,17 @@
</action>
</widget>
<customwidgets>
<customwidget>
<class>rtabmap::ImageView</class>
<extends>QGraphicsView</extends>
<header>rtabmap/gui/ImageView.h</header>
</customwidget>
<customwidget>
<class>rtabmap::CloudViewer</class>
<extends>QWidget</extends>
<header>rtabmap/gui/CloudViewer.h</header>
<container>1</container>
</customwidget>
<customwidget>
<class>rtabmap::ImageView</class>
<extends>QGraphicsView</extends>
<header>rtabmap/gui/ImageView.h</header>
</customwidget>
<customwidget>
<class>rtabmap::GraphViewer</class>
<extends>QGraphicsView</extends>

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

File diff suppressed because it is too large Load Diff