Skip to content

Commit

Permalink
added Odometry view to MainWindow, increased version to 0.7.3
Browse files Browse the repository at this point in the history
  • Loading branch information
@matlabbe
matlabbe committed Dec 2, 2014
1 parent 72bac39 commit 9e1d38f
Show file tree
Hide file tree
Showing 7 changed files with 135 additions and 61 deletions.
2 changes: 1 addition & 1 deletion CMakeLists.txt
View file
Original file line number Diff line number Diff line change
Expand Up @@ -16,7 +16,7 @@ SET(CMAKE_MODULE_PATH "${PROJECT_SOURCE_DIR}/cmake_modules")
#######################
SET(RTABMAP_MAJOR_VERSION 0)
SET(RTABMAP_MINOR_VERSION 7)
SET(RTABMAP_PATCH_VERSION 2)
SET(RTABMAP_PATCH_VERSION 3)
SET(RTABMAP_VERSION
${RTABMAP_MAJOR_VERSION}.${RTABMAP_MINOR_VERSION}.${RTABMAP_PATCH_VERSION})

Expand Down
14 changes: 7 additions & 7 deletions corelib/src/Features2d.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -481,7 +481,7 @@ void SURF::parseParameters(const ParametersMap & parameters)
_surf = new cv::SURF(hessianThreshold_, nOctaves_, nOctaveLayers_, extended_, upright_);
}
#else
UERROR("RTAB-Map is not built with OpenCV nonfree module so SURF cannot be used!");
UWARN("RTAB-Map is not built with OpenCV nonfree module so SURF cannot be used!");
#endif
}

Expand All @@ -502,7 +502,7 @@ std::vector<cv::KeyPoint> SURF::generateKeypointsImpl(const cv::Mat & image, con
_surf->detect(imgRoi, keypoints);
}
#else
UERROR("RTAB-Map is not built with OpenCV nonfree module so SURF cannot be used!");
UWARN("RTAB-Map is not built with OpenCV nonfree module so SURF cannot be used!");
#endif
return keypoints;
}
Expand Down Expand Up @@ -533,7 +533,7 @@ cv::Mat SURF::generateDescriptorsImpl(const cv::Mat & image, std::vector<cv::Key
_surf->compute(image, keypoints, descriptors);
}
#else
UERROR("RTAB-Map is not built with OpenCV nonfree module so SURF cannot be used!");
UWARN("RTAB-Map is not built with OpenCV nonfree module so SURF cannot be used!");
#endif

return descriptors;
Expand Down Expand Up @@ -561,7 +561,7 @@ SIFT::~SIFT()
delete _sift;
}
#else
UERROR("RTAB-Map is not built with OpenCV nonfree module so SIFT cannot be used!");
UWARN("RTAB-Map is not built with OpenCV nonfree module so SIFT cannot be used!");
#endif
}

Expand All @@ -582,7 +582,7 @@ void SIFT::parseParameters(const ParametersMap & parameters)

_sift = new cv::SIFT(nfeatures_, nOctaveLayers_, contrastThreshold_, edgeThreshold_, sigma_);
#else
UERROR("RTAB-Map is not built with OpenCV nonfree module so SIFT cannot be used!");
UWARN("RTAB-Map is not built with OpenCV nonfree module so SIFT cannot be used!");
#endif
}

Expand All @@ -594,7 +594,7 @@ std::vector<cv::KeyPoint> SIFT::generateKeypointsImpl(const cv::Mat & image, con
cv::Mat imgRoi(image, roi);
_sift->detect(imgRoi, keypoints); // Opencv keypoints
#else
UERROR("RTAB-Map is not built with OpenCV nonfree module so SIFT cannot be used!");
UWARN("RTAB-Map is not built with OpenCV nonfree module so SIFT cannot be used!");
#endif
return keypoints;
}
Expand All @@ -606,7 +606,7 @@ cv::Mat SIFT::generateDescriptorsImpl(const cv::Mat & image, std::vector<cv::Key
#if RTABMAP_NONFREE == 1
_sift->compute(image, keypoints, descriptors);
#else
UERROR("RTAB-Map is not built with OpenCV nonfree module so SIFT cannot be used!");
UWARN("RTAB-Map is not built with OpenCV nonfree module so SIFT cannot be used!");
#endif
return descriptors;
}
Expand Down
4 changes: 3 additions & 1 deletion guilib/include/rtabmap/gui/CloudViewer.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -147,7 +147,8 @@ class RTABMAPGUI_EXP CloudViewer : public QVTKWidget
bool getPose(const std::string & id, Transform & pose); //including meshes
bool getCloudVisibility(const std::string & id);

