3D->3D estimation refining: using 3x sqrt(variance) instead of sqrt(9x variance). GUI: added features cloud rendering option. CloudViewer: fixed slow updateCameraTargetPosition()

guilib/include/rtabmap/gui/MainWindow.h

@@ -234,6 +234,7 @@ private:
 			bool verboseProgress = false);
 	void createAndAddCloudToMap(int nodeId,	const Transform & pose, int mapId);
 	void createAndAddScanToMap(int nodeId, const Transform & pose, int mapId);
+	void createAndAddFeaturesToMap(int nodeId, const Transform & pose, int mapId);
 	Transform alignPosesToGroundTruth(std::map<int, Transform> & poses, const std::map<int, Transform> & groundTruth);
 	void drawKeypoints(const std::multimap<int, cv::KeyPoint> & refWords, const std::multimap<int, cv::KeyPoint> & loopWords);
 	void setupMainLayout(bool vertical);
@@ -289,6 +290,9 @@ private:
 	std::map<int, pcl::PointCloud<pcl::PointXYZ>::Ptr > _createdScans;
 	std::map<int, std::pair<cv::Mat, cv::Mat> > _projectionLocalMaps; // <ground, obstacles>
 	std::map<int, std::pair<cv::Mat, cv::Mat> > _gridLocalMaps; // <ground, obstacles>
+
+	std::map<int, pcl::PointCloud<pcl::PointXYZRGB>::Ptr> _createdFeatures;
+
 	Transform _odometryCorrection;
 	Transform _lastOdomPose;
 	bool _processingOdometry;

guilib/include/rtabmap/gui/PreferencesDialog.h

@@ -157,6 +157,9 @@ public:
 	double getScanOpacity(int index) const;    // 0=map, 1=odom
 	int getScanPointSize(int index) const;     // 0=map, 1=odom
 
+	bool isFeaturesShown(int index) const;     // 0=map, 1=odom
+	int getFeaturesPointSize(int index) const; // 0=map, 1=odom
+
 	bool isCloudFiltering() const;
 	bool isSubtractFiltering() const;
 	double getCloudFilteringRadius() const;
@@ -344,6 +347,8 @@ private:
 	QVector<QDoubleSpinBox*> _3dRenderingVoxelSizeScan;
 	QVector<QDoubleSpinBox*> _3dRenderingOpacityScan;
 	QVector<QSpinBox*> _3dRenderingPtSizeScan;
+	QVector<QCheckBox*> _3dRenderingShowFeatures;
+	QVector<QSpinBox*> _3dRenderingPtSizeFeatures;
 };
 
 Q_DECLARE_OPERATORS_FOR_FLAGS(PreferencesDialog::PANEL_FLAGS)

guilib/src/CloudViewer.cpp

@@ -1072,10 +1072,12 @@ void CloudViewer::updateCameraTargetPosition(const Transform & pose)
 
 			this->addOrUpdateCoordinate("reference", pose, 0.2);
 
-			_visualizer->setCameraPosition(
-					cameras.front().pos[0], cameras.front().pos[1], cameras.front().pos[2],
-					cameras.front().focal[0], cameras.front().focal[1], cameras.front().focal[2],
-					cameras.front().view[0], cameras.front().view[1], cameras.front().view[2]);
+			vtkRenderer* renderer = _visualizer->getRendererCollection()->GetFirstRenderer();
+			vtkSmartPointer<vtkCamera> cam = renderer->GetActiveCamera ();
+			cam->SetPosition (cameras.front().pos[0], cameras.front().pos[1], cameras.front().pos[2]);
+			cam->SetFocalPoint (cameras.front().focal[0], cameras.front().focal[1], cameras.front().focal[2]);
+			cam->SetViewUp (cameras.front().view[0], cameras.front().view[1], cameras.front().view[2]);
+			renderer->ResetCameraClippingRange();
 		}
 	}
 

guilib/src/MainWindow.cpp

@@ -709,7 +709,7 @@ void MainWindow::handleEvent(UEvent* anEvent)
 			// we receive too many odometry events! just send without data
 			SensorData data(cv::Mat(), odomEvent->data().id(), odomEvent->data().stamp());
 			data.setGroundTruth(odomEvent->data().groundTruth());