const QMap<std::string, Transform> & getAddedClouds() {return _addedClouds;} //including meshes
const QMap<std::string, Transform> & getAddedClouds() const {return _addedClouds;} //including meshes
const QColor & getBackgroundColor() const;

void setCameraTargetLocked(bool enabled = true);
void setCameraTargetFollow(bool enabled = true);
Expand Down Expand Up @@ -197,6 +198,7 @@ public slots:
std::list<std::string> _gridLines;
QSet<Qt::Key> _keysPressed;
QString _workingDirectory;
QColor _backgroundColor;
};

} /* namespace rtabmap */
Expand Down
9 changes: 8 additions & 1 deletion guilib/src/CloudViewer.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -74,7 +74,8 @@ CloudViewer::CloudViewer(QWidget *parent) :
_menu(0),
_trajectory(new pcl::PointCloud<pcl::PointXYZ>),
_maxTrajectorySize(100),
_workingDirectory(".")
_workingDirectory("."),
_backgroundColor(Qt::black)
{
this->setMinimumSize(200, 200);

Expand Down Expand Up @@ -657,8 +658,14 @@ void CloudViewer::render()
this->GetRenderWindow()->Render();
}

const QColor & CloudViewer::getBackgroundColor() const
{
return _backgroundColor;
}

void CloudViewer::setBackgroundColor(const QColor & color)
{
_backgroundColor = color;
_visualizer->setBackgroundColor(color.redF(), color.greenF(), color.blueF());
}

Expand Down
144 changes: 94 additions & 50 deletions guilib/src/MainWindow.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -161,6 +161,7 @@ MainWindow::MainWindow(PreferencesDialog * prefDialog, QWidget * parent) :
_ui->dockWidget_loopClosureViewer->setVisible(false);
_ui->dockWidget_mapVisibility->setVisible(false);
_ui->dockWidget_graphViewer->setVisible(false);
_ui->dockWidget_odometry->setVisible(false);
//_ui->dockWidget_cloudViewer->setVisible(false);
//_ui->dockWidget_imageView->setVisible(false);
}
Expand Down Expand Up @@ -194,6 +195,7 @@ MainWindow::MainWindow(PreferencesDialog * prefDialog, QWidget * parent) :
//Graphics scenes
_ui->imageView_source->setBackgroundBrush(QBrush(Qt::black));
_ui->imageView_loopClosure->setBackgroundBrush(QBrush(Qt::black));
_ui->imageView_odometry->setBackgroundBrush(QBrush(Qt::black));

_posteriorCurve = new PdfPlotCurve("Posterior", &_cachedSignatures, this);
_ui->posteriorPlot->addCurve(_posteriorCurve, false);
Expand Down Expand Up @@ -240,6 +242,7 @@ MainWindow::MainWindow(PreferencesDialog * prefDialog, QWidget * parent) :
_ui->menuShow_view->addAction(_ui->dockWidget_loopClosureViewer->toggleViewAction());
_ui->menuShow_view->addAction(_ui->dockWidget_mapVisibility->toggleViewAction());
_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"));
QAction * a = _ui->menuShow_view->addAction("Progress dialog");
Expand Down Expand Up @@ -455,6 +458,7 @@ void MainWindow::closeEvent(QCloseEvent* event)
_ui->dockWidget_loopClosureViewer->close();
_ui->dockWidget_mapVisibility->close();
_ui->dockWidget_graphViewer->close();
_ui->dockWidget_odometry->close();