-			OdometryEvent tmp(data, odomEvent->pose(), odomEvent->covariance(), odomEvent->info());
+			OdometryEvent tmp(data, odomEvent->pose(), odomEvent->covariance(), odomEvent->info().copyWithoutData());
 			emit odometryReceived(tmp);
 		}
 	}
@@ -799,6 +799,7 @@ void MainWindow::processOdometry(const rtabmap::OdometryEvent & odom)
 		{
 			bool cloudUpdated = false;
 			bool scanUpdated = false;
+			bool featuresUpdated = false;
 			if(!pose.isNull())
 			{
 				// 3d cloud
@@ -880,6 +881,35 @@ void MainWindow::processOdometry(const rtabmap::OdometryEvent & odom)
 
 					scanUpdated = true;
 				}
+
+				// 3d features
+				if(_preferencesDialog->isFeaturesShown(1))
+				{
+					if(!odom.info().localMap.empty())
+					{
+						pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZRGB>);
+						cloud->resize(odom.info().localMap.size());
+						int i=0;
+						for(std::map<int, cv::Point3f>::const_iterator iter=odom.info().localMap.begin(); iter!=odom.info().localMap.end(); ++iter)
+						{
+							(*cloud)[i].x = iter->second.x;
+							(*cloud)[i].y = iter->second.y;
+							(*cloud)[i].z = iter->second.z;
+
+							// green = inlier, yellow = outliers
+							bool inlier = odom.info().words.find(iter->first) != odom.info().words.end();
+							(*cloud)[i].r = inlier?0:255;
+							(*cloud)[i].g = 255;
+							(*cloud)[i++].b = 0;
+						}
+
+						_ui->widget_cloudViewer->addCloud("featuresOdom", cloud, _odometryCorrection);
+						_ui->widget_cloudViewer->setCloudVisibility("featuresOdom", true);
+						_ui->widget_cloudViewer->setCloudPointSize("featuresOdom", _preferencesDialog->getFeaturesPointSize(1));
+
+						featuresUpdated = true;
+					}
+				}
 			}
 			if(!cloudUpdated && _ui->widget_cloudViewer->getAddedClouds().contains("cloudOdom"))
 			{
@@ -889,6 +919,10 @@ void MainWindow::processOdometry(const rtabmap::OdometryEvent & odom)
 			{
 				_ui->widget_cloudViewer->setCloudVisibility("scanOdom", false);
 			}
+			if(!featuresUpdated && _ui->widget_cloudViewer->getAddedClouds().contains("featuresOdom"))
+			{
+				_ui->widget_cloudViewer->setCloudVisibility("featuresOdom", false);
+			}
 		}
 
 		if(!odom.pose().isNull())
@@ -1658,6 +1692,44 @@ void MainWindow::updateMapCloud(
 				_ui->widget_cloudViewer->setCloudVisibility(scanName.c_str(), false);
 			}
 
+			// 3d features
+			std::string featuresName = uFormat("features%d", iter->first);
+			if(_ui->widget_cloudViewer->isVisible() && _preferencesDialog->isFeaturesShown(0))
+			{
+				if(viewerClouds.contains(featuresName))
+				{
+					// Update only if the pose has changed
+					Transform tFeatures;
+					_ui->widget_cloudViewer->getPose(featuresName, tFeatures);
+					if(tFeatures.isNull() || iter->second != tFeatures)
+					{
+						if(!_ui->widget_cloudViewer->updateCloudPose(featuresName, iter->second))
+						{
+							UERROR("Updating pose features %d failed!", iter->first);
+						}
+					}
+					_ui->widget_cloudViewer->setCloudVisibility(featuresName, true);
+					_ui->widget_cloudViewer->setCloudPointSize(featuresName, _preferencesDialog->getFeaturesPointSize(0));
+				}
+				else if(_cachedSignatures.contains(iter->first))
+				{
+					QMap<int, Signature>::iterator jter = _cachedSignatures.find(iter->first);
+					if(!jter->getWords3().empty())
+					{
+						this->createAndAddFeaturesToMap(iter->first, iter->second, uValue(mapIds, iter->first, -1));
+					}
+				}
+				if(!_preferencesDialog->isFeaturesShown(0))
+				{
+					UDEBUG("Hide features %s", featuresName.c_str());
+					_ui->widget_cloudViewer->setCloudVisibility(featuresName.c_str(), false);
+				}
+			}
+			else if(viewerClouds.contains(featuresName))
+			{
+				_ui->widget_cloudViewer->setCloudVisibility(featuresName.c_str(), false);
+			}
+
 			if(verboseProgress)
 			{
 				_initProgressDialog->appendText(tr("Updated cloud %1 (%2/%3)").arg(iter->first).arg(i).arg(poses.size()));
@@ -1858,6 +1930,20 @@ void MainWindow::updateMapCloud(
 			_ui->widget_cloudViewer->setCloudPointSize("scanOdom", _preferencesDialog->getScanPointSize(1));
 		}
 	}
+	if(viewerClouds.contains("featuresOdom"))
+	{
+		if(!_preferencesDialog->isFeaturesShown(1))
+		{
+			UDEBUG("");
+			_ui->widget_cloudViewer->setCloudVisibility("featuresOdom", false);
+		}
+		else
+		{
+			UDEBUG("");
+			_ui->widget_cloudViewer->updateCloudPose("featuresOdom", _odometryCorrection);
+			_ui->widget_cloudViewer->setCloudPointSize("featuresOdom", _preferencesDialog->getFeaturesPointSize(1));
+		}
+	}
 
 	if(!currentPose.isNull())
 	{
@@ -2023,60 +2109,6 @@ void MainWindow::createAndAddCloudToMap(int nodeId, const Transform & pose, int
 			}
 		}
 	}
-	else if(iter->getWords3().size())
-	{
-		UINFO("Create cloud from 3D words");
-		QColor color = Qt::gray;
-		if(mapId >= 0)
-		{
-			color = (Qt::GlobalColor)(mapId+3 % 12 + 7 );
-		}
-		pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZRGBNormal>);
-		pcl::IndicesPtr indices(new std::vector<int>);
-		cloud->resize(iter->getWords3().size());
-		indices->resize(cloud->size());
-		int oi=0;
-		UASSERT(iter->getWords().size() == iter->getWords3().size());
-		std::multimap<int, cv::KeyPoint>::const_iterator kter=iter->getWords().begin();
-		for(std::multimap<int, cv::Point3f>::const_iterator jter=iter->getWords3().begin();
-				jter!=iter->getWords3().end(); ++jter, ++kter, ++oi)
-		{
-			indices->at(oi) = oi;
-			(*cloud)[oi].x = jter->second.x;
-			(*cloud)[oi].y = jter->second.y;
-			(*cloud)[oi].z = jter->second.z;
-			int u = kter->second.pt.x+0.5;
-			int v = kter->second.pt.x+0.5;
-			if(!iter->sensorData().imageRaw().empty() &&
-				uIsInBounds(u, 0, iter->sensorData().imageRaw().cols-1) &&
-				uIsInBounds(v, 0, iter->sensorData().imageRaw().rows-1))
-			{
-				if(iter->sensorData().imageRaw().channels() == 1)
-				{
-					(*cloud)[oi].r = (*cloud)[oi].g = (*cloud)[oi].b = iter->sensorData().imageRaw().at<unsigned char>(u, v);
-				}
-				else
-				{
-					cv::Vec3b bgr = iter->sensorData().imageRaw().at<cv::Vec3b>(u, v);
-					(*cloud)[oi].r = bgr.val[0];
-					(*cloud)[oi].g = bgr.val[1];
-					(*cloud)[oi].b = bgr.val[2];
-				}
-			}
-			else
-			{
-				(*cloud)[oi].r = (*cloud)[oi].g = (*cloud)[oi].b = 255;
-			}
-		}
-		if(!_ui->widget_cloudViewer->addCloud(cloudName, cloud, pose, color))
-		{
-			UERROR("Adding cloud %d to viewer failed!", nodeId);
-		}
-		else
-		{
-			_createdClouds.insert(std::make_pair(nodeId, std::make_pair(cloud, indices)));
-		}
-	}
 	else
 	{
 		return;
@@ -2171,6 +2203,98 @@ void MainWindow::createAndAddScanToMap(int nodeId, const Transform & pose, int m
 	}
 }
 