if(_camera)
{
Expand Down Expand Up @@ -561,7 +565,7 @@ void MainWindow::handleEvent(UEvent* anEvent)
else if(anEvent->getClassName().compare("OdometryEvent") == 0)
{
OdometryEvent * odomEvent = (OdometryEvent*)anEvent;
if(_ui->dockWidget_cloudViewer->isVisible() &&
if((_ui->dockWidget_cloudViewer->isVisible() || _ui->dockWidget_odometry->isVisible()) &&
_lastOdometryProcessed &&
!_processingStatistics)
{
Expand Down Expand Up @@ -592,24 +596,30 @@ void MainWindow::handleEvent(UEvent* anEvent)
void MainWindow::processOdometry(const rtabmap::SensorData & data, int quality, float time, int features, int localMapSize)
{
Transform pose = data.pose();
bool lost = false;
_ui->imageView_odometry->resetTransform();
if(pose.isNull())
{
UDEBUG("odom lost"); // use last pose
_ui->widget_cloudViewer->setBackgroundColor(Qt::darkRed);
_ui->imageView_odometry->setBackgroundBrush(QBrush(Qt::darkRed));

pose = _lastOdomPose;
lost = true;
}
else if(quality>=0 &&
_preferencesDialog->getOdomQualityWarnThr() &&
quality < _preferencesDialog->getOdomQualityWarnThr())
{
UDEBUG("odom warn, quality=%d thr=%d", quality, _preferencesDialog->getOdomQualityWarnThr());
_ui->widget_cloudViewer->setBackgroundColor(Qt::darkYellow);
_ui->imageView_odometry->setBackgroundBrush(QBrush(Qt::darkYellow));
}
else
{
UDEBUG("odom ok");
_ui->widget_cloudViewer->setBackgroundColor(Qt::black);
_ui->imageView_odometry->setBackgroundBrush(QBrush(Qt::black));
}
if(quality >= 0)
{
Expand All @@ -627,65 +637,94 @@ void MainWindow::processOdometry(const rtabmap::SensorData & data, int quality,
{
_ui->statsToolBox->updateStat("Odometry/LocalMapSize/", (float)data.id(), (float)localMapSize);
}
if(!pose.isNull())
{
_lastOdomPose = pose;
_odometryReceived = true;

// 3d cloud
if(data.depth().cols == data.image().cols &&
data.depth().rows == data.image().rows &&
!data.depth().empty() &&
data.fx() > 0.0f &&
data.fy() > 0.0f &&
_preferencesDialog->isCloudsShown(1))
{
pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud;
cloud = createCloud(0,
data.image(),
data.depth(),
data.fx(),
data.fy(),
data.cx(),
data.cy(),
data.localTransform(),
pose,
_preferencesDialog->getCloudVoxelSize(1),
_preferencesDialog->getCloudDecimation(1),
_preferencesDialog->getCloudMaxDepth(1));

if(!_ui->widget_cloudViewer->addOrUpdateCloud("cloudOdom", cloud, _odometryCorrection))

if(_ui->dockWidget_cloudViewer->isVisible())
{
if(!pose.isNull())
{
_lastOdomPose = pose;
_odometryReceived = true;

// 3d cloud
if(data.depth().cols == data.image().cols &&
data.depth().rows == data.image().rows &&
!data.depth().empty() &&
data.fx() > 0.0f &&
data.fy() > 0.0f &&
_preferencesDialog->isCloudsShown(1))
{
UERROR("Adding cloudOdom to viewer failed!");
pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud;
cloud = createCloud(0,
data.image(),
data.depth(),
data.fx(),
data.fy(),
data.cx(),
data.cy(),
data.localTransform(),
pose,
_preferencesDialog->getCloudVoxelSize(1),
_preferencesDialog->getCloudDecimation(1),
_preferencesDialog->getCloudMaxDepth(1));

if(!_ui->widget_cloudViewer->addOrUpdateCloud("cloudOdom", cloud, _odometryCorrection))
{
UERROR("Adding cloudOdom to viewer failed!");
}
_ui->widget_cloudViewer->setCloudVisibility("cloudOdom", true);
_ui->widget_cloudViewer->setCloudOpacity("cloudOdom", _preferencesDialog->getCloudOpacity(1));
_ui->widget_cloudViewer->setCloudPointSize("cloudOdom", _preferencesDialog->getCloudPointSize(1));
}
_ui->widget_cloudViewer->setCloudVisibility("cloudOdom", true);
_ui->widget_cloudViewer->setCloudOpacity("cloudOdom", _preferencesDialog->getCloudOpacity(1));
_ui->widget_cloudViewer->setCloudPointSize("cloudOdom", _preferencesDialog->getCloudPointSize(1));
}

// 2d cloud
if(!data.depth2d().empty() &&
_preferencesDialog->isScansShown(1))
{
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud;
cloud = util3d::depth2DToPointCloud(data.depth2d());
cloud = util3d::transformPointCloud<pcl::PointXYZ>(cloud, pose);
if(!_ui->widget_cloudViewer->addOrUpdateCloud("scanOdom", cloud, _odometryCorrection))
// 2d cloud
if(!data.depth2d().empty() &&
_preferencesDialog->isScansShown(1))
{
UERROR("Adding scanOdom to viewer failed!");
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud;
cloud = util3d::depth2DToPointCloud(data.depth2d());
cloud = util3d::transformPointCloud<pcl::PointXYZ>(cloud, pose);
if(!_ui->widget_cloudViewer->addOrUpdateCloud("scanOdom", cloud, _odometryCorrection))
{
UERROR("Adding scanOdom to viewer failed!");
}
_ui->widget_cloudViewer->setCloudVisibility("scanOdom", true);
_ui->widget_cloudViewer->setCloudOpacity("scanOdom", _preferencesDialog->getScanOpacity(1));
_ui->widget_cloudViewer->setCloudPointSize("scanOdom", _preferencesDialog->getScanPointSize(1));
}
_ui->widget_cloudViewer->setCloudVisibility("scanOdom", true);
_ui->widget_cloudViewer->setCloudOpacity("scanOdom", _preferencesDialog->getScanOpacity(1));
_ui->widget_cloudViewer->setCloudPointSize("scanOdom", _preferencesDialog->getScanPointSize(1));

if(!data.pose().isNull())
{
// update camera position
_ui->widget_cloudViewer->updateCameraPosition(_odometryCorrection*data.pose());
}
}
_ui->widget_cloudViewer->render();
}

if(_ui->dockWidget_odometry->isVisible() &&
!data.image().empty())
{
if(lost)
{
_ui->imageView_odometry->setImageDepth(uCvMat2QImage(data.image()));
_ui->imageView_odometry->setImageShown(true);
_ui->imageView_odometry->setImageDepthShown(true);
}
else
{
_ui->imageView_odometry->setImage(uCvMat2QImage(data.image()));
_ui->imageView_odometry->setImageShown(true);
_ui->imageView_odometry->setImageDepthShown(false);
}

if(!data.pose().isNull())
_ui->imageView_odometry->resetZoom();
_ui->imageView_odometry->setSceneRect(_ui->imageView_odometry->scene()->itemsBoundingRect());
_ui->imageView_odometry->fitInView(_ui->imageView_odometry->sceneRect(), Qt::KeepAspectRatio);
if(_preferencesDialog->isImageFlipped())
{
// update camera position
_ui->widget_cloudViewer->updateCameraPosition(_odometryCorrection*data.pose());
_ui->imageView_odometry->scale(-1.0, 1.0);
}
}
_ui->widget_cloudViewer->render();

_lastOdometryProcessed = true;

Expand Down Expand Up @@ -1885,8 +1924,10 @@ 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_source->resetZoom();
_ui->imageView_loopClosure->resetZoom();
_ui->imageView_odometry->resetZoom();
}

void MainWindow::updateSelectSourceImageMenu(int type)
Expand Down Expand Up @@ -3266,10 +3307,13 @@ void MainWindow::clearTheCache()
_ui->graphicsView_graphView->clearAll();
_ui->imageView_source->clear();
_ui->imageView_loopClosure->clear();
_ui->imageView_odometry->clear();
_ui->imageView_source->resetTransform();
_ui->imageView_loopClosure->resetTransform();
_ui->imageView_odometry->resetTransform();
_ui->imageView_source->setBackgroundBrush(QBrush(Qt::black));
_ui->imageView_loopClosure->setBackgroundBrush(QBrush(Qt::black));
_ui->imageView_odometry->setBackgroundBrush(QBrush(Qt::black));
}

void MainWindow::updateElapsedTime()
Expand Down
21 changes: 21 additions & 0 deletions guilib/src/ui/mainWindow.ui
View file
Original file line number Diff line number Diff line change
Expand Up @@ -716,6 +716,27 @@
</layout>
</widget>
</widget>
<widget class="QDockWidget" name="dockWidget_odometry">
<property name="windowTitle">
<string>Odometry</string>
</property>
<attribute name="dockWidgetArea">
<number>1</number>
</attribute>
<widget class="QWidget" name="dockWidgetContents_10">
<layout class="QVBoxLayout" name="verticalLayout_11">
<property name="spacing">
<number>0</number>
</property>
<property name="margin">
<number>0</number>
</property>
<item>
<widget class="rtabmap::ImageView" name="imageView_odometry"/>
</item>
</layout>
</widget>
</widget>
<action name="actionExit">
<property name="text">
<string>Exit</string>
Expand Down
2 changes: 1 addition & 1 deletion package.xml
View file
Original file line number Diff line number Diff line change
@@ -1,7 +1,7 @@
<?xml version="1.0"?>
<package>
<name>rtabmap</name>
<version>0.7.2</version>
<version>0.7.3</version>
<description>RTAB-Map's standalone library. RTAB-Map is an RGB-D SLAM approach with real-time constraints.</description>
<maintainer email="matlabbe@gmail.com">Mathieu Labbe</maintainer>
<author>Mathieu Labbe</author>
Expand Down

0 comments on commit 9e1d38f

Please sign in to comment.