+void MainWindow::createAndAddFeaturesToMap(int nodeId, const Transform & pose, int mapId)
+{
+	UDEBUG("");
+	UASSERT(!pose.isNull());
+	std::string cloudName = uFormat("features%d", nodeId);
+	if(_ui->widget_cloudViewer->getAddedClouds().contains(cloudName))
+	{
+		UERROR("Features cloud %d already added to map.", nodeId);
+		return;
+	}
+
+	QMap<int, Signature>::iterator iter = _cachedSignatures.find(nodeId);
+	if(iter == _cachedSignatures.end())
+	{
+		UERROR("Node %d is not in the cache.", nodeId);
+		return;
+	}
+
+	if(_createdFeatures.find(nodeId) != _createdFeatures.end())
+	{
+		UDEBUG("Features cloud %d already created.");
+		return;
+	}
+
+	if(iter->getWords3().size())
+	{
+		UINFO("Create cloud from 3D words");
+		QColor color = Qt::gray;
+		if(mapId >= 0)
+		{
+			color = (Qt::GlobalColor)(mapId+3 % 12 + 7 );
+		}
+
+		cv::Mat rgb;
+		if(!iter->sensorData().imageCompressed().empty() || !iter->sensorData().imageRaw().empty())
+		{
+			SensorData data = iter->sensorData();
+			data.uncompressData(&rgb, 0, 0);
+		}
+
+		pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZRGB>);
+		cloud->resize(iter->getWords3().size());
+		int oi=0;
+		UASSERT(iter->getWords().size() == iter->getWords3().size());
+		std::multimap<int, cv::KeyPoint>::const_iterator kter=iter->getWords().begin();
+		for(std::multimap<int, cv::Point3f>::const_iterator jter=iter->getWords3().begin();
+				jter!=iter->getWords3().end(); ++jter, ++kter, ++oi)
+		{
+			(*cloud)[oi].x = jter->second.x;
+			(*cloud)[oi].y = jter->second.y;
+			(*cloud)[oi].z = jter->second.z;
+			int u = kter->second.pt.x+0.5;
+			int v = kter->second.pt.y+0.5;
+			if(!rgb.empty() &&
+				uIsInBounds(u, 0, rgb.cols-1) &&
+				uIsInBounds(v, 0, rgb.rows-1))
+			{
+				if(rgb.channels() == 1)
+				{
+					(*cloud)[oi].r = (*cloud)[oi].g = (*cloud)[oi].b = rgb.at<unsigned char>(v, u);
+				}
+				else
+				{
+					cv::Vec3b bgr = rgb.at<cv::Vec3b>(v, u);
+					(*cloud)[oi].b = bgr.val[0];
+					(*cloud)[oi].g = bgr.val[1];
+					(*cloud)[oi].r = bgr.val[2];
+				}
+			}
+			else
+			{
+				(*cloud)[oi].r = (*cloud)[oi].g = (*cloud)[oi].b = 255;
+			}
+		}
+		if(!_ui->widget_cloudViewer->addCloud(cloudName, cloud, pose, color))
+		{
+			UERROR("Adding features cloud %d to viewer failed!", nodeId);
+		}
+		else
+		{
+			_createdFeatures.insert(std::make_pair(nodeId, cloud));
+		}
+	}
+	else
+	{
+		return;
+	}
+
+	_ui->widget_cloudViewer->setCloudPointSize(cloudName, _preferencesDialog->getFeaturesPointSize(0));
+	UDEBUG("");
+}
+
 Transform MainWindow::alignPosesToGroundTruth(
 		std::map<int, Transform> & poses,
 		const std::map<int, Transform> & groundTruth)
@@ -4301,6 +4425,7 @@ void MainWindow::clearTheCache()
 	_createdScans.clear();
 	_gridLocalMaps.clear();
 	_projectionLocalMaps.clear();
+	_createdFeatures.clear();
 	_ui->widget_cloudViewer->clear();
 	_ui->widget_cloudViewer->setBackgroundColor(_ui->widget_cloudViewer->getDefaultBackgroundColor());
 	_ui->widget_cloudViewer->clearTrajectory();

guilib/src/PreferencesDialog.cpp

@@ -308,12 +308,21 @@ PreferencesDialog::PreferencesDialog(QWidget * parent) :
 	_3dRenderingPtSizeScan[0] = _ui->spinBox_ptsize_scan;
 	_3dRenderingPtSizeScan[1] = _ui->spinBox_ptsize_odom_scan;
 
+	_3dRenderingShowFeatures.resize(2);
+	_3dRenderingShowFeatures[0] = _ui->checkBox_showFeatures;
+	_3dRenderingShowFeatures[1] = _ui->checkBox_showOdomFeatures;
+
+	_3dRenderingPtSizeFeatures.resize(2);
+	_3dRenderingPtSizeFeatures[0] = _ui->spinBox_ptsize_features;
+	_3dRenderingPtSizeFeatures[1] = _ui->spinBox_ptsize_odom_features;
+
 	for(int i=0; i<2; ++i)
 	{
 		connect(_3dRenderingShowClouds[i], SIGNAL(stateChanged(int)), this, SLOT(makeObsoleteCloudRenderingPanel()));
 		connect(_3dRenderingDecimation[i], SIGNAL(valueChanged(int)), this, SLOT(makeObsoleteCloudRenderingPanel()));
 		connect(_3dRenderingMaxDepth[i], SIGNAL(valueChanged(double)), this, SLOT(makeObsoleteCloudRenderingPanel()));
 		connect(_3dRenderingShowScans[i], SIGNAL(stateChanged(int)), this, SLOT(makeObsoleteCloudRenderingPanel()));
+		connect(_3dRenderingShowFeatures[i], SIGNAL(stateChanged(int)), this, SLOT(makeObsoleteCloudRenderingPanel()));
 
 		connect(_3dRenderingDownsamplingScan[i], SIGNAL(valueChanged(int)), this, SLOT(makeObsoleteCloudRenderingPanel()));
 		connect(_3dRenderingVoxelSizeScan[i], SIGNAL(valueChanged(double)), this, SLOT(makeObsoleteCloudRenderingPanel()));
@@ -321,6 +330,7 @@ PreferencesDialog::PreferencesDialog(QWidget * parent) :
 		connect(_3dRenderingPtSize[i], SIGNAL(valueChanged(int)), this, SLOT(makeObsoleteCloudRenderingPanel()));
 		connect(_3dRenderingOpacityScan[i], SIGNAL(valueChanged(double)), this, SLOT(makeObsoleteCloudRenderingPanel()));
 		connect(_3dRenderingPtSizeScan[i], SIGNAL(valueChanged(int)), this, SLOT(makeObsoleteCloudRenderingPanel()));
+		connect(_3dRenderingPtSizeFeatures[i], SIGNAL(valueChanged(int)), this, SLOT(makeObsoleteCloudRenderingPanel()));
 	}
 
 	connect(_ui->checkBox_showGraphs, SIGNAL(stateChanged(int)), this, SLOT(makeObsoleteCloudRenderingPanel()));
@@ -1119,6 +1129,7 @@ void PreferencesDialog::resetSettings(QGroupBox * groupBox)
 			_3dRenderingDecimation[i]->setValue(8);
 			_3dRenderingMaxDepth[i]->setValue(0.0);
 			_3dRenderingShowScans[i]->setChecked(true);
+			_3dRenderingShowFeatures[i]->setChecked(false);
 
 			_3dRenderingDownsamplingScan[i]->setValue(1);
 			_3dRenderingVoxelSizeScan[i]->setValue(0.0);
@@ -1126,6 +1137,7 @@ void PreferencesDialog::resetSettings(QGroupBox * groupBox)
 			_3dRenderingPtSize[i]->setValue(2);
 			_3dRenderingOpacityScan[i]->setValue(i==0?1.0:0.5);
 			_3dRenderingPtSizeScan[i]->setValue(2);
+			_3dRenderingPtSizeFeatures[i]->setValue(3);
 		}
 
 		_ui->checkBox_showGraphs->setChecked(true);
@@ -1454,6 +1466,7 @@ void PreferencesDialog::readGuiSettings(const QString & filePath)
 		_3dRenderingDecimation[i]->setValue(settings.value(QString("decimation%1").arg(i), _3dRenderingDecimation[i]->value()).toInt());
 		_3dRenderingMaxDepth[i]->setValue(settings.value(QString("maxDepth%1").arg(i), _3dRenderingMaxDepth[i]->value()).toDouble());
 		_3dRenderingShowScans[i]->setChecked(settings.value(QString("showScans%1").arg(i), _3dRenderingShowScans[i]->isChecked()).toBool());
+		_3dRenderingShowFeatures[i]->setChecked(settings.value(QString("showFeatures%1").arg(i), _3dRenderingShowFeatures[i]->isChecked()).toBool());
 
 		_3dRenderingDownsamplingScan[i]->setValue(settings.value(QString("downsamplingScan%1").arg(i), _3dRenderingDownsamplingScan[i]->value()).toInt());
 		_3dRenderingVoxelSizeScan[i]->setValue(settings.value(QString("voxelSizeScan%1").arg(i), _3dRenderingVoxelSizeScan[i]->value()).toDouble());
@@ -1461,6 +1474,7 @@ void PreferencesDialog::readGuiSettings(const QString & filePath)
 		_3dRenderingPtSize[i]->setValue(settings.value(QString("ptSize%1").arg(i), _3dRenderingPtSize[i]->value()).toInt());
 		_3dRenderingOpacityScan[i]->setValue(settings.value(QString("opacityScan%1").arg(i), _3dRenderingOpacityScan[i]->value()).toDouble());
 		_3dRenderingPtSizeScan[i]->setValue(settings.value(QString("ptSizeScan%1").arg(i), _3dRenderingPtSizeScan[i]->value()).toInt());
+		_3dRenderingPtSizeFeatures[i]->setValue(settings.value(QString("ptSizeFeatures%1").arg(i), _3dRenderingPtSizeFeatures[i]->value()).toInt());
 	}
 	_ui->checkBox_showGraphs->setChecked(settings.value("showGraphs", _ui->checkBox_showGraphs->isChecked()).toBool());
 	_ui->checkBox_showLabels->setChecked(settings.value("showLabels", _ui->checkBox_showLabels->isChecked()).toBool());
@@ -1832,6 +1846,7 @@ void PreferencesDialog::writeGuiSettings(const QString & filePath) const
 		settings.setValue(QString("decimation%1").arg(i), _3dRenderingDecimation[i]->value());
 		settings.setValue(QString("maxDepth%1").arg(i), _3dRenderingMaxDepth[i]->value());
 		settings.setValue(QString("showScans%1").arg(i), _3dRenderingShowScans[i]->isChecked());
+		settings.setValue(QString("showFeatures%1").arg(i), _3dRenderingShowFeatures[i]->isChecked());
 
 		settings.setValue(QString("downsamplingScan%1").arg(i), _3dRenderingDownsamplingScan[i]->value());
 		settings.setValue(QString("voxelSizeScan%1").arg(i), _3dRenderingVoxelSizeScan[i]->value());
@@ -1839,6 +1854,7 @@ void PreferencesDialog::writeGuiSettings(const QString & filePath) const
 		settings.setValue(QString("ptSize%1").arg(i), _3dRenderingPtSize[i]->value());
 		settings.setValue(QString("opacityScan%1").arg(i), _3dRenderingOpacityScan[i]->value());
 		settings.setValue(QString("ptSizeScan%1").arg(i), _3dRenderingPtSizeScan[i]->value());
+		settings.setValue(QString("ptSizeFeatures%1").arg(i), _3dRenderingPtSizeFeatures[i]->value());
 	}
 	settings.setValue("showGraphs", _ui->checkBox_showGraphs->isChecked());
 	settings.setValue("showLabels", _ui->checkBox_showLabels->isChecked());
@@ -2137,7 +2153,7 @@ bool PreferencesDialog::validateForm()
 	}
 
 	// verify that Robust and Reject threshold are not set at the same time
-	if(_ui->graphOptimization_robust->isEnabled() && _ui->graphOptimization_maxError->value()>0.0)
+	if(_ui->graphOptimization_robust->isChecked() && _ui->graphOptimization_maxError->value()>0.0)
 	{
 		QMessageBox::warning(this, tr("Parameter warning"),
 				tr("Robust graph optimization and maximum optimization error threshold cannot be "
@@ -3544,6 +3560,18 @@ int PreferencesDialog::getScanPointSize(int index) const
 	UASSERT(index >= 0 && index <= 1);
 	return _3dRenderingPtSizeScan[index]->value();
 }
+
+bool PreferencesDialog::isFeaturesShown(int index) const
+{
+	UASSERT(index >= 0 && index <= 1);
+	return _3dRenderingShowFeatures[index]->isChecked();
+}
+int PreferencesDialog::getFeaturesPointSize(int index) const
+{
+	UASSERT(index >= 0 && index <= 1);
+	return _3dRenderingPtSizeFeatures[index]->value();
+}
+
 bool PreferencesDialog::isCloudFiltering() const
 {
 	return _ui->radioButton_nodeFiltering->isChecked();

guilib/src/ui/preferencesDialog.ui

@@ -63,7 +63,7 @@
           <property name="geometry">
            <rect>
             <x>0</x>
-            <y>0</y>
+            <y>-629</y>
             <width>681</width>
             <height>2010</height>
            </rect>
@@ -86,7 +86,7 @@
                 <enum>QFrame::Raised</enum>
                </property>
                <property name="currentIndex">
-                <number>7</number>
+                <number>1</number>
                </property>
                <widget class="QWidget" name="page_22">
                 <layout class="QVBoxLayout" name="verticalLayout_29" stretch="0,1">
@@ -773,16 +773,22 @@ Show a yellow background when the number of odometry inliers goes under this thr
                        <enum>QFrame::Raised</enum>
                       </property>
                       <layout class="QGridLayout" name="gridLayout_2" columnstretch="0,0,1">
-                       <item row="1" column="2">
-                        <widget class="QLabel" name="label_119">
-                         <property name="text">
-                          <string>Show 3D clouds.</string>
+                       <item row="11" column="0">
+                        <widget class="QDoubleSpinBox" name="doubleSpinBox_opacity_scan">
+                         <property name="suffix">
+                          <string/>
                          </property>
-                         <property name="wordWrap">
-                          <bool>true</bool>
+                         <property name="decimals">
+                          <number>2</number>
                          </property>
-                         <property name="textInteractionFlags">
-                          <set>Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse</set>
+                         <property name="maximum">
+                          <double>1.000000000000000</double>
+                         </property>
+                         <property name="singleStep">
+                          <double>0.100000000000000</double>
+                         </property>
+                         <property name="value">
+                          <double>1.000000000000000</double>
                          </property>
                         </widget>
                        </item>
@@ -799,6 +805,39 @@ Show a yellow background when the number of odometry inliers goes under this thr
                          </property>
                         </widget>
                        </item>
+                       <item row="1" column="2">
+                        <widget class="QLabel" name="label_119">
+                         <property name="text">
+                          <string>Show 3D clouds.</string>
+                         </property>
+                         <property name="wordWrap">
+                          <bool>true</bool>
+                         </property>
+                         <property name="textInteractionFlags">
+                          <set>Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse</set>
+                         </property>
+                        </widget>
+                       </item>
+                       <item row="13" column="1">
+                        <widget class="QCheckBox" name="checkBox_showOdomFeatures">
+                         <property name="text">
+                          <string/>
+                         </property>
+                         <property name="checked">
+                          <bool>true</bool>
+                         </property>
+                        </widget>
+                       </item>
+                       <item row="13" column="0">
+                        <widget class="QCheckBox" name="checkBox_showFeatures">
+                         <property name="text">
+                          <string/>
+                         </property>
+                         <property name="checked">
+                          <bool>true</bool>
+                         </property>
+                        </widget>
+                       </item>
                        <item row="0" column="0">
                         <widget class="QLabel" name="label_154">
                          <property name="text">
@@ -876,25 +915,6 @@ Show a yellow background when the number of odometry inliers goes under this thr
                          </property>
                         </widget>
                        </item>
-                       <item row="11" column="0">
-                        <widget class="QDoubleSpinBox" name="doubleSpinBox_opacity_scan">
-                         <property name="suffix">
-                          <string/>
-                         </property>
-                         <property name="decimals">
-                          <number>2</number>
-                         </property>
-                         <property name="maximum">
-                          <double>1.000000000000000</double>
-                         </property>
-                         <property name="singleStep">
-                          <double>0.100000000000000</double>
-                         </property>
-                         <property name="value">
-                          <double>1.000000000000000</double>
-                         </property>
-                        </widget>
-                       </item>
                        <item row="11" column="1">
                         <widget class="QDoubleSpinBox" name="doubleSpinBox_opacity_odom_scan">
                          <property name="suffix">
@@ -1106,7 +1126,7 @@ Show a yellow background when the number of odometry inliers goes under this thr
                          </property>
                         </widget>
                        </item>
-                       <item row="14" column="2">
+                       <item row="15" column="2">
                         <widget class="QLabel" name="label_213">
                          <property name="text">
                           <string>Show graphs.</string>
@@ -1119,7 +1139,7 @@ Show a yellow background when the number of odometry inliers goes under this thr
                          </property>
                         </widget>
                        </item>
-                       <item row="14" column="0">
+                       <item row="15" column="0">
                         <widget class="QCheckBox" name="checkBox_showGraphs">
                          <property name="text">
                           <string/>
@@ -1168,7 +1188,7 @@ Show a yellow background when the number of odometry inliers goes under this thr
                          </property>
                         </widget>
                        </item>
-                       <item row="15" column="2">
+                       <item row="16" column="2">
                         <widget class="QLabel" name="label_243">
                          <property name="text">
                           <string>Show labels.</string>
@@ -1181,7 +1201,7 @@ Show a yellow background when the number of odometry inliers goes under this thr
                          </property>
                         </widget>
                        </item>
-                       <item row="15" column="0">
+                       <item row="16" column="0">
                         <widget class="QCheckBox" name="checkBox_showLabels">
                          <property name="text">
                           <string/>
@@ -1259,6 +1279,52 @@ Show a yellow background when the number of odometry inliers goes under this thr
                          </property>
                         </widget>
                        </item>
+                       <item row="13" column="2">
+                        <widget class="QLabel" name="label_123">
+                         <property name="text">
+                          <string>Show 3D features.</string>
+                         </property>
+                         <property name="wordWrap">
+                          <bool>true</bool>
+                         </property>
+                         <property name="textInteractionFlags">
+                          <set>Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse</set>
+                         </property>
+                        </widget>
+                       </item>
+                       <item row="14" column="2">
+                        <widget class="QLabel" name="label_166">
+                         <property name="text">
+                          <string>Feature point size.</string>
+                         </property>
+                         <property name="wordWrap">
+                          <bool>true</bool>
+                         </property>
+                         <property name="textInteractionFlags">
+                          <set>Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse</set>
+                         </property>
+                        </widget>
+                       </item>
+                       <item row="14" column="1">
+                        <widget class="QSpinBox" name="spinBox_ptsize_odom_features">
+                         <property name="minimum">
+                          <number>1</number>
+                         </property>
+                         <property name="maximum">
+                          <number>64</number>
+                         </property>
+                        </widget>
+                       </item>
+                       <item row="14" column="0">
+                        <widget class="QSpinBox" name="spinBox_ptsize_features">
+                         <property name="minimum">
+                          <number>1</number>
+                         </property>
+                         <property name="maximum">
+                          <number>64</number>
+                         </property>
+                        </widget>
+                       </item>
                       </layout>
                      </widget>
                     </item>