Gui 3d rendering: added default color scheme option, added min and max range options for laser scans

corelib/include/rtabmap/core/util3d_filtering.h

@@ -48,8 +48,8 @@ namespace util3d
  * operations like computing normals while the scan has already
  * normals and voxel filtering is not used.
  */
-void RTABMAP_EXP commonFiltering(
+LaserScan RTABMAP_EXP commonFiltering(
-		LaserScan & scan,
+		const LaserScan & scan,
 		int downsamplingStep,
 		float rangeMin = 0.0f,
 		float rangeMax = 0.0f,
@@ -99,6 +99,10 @@ pcl::PointCloud<pcl::PointXYZI>::Ptr RTABMAP_EXP voxelize(
 		const pcl::PointCloud<pcl::PointXYZI>::Ptr & cloud,
 		const pcl::IndicesPtr & indices,
 		float voxelSize);
+pcl::PointCloud<pcl::PointXYZINormal>::Ptr RTABMAP_EXP voxelize(
+		const pcl::PointCloud<pcl::PointXYZINormal>::Ptr & cloud,
+		const pcl::IndicesPtr & indices,
+		float voxelSize);
 pcl::PointCloud<pcl::PointXYZ>::Ptr RTABMAP_EXP voxelize(
 		const pcl::PointCloud<pcl::PointXYZ>::Ptr & cloud,
 		float voxelSize);
@@ -114,6 +118,9 @@ pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr RTABMAP_EXP voxelize(
 pcl::PointCloud<pcl::PointXYZI>::Ptr RTABMAP_EXP voxelize(
 		const pcl::PointCloud<pcl::PointXYZI>::Ptr & cloud,
 		float voxelSize);
+pcl::PointCloud<pcl::PointXYZINormal>::Ptr RTABMAP_EXP voxelize(
+		const pcl::PointCloud<pcl::PointXYZINormal>::Ptr & cloud,
+		float voxelSize);
 
 inline pcl::PointCloud<pcl::PointXYZ>::Ptr uniformSampling(
 		const pcl::PointCloud<pcl::PointXYZ>::Ptr & cloud,
@@ -157,6 +164,13 @@ pcl::IndicesPtr RTABMAP_EXP passThrough(
 		float min,
 		float max,
 		bool negative = false);
+pcl::IndicesPtr RTABMAP_EXP passThrough(
+		const pcl::PointCloud<pcl::PointXYZI>::Ptr & cloud,
+		const pcl::IndicesPtr & indices,
+		const std::string & axis,
+		float min,
+		float max,
+		bool negative = false);
 pcl::IndicesPtr RTABMAP_EXP passThrough(
 		const pcl::PointCloud<pcl::PointNormal>::Ptr & cloud,
 		const pcl::IndicesPtr & indices,
@@ -171,6 +185,13 @@ pcl::IndicesPtr RTABMAP_EXP passThrough(
 		float min,
 		float max,
 		bool negative = false);
+pcl::IndicesPtr RTABMAP_EXP passThrough(
+		const pcl::PointCloud<pcl::PointXYZINormal>::Ptr & cloud,
+		const pcl::IndicesPtr & indices,
+		const std::string & axis,
+		float min,
+		float max,
+		bool negative = false);
 pcl::PointCloud<pcl::PointXYZ>::Ptr RTABMAP_EXP passThrough(
 		const pcl::PointCloud<pcl::PointXYZ>::Ptr & cloud,
 		const std::string & axis,
@@ -183,6 +204,12 @@ pcl::PointCloud<pcl::PointXYZRGB>::Ptr RTABMAP_EXP passThrough(
 		float min,
 		float max,
 		bool negative = false);
+pcl::PointCloud<pcl::PointXYZI>::Ptr RTABMAP_EXP passThrough(
+		const pcl::PointCloud<pcl::PointXYZI>::Ptr & cloud,
+		const std::string & axis,
+		float min,
+		float max,
+		bool negative = false);
 pcl::PointCloud<pcl::PointNormal>::Ptr RTABMAP_EXP passThrough(
 		const pcl::PointCloud<pcl::PointNormal>::Ptr & cloud,
 		const std::string & axis,
@@ -195,6 +222,12 @@ pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr RTABMAP_EXP passThrough(
 		float min,
 		float max,
 		bool negative = false);
+pcl::PointCloud<pcl::PointXYZINormal>::Ptr RTABMAP_EXP passThrough(
+		const pcl::PointCloud<pcl::PointXYZINormal>::Ptr & cloud,
+		const std::string & axis,
+		float min,
+		float max,
+		bool negative = false);
 
 pcl::IndicesPtr RTABMAP_EXP cropBox(
 		const pcl::PointCloud<pcl::PointXYZ>::Ptr & cloud,

corelib/src/CameraRGB.cpp

@@ -753,7 +753,7 @@ SensorData CameraImages::captureImage(CameraInfo * info)
 				}
 			}
 			// filter the scan after registration
-			util3d::commonFiltering(scan, _scanDownsampleStep, 0, 0, _scanVoxelSize, _scanNormalsK, _scanNormalsRadius, _scanForceGroundNormalsUp);
+			scan = util3d::commonFiltering(scan, _scanDownsampleStep, 0, 0, _scanVoxelSize, _scanNormalsK, _scanNormalsRadius, _scanForceGroundNormalsUp);
 		}
 	}
 	else

corelib/src/Memory.cpp

@@ -3851,7 +3851,7 @@ Signature * Memory::createSignature(const SensorData & data, const Transform & p
 	LaserScan laserScan = data.laserScanRaw();
 	if(!isIntermediateNode && laserScan.size())
 	{
-		util3d::commonFiltering(laserScan,
+		laserScan = util3d::commonFiltering(laserScan,
 				_laserScanDownsampleStepSize,
 				0,
 				0,

corelib/src/util3d_filtering.cpp

@@ -68,8 +68,8 @@ namespace rtabmap
 namespace util3d
 {
 
-void commonFiltering(
+LaserScan commonFiltering(
-		LaserScan & scan,
+		const LaserScan & scanIn,
 		int downsamplingStep,
 		float rangeMin,
 		float rangeMax,
@@ -78,6 +78,7 @@ void commonFiltering(
 		float normalRadius,
 		bool forceGroundNormalsUp)
 {
+	LaserScan scan = scanIn;
 	UDEBUG("scan size=%d format=%d, step=%d, rangeMin=%f, rangeMax=%f, voxel=%f, normalK=%d, normalRadius=%f",
 			scan.size(), (int)scan.format(), downsamplingStep, rangeMin, rangeMax, voxelSize, normalK, normalRadius);
 	if(!scan.isEmpty())
@@ -256,6 +257,7 @@ void commonFiltering(
 			scan = util3d::adjustNormalsToViewPoint(scan, Eigen::Vector3f(0,0,0), forceGroundNormalsUp);
 		}
 	}
+	return scan;
 }
 
 LaserScan rangeFiltering(
@@ -415,6 +417,10 @@ pcl::PointCloud<pcl::PointXYZI>::Ptr voxelize(const pcl::PointCloud<pcl::PointXY
 {
 	return voxelizeImpl<pcl::PointXYZI>(cloud, indices, voxelSize);
 }
+pcl::PointCloud<pcl::PointXYZINormal>::Ptr voxelize(const pcl::PointCloud<pcl::PointXYZINormal>::Ptr & cloud, const pcl::IndicesPtr & indices, float voxelSize)
+{
+	return voxelizeImpl<pcl::PointXYZINormal>(cloud, indices, voxelSize);
+}
 
 pcl::PointCloud<pcl::PointXYZ>::Ptr voxelize(const pcl::PointCloud<pcl::PointXYZ>::Ptr & cloud, float voxelSize)
 {
@@ -441,6 +447,11 @@ pcl::PointCloud<pcl::PointXYZI>::Ptr voxelize(const pcl::PointCloud<pcl::PointXY
 	pcl::IndicesPtr indices(new std::vector<int>);
 	return voxelize(cloud, indices, voxelSize);
 }
+pcl::PointCloud<pcl::PointXYZINormal>::Ptr voxelize(const pcl::PointCloud<pcl::PointXYZINormal>::Ptr & cloud, float voxelSize)
+{
+	pcl::IndicesPtr indices(new std::vector<int>);
+	return voxelize(cloud, indices, voxelSize);
+}
 
 template<typename PointT>
 typename pcl::PointCloud<PointT>::Ptr randomSamplingImpl(
@@ -494,6 +505,10 @@ pcl::IndicesPtr passThrough(const pcl::PointCloud<pcl::PointXYZRGB>::Ptr & cloud
 {
 	return passThroughImpl<pcl::PointXYZRGB>(cloud, indices, axis, min, max, negative);
 }
+pcl::IndicesPtr passThrough(const pcl::PointCloud<pcl::PointXYZI>::Ptr & cloud, const pcl::IndicesPtr & indices, const std::string & axis, float min, float max, bool negative)
+{
+	return passThroughImpl<pcl::PointXYZI>(cloud, indices, axis, min, max, negative);
+}
 pcl::IndicesPtr passThrough(const pcl::PointCloud<pcl::PointNormal>::Ptr & cloud, const pcl::IndicesPtr & indices, const std::string & axis, float min, float max, bool negative)
 {
 	return passThroughImpl<pcl::PointNormal>(cloud, indices, axis, min, max, negative);
@@ -502,6 +517,10 @@ pcl::IndicesPtr passThrough(const pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr &
 {
 	return passThroughImpl<pcl::PointXYZRGBNormal>(cloud, indices, axis, min, max, negative);
 }
+pcl::IndicesPtr passThrough(const pcl::PointCloud<pcl::PointXYZINormal>::Ptr & cloud, const pcl::IndicesPtr & indices, const std::string & axis, float min, float max, bool negative)
+{
+	return passThroughImpl<pcl::PointXYZINormal>(cloud, indices, axis, min, max, negative);
+}
 
 template<typename PointT>
 typename pcl::PointCloud<PointT>::Ptr passThroughImpl(
@@ -531,6 +550,10 @@ pcl::PointCloud<pcl::PointXYZRGB>::Ptr passThrough(const pcl::PointCloud<pcl::Po
 {
 	return passThroughImpl<pcl::PointXYZRGB>(cloud, axis, min ,max, negative);
 }
+pcl::PointCloud<pcl::PointXYZI>::Ptr passThrough(const pcl::PointCloud<pcl::PointXYZI>::Ptr & cloud, const std::string & axis, float min, float max, bool negative)
+{
+	return passThroughImpl<pcl::PointXYZI>(cloud, axis, min ,max, negative);
+}
 pcl::PointCloud<pcl::PointNormal>::Ptr passThrough(const pcl::PointCloud<pcl::PointNormal>::Ptr & cloud, const std::string & axis, float min, float max, bool negative)
 {
 	return passThroughImpl<pcl::PointNormal>(cloud, axis, min ,max, negative);
@@ -539,6 +562,10 @@ pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr passThrough(const pcl::PointCloud<p
 {
 	return passThroughImpl<pcl::PointXYZRGBNormal>(cloud, axis, min ,max, negative);
 }
+pcl::PointCloud<pcl::PointXYZINormal>::Ptr passThrough(const pcl::PointCloud<pcl::PointXYZINormal>::Ptr & cloud, const std::string & axis, float min, float max, bool negative)
+{
+	return passThroughImpl<pcl::PointXYZINormal>(cloud, axis, min ,max, negative);
+}
 
 template<typename PointT>
 pcl::IndicesPtr cropBoxImpl(

guilib/include/rtabmap/gui/CloudViewer.h

@@ -85,7 +85,8 @@ public:
 			const pcl::PCLPointCloud2Ptr & binaryCloud,
 			const Transform & pose,
 			bool rgb,
-			bool haveNormals,
+			bool hasNormals,
+			bool hasIntensity,
 			const QColor & color = QColor());
 
 	bool addCloud(
@@ -100,6 +101,18 @@ public:
 			const Transform & pose = Transform::getIdentity(),
 			const QColor & color = QColor());
 
+	bool addCloud(
+				const std::string & id,
+				const pcl::PointCloud<pcl::PointXYZINormal>::Ptr & cloud,
+				const Transform & pose = Transform::getIdentity(),
+				const QColor & color = QColor());
+
+	bool addCloud(
+			const std::string & id,
+			const pcl::PointCloud<pcl::PointXYZI>::Ptr & cloud,
+			const Transform & pose = Transform::getIdentity(),
+			const QColor & color = QColor());
+
 	bool addCloud(
 			const std::string & id,
 			const pcl::PointCloud<pcl::PointNormal>::Ptr & cloud,
@@ -287,6 +300,7 @@ public slots:
 	void setDefaultBackgroundColor(const QColor & color);
 	void setBackgroundColor(const QColor & color);
 	void setCloudVisibility(const std::string & id, bool isVisible);
+	void setCloudColorIndex(const std::string & id, int index);
 	void setCloudOpacity(const std::string & id, double opacity = 1.0);
 	void setCloudPointSize(const std::string & id, int size);
 	virtual void clear();

guilib/include/rtabmap/gui/PreferencesDialog.h

@@ -181,12 +181,16 @@ public:
 	double getCloudMaxDepth(int index) const;  // 0=map, 1=odom
 	double getCloudMinDepth(int index) const;  // 0=map, 1=odom
 	std::vector<float> getCloudRoiRatios(int index) const; // 0=map, 1=odom
+	int getCloudColorScheme(int index) const;   // 0=map, 1=odom
 	double getCloudOpacity(int index) const;   // 0=map, 1=odom
 	int getCloudPointSize(int index) const;    // 0=map, 1=odom
 
 	bool isScansShown(int index) const;       // 0=map, 1=odom
 	int getDownsamplingStepScan(int index) const; // 0=map, 1=odom
+	double getScanMaxRange(int index) const; // 0=map, 1=odom
+	double getScanMinRange(int index) const; // 0=map, 1=odom
 	double getCloudVoxelSizeScan(int index) const; // 0=map, 1=odom
+	int getScanColorScheme(int index) const;    // 0=map, 1=odom
 	double getScanOpacity(int index) const;    // 0=map, 1=odom
 	int getScanPointSize(int index) const;     // 0=map, 1=odom
 
@@ -390,11 +394,15 @@ private:
 	QVector<QDoubleSpinBox*> _3dRenderingMaxDepth;
 	QVector<QDoubleSpinBox*> _3dRenderingMinDepth;
 	QVector<QLineEdit*> _3dRenderingRoiRatios;
+	QVector<QSpinBox*> _3dRenderingColorScheme;
 	QVector<QDoubleSpinBox*> _3dRenderingOpacity;
 	QVector<QSpinBox*> _3dRenderingPtSize;
 	QVector<QCheckBox*> _3dRenderingShowScans;
 	QVector<QSpinBox*> _3dRenderingDownsamplingScan;
+	QVector<QDoubleSpinBox*> _3dRenderingMaxRange;
+	QVector<QDoubleSpinBox*> _3dRenderingMinRange;
 	QVector<QDoubleSpinBox*> _3dRenderingVoxelSizeScan;
+	QVector<QSpinBox*> _3dRenderingColorSchemeScan;
 	QVector<QDoubleSpinBox*> _3dRenderingOpacityScan;
 	QVector<QSpinBox*> _3dRenderingPtSizeScan;
 	QVector<QCheckBox*> _3dRenderingShowFeatures;

guilib/src/CloudViewer.cpp

@@ -528,7 +528,8 @@ bool CloudViewer::addCloud(
 		const pcl::PCLPointCloud2Ptr & binaryCloud,
 		const Transform & pose,
 		bool rgb,
-		bool haveNormals,
+		bool hasNormals,
+		bool hasIntensity,
 		const QColor & color)
 {
 	int previousColorIndex = -1;
@@ -541,7 +542,7 @@ bool CloudViewer::addCloud(
 	Eigen::Vector4f origin(pose.x(), pose.y(), pose.z(), 0.0f);
 	Eigen::Quaternionf orientation = Eigen::Quaternionf(pose.toEigen3f().linear());
 
-	if(haveNormals && _aShowNormals->isChecked())
+	if(hasNormals && _aShowNormals->isChecked())
 	{
 		pcl::PointCloud<pcl::PointNormal>::Ptr cloud_xyz (new pcl::PointCloud<pcl::PointNormal>);
 		pcl::fromPCLPointCloud2 (*binaryCloud, *cloud_xyz);
@@ -580,12 +581,18 @@ bool CloudViewer::addCloud(
 			colorHandler.reset(new pcl::visualization::PointCloudColorHandlerRGBField<pcl::PCLPointCloud2>(binaryCloud));
 			_visualizer->addPointCloud (binaryCloud, colorHandler, origin, orientation, id);
 		}
+		else if(hasIntensity)
+		{
+			//rgb
+			colorHandler.reset(new pcl::visualization::PointCloudColorHandlerGenericField<pcl::PCLPointCloud2>(binaryCloud, "intensity"));
+			_visualizer->addPointCloud (binaryCloud, colorHandler, origin, orientation, id);
+		}
 		else if(previousColorIndex == 5)
 		{
 			previousColorIndex = -1;
 		}
 
-		if(haveNormals)
+		if(hasNormals)
 		{
 			//normals
 			colorHandler.reset (new pcl::visualization::PointCloudColorHandlerGenericField<pcl::PCLPointCloud2> (binaryCloud, "normal_x"));
@@ -604,7 +611,7 @@ bool CloudViewer::addCloud(
 		{
 			_visualizer->updateColorHandlerIndex(id, previousColorIndex);
 		}
-		else if(rgb)
+		else if(rgb || hasIntensity)
 		{
 			_visualizer->updateColorHandlerIndex(id, 5);
 		}
@@ -627,7 +634,7 @@ bool CloudViewer::addCloud(
 {
 	pcl::PCLPointCloud2Ptr binaryCloud(new pcl::PCLPointCloud2);
 	pcl::toPCLPointCloud2(*cloud, *binaryCloud);
-	return addCloud(id, binaryCloud, pose, true, true, color);
+	return addCloud(id, binaryCloud, pose, true, true, false, color);
 }
 
 bool CloudViewer::addCloud(
@@ -638,7 +645,29 @@ bool CloudViewer::addCloud(
 {
 	pcl::PCLPointCloud2Ptr binaryCloud(new pcl::PCLPointCloud2);
 	pcl::toPCLPointCloud2(*cloud, *binaryCloud);
-	return addCloud(id, binaryCloud, pose, true, false, color);
+	return addCloud(id, binaryCloud, pose, true, false, false, color);
+}
+
+bool CloudViewer::addCloud(
+		const std::string & id,
+		const pcl::PointCloud<pcl::PointXYZINormal>::Ptr & cloud,
+		const Transform & pose,
+		const QColor & color)
+{
+	pcl::PCLPointCloud2Ptr binaryCloud(new pcl::PCLPointCloud2);
+	pcl::toPCLPointCloud2(*cloud, *binaryCloud);
+	return addCloud(id, binaryCloud, pose, false, true, true, color);
+}
+
+bool CloudViewer::addCloud(
+		const std::string & id,
+		const pcl::PointCloud<pcl::PointXYZI>::Ptr & cloud,
+		const Transform & pose,
+		const QColor & color)
+{
+	pcl::PCLPointCloud2Ptr binaryCloud(new pcl::PCLPointCloud2);
+	pcl::toPCLPointCloud2(*cloud, *binaryCloud);
+	return addCloud(id, binaryCloud, pose, false, false, true, color);
 }
 
 bool CloudViewer::addCloud(
@@ -649,7 +678,7 @@ bool CloudViewer::addCloud(
 {
 	pcl::PCLPointCloud2Ptr binaryCloud(new pcl::PCLPointCloud2);
 	pcl::toPCLPointCloud2(*cloud, *binaryCloud);
-	return addCloud(id, binaryCloud, pose, false, true, color);
+	return addCloud(id, binaryCloud, pose, false, true, false, color);
 }
 
 bool CloudViewer::addCloud(
@@ -660,7 +689,7 @@ bool CloudViewer::addCloud(
 {
 	pcl::PCLPointCloud2Ptr binaryCloud(new pcl::PCLPointCloud2);
 	pcl::toPCLPointCloud2(*cloud, *binaryCloud);
-	return addCloud(id, binaryCloud, pose, false, false, color);
+	return addCloud(id, binaryCloud, pose, false, false, false, color);
 }
 
 bool CloudViewer::addCloudMesh(
@@ -2132,6 +2161,14 @@ bool CloudViewer::getCloudVisibility(const std::string & id)
 	return false;
 }
 
+void CloudViewer::setCloudColorIndex(const std::string & id, int index)
+{
+	if(index>0)
+	{
+		_visualizer->updateColorHandlerIndex(id, index-1);
+	}
+}
+
 void CloudViewer::setCloudOpacity(const std::string & id, double opacity)
 {
 	double lastOpacity;

guilib/src/DatabaseViewer.cpp

@@ -3195,6 +3195,15 @@ void DatabaseViewer::update(int value,
 							}
 							cloudViewer_->addCloud("scan", scan, pose, Qt::yellow);
 						}
+						else if(data.laserScanRaw().hasIntensity() && data.laserScanRaw().hasNormals())
+						{
+							pcl::PointCloud<pcl::PointXYZINormal>::Ptr scan = util3d::laserScanToPointCloudINormal(data.laserScanRaw(), data.laserScanRaw().localTransform());
+							if(ui_->doubleSpinBox_voxelSize->value() > 0.0)
+							{
+								scan = util3d::voxelize(scan, ui_->doubleSpinBox_voxelSize->value());
+							}
+							cloudViewer_->addCloud("scan", scan, pose, Qt::yellow);
+						}
 						else if(data.laserScanRaw().hasNormals())
 						{
 							pcl::PointCloud<pcl::PointNormal>::Ptr scan = util3d::laserScanToPointCloudNormal(data.laserScanRaw(), data.laserScanRaw().localTransform());

guilib/src/MainWindow.cpp

@@ -1099,6 +1099,7 @@ void MainWindow::processOdometry(const rtabmap::OdometryEvent & odom, bool dataI
 						}
 					}
 					_cloudViewer->setCloudVisibility("cloudOdom", true);
+					_cloudViewer->setCloudColorIndex("cloudOdom", _preferencesDialog->getCloudColorScheme(1));
 					_cloudViewer->setCloudOpacity("cloudOdom", _preferencesDialog->getCloudOpacity(1));
 					_cloudViewer->setCloudPointSize("cloudOdom", _preferencesDialog->getCloudPointSize(1));
 
@@ -1122,6 +1123,7 @@ void MainWindow::processOdometry(const rtabmap::OdometryEvent & odom, bool dataI
 						else
 						{
 							_cloudViewer->setCloudVisibility("scanMapOdom", true);
+							_cloudViewer->setCloudColorIndex("scanMapOdom", _preferencesDialog->getScanColorScheme(1));
 							_cloudViewer->setCloudOpacity("scanMapOdom", _preferencesDialog->getScanOpacity(1));
 							_cloudViewer->setCloudPointSize("scanMapOdom", _preferencesDialog->getScanPointSize(1));
 						}
@@ -1133,9 +1135,14 @@ void MainWindow::processOdometry(const rtabmap::OdometryEvent & odom, bool dataI
 				{
 					LaserScan scan = odom.data().laserScanRaw();
 
-					if(_preferencesDialog->getDownsamplingStepScan(1) > 0)
+					if(_preferencesDialog->getDownsamplingStepScan(1) > 1 ||
+						_preferencesDialog->getScanMaxRange(1) > 0.0f ||
+						_preferencesDialog->getScanMinRange(1) > 0.0f)
 					{
-						scan = util3d::downsample(scan, _preferencesDialog->getDownsamplingStepScan(1));
+						scan = util3d::commonFiltering(scan,
+								_preferencesDialog->getDownsamplingStepScan(1),
+								_preferencesDialog->getScanMinRange(1),
+								_preferencesDialog->getScanMaxRange(1));
 					}
 
 					pcl::PointCloud<pcl::PointNormal>::Ptr cloud;
@@ -1152,6 +1159,7 @@ void MainWindow::processOdometry(const rtabmap::OdometryEvent & odom, bool dataI
 					else
 					{
 						_cloudViewer->setCloudVisibility("scanOdom", true);
+						_cloudViewer->setCloudColorIndex("scanOdom", _preferencesDialog->getScanColorScheme(1));
 						_cloudViewer->setCloudOpacity("scanOdom", _preferencesDialog->getScanOpacity(1));
 						_cloudViewer->setCloudPointSize("scanOdom", _preferencesDialog->getScanPointSize(1));
 						scanUpdated = true;
@@ -2169,6 +2177,7 @@ void MainWindow::updateMapCloud(
 						}
 					}
 					_cloudViewer->setCloudVisibility(cloudName, (_cloudViewer->isVisible() && _preferencesDialog->isCloudsShown(0)));
+					_cloudViewer->setCloudColorIndex(cloudName, _preferencesDialog->getCloudColorScheme(0));
 					_cloudViewer->setCloudOpacity(cloudName, _preferencesDialog->getCloudOpacity(0));
 					_cloudViewer->setCloudPointSize(cloudName, _preferencesDialog->getCloudPointSize(0));
 				}
@@ -2210,6 +2219,7 @@ void MainWindow::updateMapCloud(
 						}
 					}
 					_cloudViewer->setCloudVisibility(scanName, _preferencesDialog->isScansShown(0));
+					_cloudViewer->setCloudColorIndex(scanName, _preferencesDialog->getScanColorScheme(0));
 					_cloudViewer->setCloudOpacity(scanName, _preferencesDialog->getScanOpacity(0));
 					_cloudViewer->setCloudPointSize(scanName, _preferencesDialog->getScanPointSize(0));
 				}
@@ -2643,6 +2653,7 @@ void MainWindow::updateMapCloud(
 		{
 			UDEBUG("");
 			_cloudViewer->updateCloudPose("cloudOdom", _odometryCorrection);
+			_cloudViewer->setCloudColorIndex("cloudOdom", _preferencesDialog->getCloudColorScheme(1));
 			_cloudViewer->setCloudOpacity("cloudOdom", _preferencesDialog->getCloudOpacity(1));
 			_cloudViewer->setCloudPointSize("cloudOdom", _preferencesDialog->getCloudPointSize(1));
 		}
@@ -2658,6 +2669,7 @@ void MainWindow::updateMapCloud(
 		{
 			UDEBUG("");
 			_cloudViewer->updateCloudPose("scanOdom", _odometryCorrection);
+			_cloudViewer->setCloudColorIndex("scanOdom", _preferencesDialog->getScanColorScheme(1));
 			_cloudViewer->setCloudOpacity("scanOdom", _preferencesDialog->getScanOpacity(1));
 			_cloudViewer->setCloudPointSize("scanOdom", _preferencesDialog->getScanPointSize(1));
 		}
@@ -2673,6 +2685,7 @@ void MainWindow::updateMapCloud(
 		{
 			UDEBUG("");
 			_cloudViewer->updateCloudPose("scanMapOdom", _odometryCorrection);
+			_cloudViewer->setCloudColorIndex("scanMapOdom", _preferencesDialog->getScanColorScheme(1));
 			_cloudViewer->setCloudOpacity("scanMapOdom", _preferencesDialog->getScanOpacity(1));
 			_cloudViewer->setCloudPointSize("scanMapOdom", _preferencesDialog->getScanPointSize(1));
 		}
@@ -3016,6 +3029,7 @@ std::pair<pcl::PointCloud<pcl::PointXYZRGB>::Ptr, pcl::IndicesPtr> MainWindow::c
 					_cachedClouds.insert(std::make_pair(nodeId, outputPair));
 					_createdCloudsMemoryUsage += (long)(output->size() * sizeof(pcl::PointXYZRGB) + indices->size()*sizeof(int));
 				}
+				_cloudViewer->setCloudColorIndex(cloudName, _preferencesDialog->getCloudColorScheme(0));
 				_cloudViewer->setCloudOpacity(cloudName, _preferencesDialog->getCloudOpacity(0));
 				_cloudViewer->setCloudPointSize(cloudName, _preferencesDialog->getCloudPointSize(0));
 			}
@@ -3054,19 +3068,30 @@ void MainWindow::createAndAddScanToMap(int nodeId, const Transform & pose, int m
 		LaserScan scan;
 		iter->sensorData().uncompressData(0, 0, &scan);
 
-		if(_preferencesDialog->getDownsamplingStepScan(0) > 0)
+		if(_preferencesDialog->getDownsamplingStepScan(0) > 1 ||
+			_preferencesDialog->getScanMaxRange(0) > 0.0f ||
+			_preferencesDialog->getScanMinRange(0) > 0.0f)
 		{
-			scan = util3d::downsample(scan, _preferencesDialog->getDownsamplingStepScan(0));
+			scan = util3d::commonFiltering(scan,
+					_preferencesDialog->getDownsamplingStepScan(0),
+					_preferencesDialog->getScanMinRange(0),
+					_preferencesDialog->getScanMaxRange(0));
 		}
 
 		pcl::PointCloud<pcl::PointXYZ>::Ptr cloud;
 		pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloudRGB;
+		pcl::PointCloud<pcl::PointXYZI>::Ptr cloudI;
 		pcl::PointCloud<pcl::PointNormal>::Ptr cloudWithNormals;
 		pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr cloudRGBWithNormals;
+		pcl::PointCloud<pcl::PointXYZINormal>::Ptr cloudIWithNormals;
 		if(scan.hasNormals() && scan.hasRGB() && _preferencesDialog->getCloudVoxelSizeScan(0) <= 0.0)
 		{
 			cloudRGBWithNormals = util3d::laserScanToPointCloudRGBNormal(scan, scan.localTransform());
 		}
+		else if(scan.hasNormals() && scan.hasIntensity() && _preferencesDialog->getCloudVoxelSizeScan(0) <= 0.0)
+		{
+			cloudIWithNormals = util3d::laserScanToPointCloudINormal(scan, scan.localTransform());
+		}
 		else if((scan.hasNormals()) && _preferencesDialog->getCloudVoxelSizeScan(0) <= 0.0)
 		{
 			cloudWithNormals = util3d::laserScanToPointCloudNormal(scan, scan.localTransform());
@@ -3075,6 +3100,10 @@ void MainWindow::createAndAddScanToMap(int nodeId, const Transform & pose, int m
 		{
 			cloudRGB = util3d::laserScanToPointCloudRGB(scan, scan.localTransform());
 		}
+		else if(scan.hasIntensity())
+		{
+			cloudI = util3d::laserScanToPointCloudI(scan, scan.localTransform());
+		}
 		else
 		{
 			cloud = util3d::laserScanToPointCloud(scan, scan.localTransform());
@@ -3090,6 +3119,10 @@ void MainWindow::createAndAddScanToMap(int nodeId, const Transform & pose, int m
 			{
 				cloudRGB = util3d::voxelize(cloudRGB, _preferencesDialog->getCloudVoxelSizeScan(0));
 			}
+			if(cloudI.get())
+			{
+				cloudI = util3d::voxelize(cloudI, _preferencesDialog->getCloudVoxelSizeScan(0));
+			}
 		}
 
 		// Do ceiling/floor filtering
@@ -3110,6 +3143,19 @@ void MainWindow::createAndAddScanToMap(int nodeId, const Transform & pose, int m
 				//transform back in sensor frame
 				cloudRGBWithNormals = util3d::transformPointCloud(cloudTransformed, pose.inverse());
 			}
+			if(cloudIWithNormals.get())
+			{
+				// perform in /map frame
+				pcl::PointCloud<pcl::PointXYZINormal>::Ptr cloudTransformed = util3d::transformPointCloud(cloudIWithNormals, pose);
+				cloudTransformed = rtabmap::util3d::passThrough(
+						cloudTransformed,
+						"z",
+						_preferencesDialog->getScanFloorFilteringHeight()==0.0?(float)std::numeric_limits<int>::min():_preferencesDialog->getScanFloorFilteringHeight(),
+						_preferencesDialog->getScanCeilingFilteringHeight()==0.0?(float)std::numeric_limits<int>::max():_preferencesDialog->getScanCeilingFilteringHeight());
+
+				//transform back in sensor frame
+				cloudIWithNormals = util3d::transformPointCloud(cloudTransformed, pose.inverse());
+			}
 			if(cloudWithNormals.get())
 			{
 				// perform in /map frame
@@ -3136,6 +3182,19 @@ void MainWindow::createAndAddScanToMap(int nodeId, const Transform & pose, int m
 				//transform back in sensor frame
 				cloudRGB = util3d::transformPointCloud(cloudTransformed, pose.inverse());
 			}
+			if(cloudI.get())
+			{
+				// perform in /map frame
+				pcl::PointCloud<pcl::PointXYZI>::Ptr cloudTransformed = util3d::transformPointCloud(cloudI, pose);
+				cloudTransformed = rtabmap::util3d::passThrough(
+						cloudTransformed,
+						"z",
+						_preferencesDialog->getScanFloorFilteringHeight()==0.0?(float)std::numeric_limits<int>::min():_preferencesDialog->getScanFloorFilteringHeight(),
+						_preferencesDialog->getScanCeilingFilteringHeight()==0.0?(float)std::numeric_limits<int>::max():_preferencesDialog->getScanCeilingFilteringHeight());
+
+				//transform back in sensor frame
+				cloudI = util3d::transformPointCloud(cloudTransformed, pose.inverse());
+			}
 			if(cloud.get())
 			{
 				// perform in /map frame
@@ -3151,7 +3210,7 @@ void MainWindow::createAndAddScanToMap(int nodeId, const Transform & pose, int m
 			}
 		}
 
-		if(	(cloud.get() || cloudRGB.get()) &&
+		if(	(cloud.get() || cloudRGB.get() || cloudI.get()) &&
 		   (_preferencesDialog->getScanNormalKSearch() > 0 || _preferencesDialog->getScanNormalRadiusSearch() > 0.0))
 		{
 			Eigen::Vector3f scanViewpoint(
@@ -3174,14 +3233,28 @@ void MainWindow::createAndAddScanToMap(int nodeId, const Transform & pose, int m
 				pcl::concatenateFields(*cloud, *normals, *cloudWithNormals);
 				cloud.reset();
 			}
-			else
+			else if(cloudRGB.get() && cloudRGB->size())
 			{
-				UASSERT(cloudRGB.get() && cloudRGB->size()); // Assuming 4 channels cannot be 2D
+				// Assuming 3D
 				normals = util3d::computeNormals(cloudRGB, _preferencesDialog->getScanNormalKSearch(), _preferencesDialog->getScanNormalRadiusSearch(), scanViewpoint);
 				cloudRGBWithNormals.reset(new pcl::PointCloud<pcl::PointXYZRGBNormal>);
 				pcl::concatenateFields(*cloudRGB, *normals, *cloudRGBWithNormals);
 				cloudRGB.reset();
 			}
+			else if(cloudI.get())
+			{
+				if(scan.is2d())
+				{
+					normals = util3d::computeFastOrganizedNormals2D(cloudI, _preferencesDialog->getScanNormalKSearch(), _preferencesDialog->getScanNormalRadiusSearch(), scanViewpoint);
+				}
+				else
+				{
+					normals = util3d::computeNormals(cloudI, _preferencesDialog->getScanNormalKSearch(), _preferencesDialog->getScanNormalRadiusSearch(), scanViewpoint);
+				}
+				cloudIWithNormals.reset(new pcl::PointCloud<pcl::PointXYZINormal>);
+				pcl::concatenateFields(*cloud, *normals, *cloudIWithNormals);
+				cloudI.reset();
+			}
 		}
 
 		QColor color = Qt::gray;
@@ -3198,6 +3271,21 @@ void MainWindow::createAndAddScanToMap(int nodeId, const Transform & pose, int m
 				scan = LaserScan(util3d::laserScanFromPointCloud(*cloudRGBWithNormals, scan.localTransform().inverse()), scan.maxPoints(), scan.maxRange(), LaserScan::kXYZRGBNormal, scan.localTransform());
 			}
 		}
+		else if(cloudIWithNormals.get())
+		{
+			added = _cloudViewer->addCloud(scanName, cloudIWithNormals, pose, color);
+			if(added && nodeId > 0)
+			{
+				if(scan.is2d())
+				{
+					scan = LaserScan(util3d::laserScan2dFromPointCloud(*cloudIWithNormals, scan.localTransform().inverse()), scan.maxPoints(), scan.maxRange(), LaserScan::kXYINormal, scan.localTransform());
+				}
+				else
+				{
+					scan = LaserScan(util3d::laserScanFromPointCloud(*cloudIWithNormals, scan.localTransform().inverse()), scan.maxPoints(), scan.maxRange(), LaserScan::kXYZINormal, scan.localTransform());
+				}
+			}
+		}
 		else if(cloudWithNormals.get())
 		{
 			added = _cloudViewer->addCloud(scanName, cloudWithNormals, pose, color);
@@ -3221,6 +3309,21 @@ void MainWindow::createAndAddScanToMap(int nodeId, const Transform & pose, int m
 				scan = LaserScan(util3d::laserScanFromPointCloud(*cloudRGB, scan.localTransform().inverse()), scan.maxPoints(), scan.maxRange(), LaserScan::kXYZRGB, scan.localTransform());
 			}
 		}
+		else if(cloudI.get())
+		{
+			added = _cloudViewer->addCloud(scanName, cloudI, pose, color);
+			if(added && nodeId > 0)
+			{
+				if(scan.is2d())
+				{
+					scan = LaserScan(util3d::laserScan2dFromPointCloud(*cloudI, scan.localTransform().inverse()), scan.maxPoints(), scan.maxRange(), LaserScan::kXYI, scan.localTransform());
+				}
+				else
+				{
+					scan = LaserScan(util3d::laserScanFromPointCloud(*cloudI, scan.localTransform().inverse()), scan.maxPoints(), scan.maxRange(), LaserScan::kXYZI, scan.localTransform());
+				}
+			}
+		}
 		else
 		{
 			UASSERT(cloud.get());
@@ -3248,6 +3351,7 @@ void MainWindow::createAndAddScanToMap(int nodeId, const Transform & pose, int m
 				_createdScans.insert(std::make_pair(nodeId, scan)); // keep scan in scan frame
 			}
 
+			_cloudViewer->setCloudColorIndex(scanName, _preferencesDialog->getScanColorScheme(0));
 			_cloudViewer->setCloudOpacity(scanName, _preferencesDialog->getScanOpacity(0));
 			_cloudViewer->setCloudPointSize(scanName, _preferencesDialog->getScanPointSize(0));
 		}

guilib/src/PreferencesDialog.cpp

@@ -363,6 +363,10 @@ PreferencesDialog::PreferencesDialog(QWidget * parent) :
 	_3dRenderingRoiRatios[0] = _ui->lineEdit_roiRatios;
 	_3dRenderingRoiRatios[1] = _ui->lineEdit_roiRatios_odom;
 
+	_3dRenderingColorScheme.resize(2);
+	_3dRenderingColorScheme[0] = _ui->spinBox_colorScheme;
+	_3dRenderingColorScheme[1] = _ui->spinBox_colorScheme_odom;
+
 	_3dRenderingOpacity.resize(2);
 	_3dRenderingOpacity[0] = _ui->doubleSpinBox_opacity;
 	_3dRenderingOpacity[1] = _ui->doubleSpinBox_opacity_odom;
@@ -379,10 +383,22 @@ PreferencesDialog::PreferencesDialog(QWidget * parent) :
 	_3dRenderingDownsamplingScan[0] = _ui->spinBox_downsamplingScan;
 	_3dRenderingDownsamplingScan[1] = _ui->spinBox_downsamplingScan_odom;
 
+	_3dRenderingMaxRange.resize(2);
+	_3dRenderingMaxRange[0] = _ui->doubleSpinBox_maxRange;
+	_3dRenderingMaxRange[1] = _ui->doubleSpinBox_maxRange_odom;
+
+	_3dRenderingMinRange.resize(2);
+	_3dRenderingMinRange[0] = _ui->doubleSpinBox_minRange;
+	_3dRenderingMinRange[1] = _ui->doubleSpinBox_minRange_odom;
+
 	_3dRenderingVoxelSizeScan.resize(2);
 	_3dRenderingVoxelSizeScan[0] = _ui->doubleSpinBox_voxelSizeScan;
 	_3dRenderingVoxelSizeScan[1] = _ui->doubleSpinBox_voxelSizeScan_odom;
 
+	_3dRenderingColorSchemeScan.resize(2);
+	_3dRenderingColorSchemeScan[0] = _ui->spinBox_colorSchemeScan;
+	_3dRenderingColorSchemeScan[1] = _ui->spinBox_colorSchemeScan_odom;
+
 	_3dRenderingOpacityScan.resize(2);
 	_3dRenderingOpacityScan[0] = _ui->doubleSpinBox_opacity_scan;
 	_3dRenderingOpacityScan[1] = _ui->doubleSpinBox_opacity_odom_scan;
@@ -415,9 +431,13 @@ PreferencesDialog::PreferencesDialog(QWidget * parent) :
 		connect(_3dRenderingShowFrustums[i], SIGNAL(stateChanged(int)), this, SLOT(makeObsoleteCloudRenderingPanel()));
 
 		connect(_3dRenderingDownsamplingScan[i], SIGNAL(valueChanged(int)), this, SLOT(makeObsoleteCloudRenderingPanel()));
+		connect(_3dRenderingMaxRange[i], SIGNAL(valueChanged(double)), this, SLOT(makeObsoleteCloudRenderingPanel()));
+		connect(_3dRenderingMinRange[i], SIGNAL(valueChanged(double)), this, SLOT(makeObsoleteCloudRenderingPanel()));
 		connect(_3dRenderingVoxelSizeScan[i], SIGNAL(valueChanged(double)), this, SLOT(makeObsoleteCloudRenderingPanel()));
+		connect(_3dRenderingColorScheme[i], SIGNAL(valueChanged(int)), this, SLOT(makeObsoleteCloudRenderingPanel()));
 		connect(_3dRenderingOpacity[i], SIGNAL(valueChanged(double)), this, SLOT(makeObsoleteCloudRenderingPanel()));
 		connect(_3dRenderingPtSize[i], SIGNAL(valueChanged(int)), this, SLOT(makeObsoleteCloudRenderingPanel()));
+		connect(_3dRenderingColorSchemeScan[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()));
@@ -1414,9 +1434,13 @@ void PreferencesDialog::resetSettings(QGroupBox * groupBox)
 			_3dRenderingShowFrustums[i]->setChecked(false);
 
 			_3dRenderingDownsamplingScan[i]->setValue(1);
+			_3dRenderingMaxRange[i]->setValue(0.0);
+			_3dRenderingMinRange[i]->setValue(0.0);
 			_3dRenderingVoxelSizeScan[i]->setValue(0.0);
+			_3dRenderingColorScheme[i]->setValue(0);
 			_3dRenderingOpacity[i]->setValue(i==0?1.0:0.75);
 			_3dRenderingPtSize[i]->setValue(2);
+			_3dRenderingColorSchemeScan[i]->setValue(0);
 			_3dRenderingOpacityScan[i]->setValue(i==0?1.0:0.5);
 			_3dRenderingPtSizeScan[i]->setValue(2);
 			_3dRenderingPtSizeFeatures[i]->setValue(3);
@@ -1818,9 +1842,13 @@ void PreferencesDialog::readGuiSettings(const QString & filePath)
 		_3dRenderingShowFrustums[i]->setChecked(settings.value(QString("showFrustums%1").arg(i), _3dRenderingShowFrustums[i]->isChecked()).toBool());
 
 		_3dRenderingDownsamplingScan[i]->setValue(settings.value(QString("downsamplingScan%1").arg(i), _3dRenderingDownsamplingScan[i]->value()).toInt());
+		_3dRenderingMaxRange[i]->setValue(settings.value(QString("maxRange%1").arg(i), _3dRenderingMaxRange[i]->value()).toDouble());
+		_3dRenderingMinRange[i]->setValue(settings.value(QString("minRange%1").arg(i), _3dRenderingMinRange[i]->value()).toDouble());
 		_3dRenderingVoxelSizeScan[i]->setValue(settings.value(QString("voxelSizeScan%1").arg(i), _3dRenderingVoxelSizeScan[i]->value()).toDouble());
+		_3dRenderingColorScheme[i]->setValue(settings.value(QString("colorScheme%1").arg(i), _3dRenderingColorScheme[i]->value()).toInt());
 		_3dRenderingOpacity[i]->setValue(settings.value(QString("opacity%1").arg(i), _3dRenderingOpacity[i]->value()).toDouble());
 		_3dRenderingPtSize[i]->setValue(settings.value(QString("ptSize%1").arg(i), _3dRenderingPtSize[i]->value()).toInt());
+		_3dRenderingColorSchemeScan[i]->setValue(settings.value(QString("colorSchemeScan%1").arg(i), _3dRenderingColorSchemeScan[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());
@@ -2221,9 +2249,13 @@ void PreferencesDialog::writeGuiSettings(const QString & filePath) const
 		settings.setValue(QString("showFrustums%1").arg(i), _3dRenderingShowFrustums[i]->isChecked());
 
 		settings.setValue(QString("downsamplingScan%1").arg(i), _3dRenderingDownsamplingScan[i]->value());
+		settings.setValue(QString("maxRange%1").arg(i), _3dRenderingMaxRange[i]->value());
+		settings.setValue(QString("minRange%1").arg(i), _3dRenderingMinRange[i]->value());
 		settings.setValue(QString("voxelSizeScan%1").arg(i), _3dRenderingVoxelSizeScan[i]->value());
+		settings.setValue(QString("colorScheme%1").arg(i), _3dRenderingColorScheme[i]->value());
 		settings.setValue(QString("opacity%1").arg(i), _3dRenderingOpacity[i]->value());
 		settings.setValue(QString("ptSize%1").arg(i), _3dRenderingPtSize[i]->value());
+		settings.setValue(QString("colorSchemeScan%1").arg(i), _3dRenderingColorSchemeScan[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());
@@ -4431,6 +4463,11 @@ std::vector<float> PreferencesDialog::getCloudRoiRatios(int index) const
 	}
 	return roiRatios;
 }
+int PreferencesDialog::getCloudColorScheme(int index) const
+{
+	UASSERT(index >= 0 && index <= 1);
+	return _3dRenderingColorScheme[index]->value();
+}
 double PreferencesDialog::getCloudOpacity(int index) const
 {
 	UASSERT(index >= 0 && index <= 1);
@@ -4452,11 +4489,26 @@ int PreferencesDialog::getDownsamplingStepScan(int index) const
 	UASSERT(index >= 0 && index <= 1);
 	return _3dRenderingDownsamplingScan[index]->value();
 }
+double PreferencesDialog::getScanMaxRange(int index) const
+{
+	UASSERT(index >= 0 && index <= 1);
+	return _3dRenderingMaxRange[index]->value();
+}
+double PreferencesDialog::getScanMinRange(int index) const
+{
+	UASSERT(index >= 0 && index <= 1);
+	return _3dRenderingMinRange[index]->value();
+}
 double PreferencesDialog::getCloudVoxelSizeScan(int index) const
 {
 	UASSERT(index >= 0 && index <= 1);
 	return _3dRenderingVoxelSizeScan[index]->value();
 }
+int PreferencesDialog::getScanColorScheme(int index) const
+{
+	UASSERT(index >= 0 && index <= 1);
+	return _3dRenderingColorSchemeScan[index]->value();
+}
 double PreferencesDialog::getScanOpacity(int index) const
 {
 	UASSERT(index >= 0 && index <= 1);

guilib/src/ui/preferencesDialog.ui

@@ -7,7 +7,7 @@
     <x>0</x>
     <y>0</y>
     <width>976</width>
-    <height>739</height>
+    <height>900</height>
    </rect>
   </property>
   <property name="sizePolicy">
@@ -63,7 +63,7 @@
           <property name="geometry">
            <rect>
             <x>0</x>
-            <y>-1377</y>
+            <y>-78</y>
             <width>673</width>
             <height>2834</height>
            </rect>
@@ -86,7 +86,7 @@
                 <enum>QFrame::Raised</enum>
                </property>
                <property name="currentIndex">
-                <number>5</number>
+                <number>1</number>
                </property>
                <widget class="QWidget" name="page_22">
                 <layout class="QVBoxLayout" name="verticalLayout_29" stretch="0,1">
@@ -503,6 +503,74 @@ Show a yellow background when the number of odometry inliers goes under this thr
                       <layout class="QVBoxLayout" name="verticalLayout_112">
                        <item>
                         <layout class="QGridLayout" name="gridLayout_2" columnstretch="0,0,1">
+                         <item row="5" column="0">
+                          <widget class="QLineEdit" name="lineEdit_roiRatios"/>
+                         </item>
+                         <item row="15" column="0">
+                          <widget class="QSpinBox" name="spinBox_ptsize">
+                           <property name="minimum">
+                            <number>1</number>
+                           </property>
+                           <property name="maximum">
+                            <number>64</number>
+                           </property>
+                           <property name="value">
+                            <number>2</number>
+                           </property>
+                          </widget>
+                         </item>
+                         <item row="10" column="2">
+                          <widget class="QLabel" name="label_355">
+                           <property name="text">
+                            <string>Floor filtering height (0=Disabled). This is done in /map frame.</string>
+                           </property>
+                           <property name="wordWrap">
+                            <bool>true</bool>
+                           </property>
+                           <property name="textInteractionFlags">
+                            <set>Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse</set>
+                           </property>
+                          </widget>
+                         </item>
+                         <item row="8" column="2">
+                          <widget class="QLabel" name="label_169">
+                           <property name="text">
+                            <string>Noise filtering min neighbors.</string>
+                           </property>
+                           <property name="wordWrap">
+                            <bool>true</bool>
+                           </property>
+                           <property name="textInteractionFlags">
+                            <set>Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse</set>
+                           </property>
+                          </widget>
+                         </item>
+                         <item row="12" column="0">
+                          <widget class="QDoubleSpinBox" name="doubleSpinBox_normalRadiusSearch">
+                           <property name="singleStep">
+                            <double>0.010000000000000</double>
+                           </property>
+                          </widget>
+                         </item>
+                         <item row="13" column="2">
+                          <widget class="QLabel" name="label_459">
+                           <property name="text">
+                            <string>Default color scheme key.</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="QSpinBox" name="spinBox_colorScheme_odom"/>
+                         </item>
+                         <item row="13" column="0">
+                          <widget class="QSpinBox" name="spinBox_colorScheme"/>
+                         </item>
                          <item row="3" column="1">
                           <widget class="QDoubleSpinBox" name="doubleSpinBox_maxDepth_odom">
                            <property name="suffix">
@@ -525,9 +593,6 @@ Show a yellow background when the number of odometry inliers goes under this thr
                          <item row="5" column="1">
                           <widget class="QLineEdit" name="lineEdit_roiRatios_odom"/>
                          </item>
-                         <item row="5" column="0">
-                          <widget class="QLineEdit" name="lineEdit_roiRatios"/>
-                         </item>
                          <item row="0" column="0">
                           <widget class="QLabel" name="label_154">
                            <property name="text">
@@ -613,7 +678,7 @@ Show a yellow background when the number of odometry inliers goes under this thr
                            </property>
                           </widget>
                          </item>
-                         <item row="13" column="0">
+                         <item row="14" column="0">
                           <widget class="QDoubleSpinBox" name="doubleSpinBox_opacity">
                            <property name="suffix">
                             <string/>
@@ -632,26 +697,7 @@ Show a yellow background when the number of odometry inliers goes under this thr
                            </property>
                           </widget>
                          </item>
-                         <item row="13" column="1">
+                         <item row="14" column="2">
-                          <widget class="QDoubleSpinBox" name="doubleSpinBox_opacity_odom">
-                           <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="13" column="2">
                           <widget class="QLabel" name="label_155">
                            <property name="text">
                             <string>Opacity.</string>
@@ -664,7 +710,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_157">
                            <property name="text">
                             <string>Point size (1..64).</string>
@@ -849,20 +895,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="1">
-                          <widget class="QSpinBox" name="spinBox_ptsize">
-                           <property name="minimum">
-                            <number>1</number>
-                           </property>
-                           <property name="maximum">
-                            <number>64</number>
-                           </property>
-                           <property name="value">
-                            <number>2</number>
-                           </property>
-                          </widget>
-                         </item>
-                         <item row="14" column="1">
                           <widget class="QSpinBox" name="spinBox_ptsize_odom">
                            <property name="minimum">
                             <number>1</number>
@@ -875,32 +908,6 @@ Show a yellow background when the number of odometry inliers goes under this thr
                            </property>
                           </widget>
                          </item>
-                         <item row="10" column="2">
-                          <widget class="QLabel" name="label_355">
-                           <property name="text">
-                            <string>Floor filtering height (0=Disabled). This is done in /map frame.</string>
-                           </property>
-                           <property name="wordWrap">
-                            <bool>true</bool>
-                           </property>
-                           <property name="textInteractionFlags">
-                            <set>Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse</set>
-                           </property>
-                          </widget>
-                         </item>
-                         <item row="8" column="2">
-                          <widget class="QLabel" name="label_169">
-                           <property name="text">
-                            <string>Noise filtering min neighbors.</string>
-                           </property>
-                           <property name="wordWrap">
-                            <bool>true</bool>
-                           </property>
-                           <property name="textInteractionFlags">
-                            <set>Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse</set>
-                           </property>
-                          </widget>
-                         </item>
                          <item row="7" column="2">
                           <widget class="QLabel" name="label_168">
                            <property name="text">
@@ -1014,10 +1021,22 @@ Show a yellow background when the number of odometry inliers goes under this thr
                            </property>
                           </widget>
                          </item>
-                         <item row="12" column="0">
+                         <item row="14" column="1">
-                          <widget class="QDoubleSpinBox" name="doubleSpinBox_normalRadiusSearch">
+                          <widget class="QDoubleSpinBox" name="doubleSpinBox_opacity_odom">
+                           <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.010000000000000</double>
+                            <double>0.100000000000000</double>
+                           </property>
+                           <property name="value">
+                            <double>1.000000000000000</double>
                            </property>
                           </widget>
                          </item>
@@ -1128,7 +1147,39 @@ Show a yellow background when the number of odometry inliers goes under this thr
                        <string>Laser Scan</string>
                       </property>
                       <layout class="QGridLayout" name="gridLayout_79" columnstretch="0,0,1">
-                       <item row="9" column="1">
+                       <item row="11" column="1">
+                        <widget class="QDoubleSpinBox" name="doubleSpinBox_opacity_odom_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="2" column="2">
+                        <widget class="QLabel" name="label_273">
+                         <property name="text">
+                          <string>Scan downsampling step size.</string>
+                         </property>
+                         <property name="wordWrap">
+                          <bool>true</bool>
+                         </property>
+                         <property name="textInteractionFlags">
+                          <set>Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse</set>
+                         </property>
+                        </widget>
+                       </item>
+                       <item row="12" column="1">
                         <widget class="QSpinBox" name="spinBox_ptsize_odom_scan">
                          <property name="minimum">
                           <number>1</number>
@@ -1138,7 +1189,143 @@ 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_110">
+                         <property name="text">
+                          <string>Show scans.</string>
+                         </property>
+                         <property name="wordWrap">
+                          <bool>true</bool>
+                         </property>
+                         <property name="textInteractionFlags">
+                          <set>Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse</set>
+                         </property>
+                        </widget>
+                       </item>
+                       <item row="3" column="2">
+                        <widget class="QLabel" name="label_271">
+                         <property name="text">
+                          <string>Scan voxel size.</string>
+                         </property>
+                         <property name="wordWrap">
+                          <bool>true</bool>
+                         </property>
+                         <property name="textInteractionFlags">
+                          <set>Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse</set>
+                         </property>
+                        </widget>
+                       </item>
+                       <item row="0" column="1">
+                        <widget class="QLabel" name="label_356">
+                         <property name="text">
+                          <string>Odometry</string>
+                         </property>
+                         <property name="wordWrap">
+                          <bool>true</bool>
+                         </property>
+                         <property name="textInteractionFlags">
+                          <set>Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse</set>
+                         </property>
+                        </widget>
+                       </item>
+                       <item row="0" column="0">
+                        <widget class="QLabel" name="label_359">
+                         <property name="text">
+                          <string>Map</string>
+                         </property>
+                         <property name="wordWrap">
+                          <bool>true</bool>
+                         </property>
+                         <property name="textInteractionFlags">
+                          <set>Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse</set>
+                         </property>
+                        </widget>
+                       </item>
+                       <item row="6" column="0">
+                        <widget class="QDoubleSpinBox" name="doubleSpinBox_ceilingFilterHeight_scan">
+                         <property name="suffix">
+                          <string> m</string>
+                         </property>
+                         <property name="decimals">
+                          <number>3</number>
+                         </property>
+                         <property name="minimum">
+                          <double>-10.000000000000000</double>
+                         </property>
+                         <property name="maximum">
+                          <double>10.000000000000000</double>
+                         </property>
+                         <property name="singleStep">
+                          <double>0.100000000000000</double>
+                         </property>
+                         <property name="value">
+                          <double>0.000000000000000</double>
+                         </property>
+                        </widget>
+                       </item>
+                       <item row="8" column="2">
+                        <widget class="QLabel" name="label_241">
+                         <property name="text">
+                          <string>Normal K search. If not 0, normals will be computed and added to created cloud for visualization (keys  7, 8 and 9).</string>
+                         </property>
+                         <property name="wordWrap">
+                          <bool>true</bool>
+                         </property>
+                         <property name="textInteractionFlags">
+                          <set>Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse</set>
+                         </property>
+                        </widget>
+                       </item>
+                       <item row="10" column="2">
+                        <widget class="QLabel" name="label_458">
+                         <property name="text">
+                          <string>Default color scheme key.</string>
+                         </property>
+                         <property name="wordWrap">
+                          <bool>true</bool>
+                         </property>
+                         <property name="textInteractionFlags">
+                          <set>Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse</set>
+                         </property>
+                        </widget>
+                       </item>
+                       <item row="4" column="2">
+                        <widget class="QLabel" name="label_460">
+                         <property name="text">
+                          <string>Maximum range (0 means no limit).</string>
+                         </property>
+                         <property name="wordWrap">
+                          <bool>true</bool>
+                         </property>
+                         <property name="textInteractionFlags">
+                          <set>Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse</set>
+                         </property>
+                        </widget>
+                       </item>
+                       <item row="4" column="0">
+                        <widget class="QDoubleSpinBox" name="doubleSpinBox_maxRange">
+                         <property name="suffix">
+                          <string> m</string>
+                         </property>
+                         <property name="maximum">
+                          <double>999.000000000000000</double>
+                         </property>
+                        </widget>
+                       </item>
                        <item row="9" column="2">
+                        <widget class="QLabel" name="label_428">
+                         <property name="text">
+                          <string>Normal radius search. If not 0, normals will be computed and added to created cloud for visualization (keys  7, 8 and 9).</string>
+                         </property>
+                         <property name="wordWrap">
+                          <bool>true</bool>
+                         </property>
+                         <property name="textInteractionFlags">
+                          <set>Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse</set>
+                         </property>
+                        </widget>
+                       </item>
+                       <item row="12" column="2">
                         <widget class="QLabel" name="label_158">
                          <property name="text">
                           <string>Scan point size (1..64).</string>
@@ -1180,7 +1367,7 @@ Show a yellow background when the number of odometry inliers goes under this thr
                          </property>
                         </widget>
                        </item>
-                       <item row="8" column="2">
+                       <item row="11" column="2">
                         <widget class="QLabel" name="label_156">
                          <property name="text">
                           <string>Scan opacity.</string>
@@ -1193,20 +1380,7 @@ Show a yellow background when the number of odometry inliers goes under this thr
                          </property>
                         </widget>
                        </item>
-                       <item row="1" column="2">
+                       <item row="12" column="0">
-                        <widget class="QLabel" name="label_110">
-                         <property name="text">
-                          <string>Show scans.</string>
-                         </property>
-                         <property name="wordWrap">
-                          <bool>true</bool>
-                         </property>
-                         <property name="textInteractionFlags">
-                          <set>Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse</set>
-                         </property>
-                        </widget>
-                       </item>
-                       <item row="9" column="0">
                         <widget class="QSpinBox" name="spinBox_ptsize_scan">
                          <property name="minimum">
                           <number>1</number>
@@ -1246,7 +1420,7 @@ Show a yellow background when the number of odometry inliers goes under this thr
                          </property>
                         </widget>
                        </item>
-                       <item row="8" column="0">
+                       <item row="11" column="0">
                         <widget class="QDoubleSpinBox" name="doubleSpinBox_opacity_scan">
                          <property name="suffix">
                           <string/>
@@ -1265,51 +1439,6 @@ Show a yellow background when the number of odometry inliers goes under this thr
                          </property>
                         </widget>
                        </item>
-                       <item row="3" column="2">
-                        <widget class="QLabel" name="label_271">
-                         <property name="text">
-                          <string>Scan voxel size.</string>
-                         </property>
-                         <property name="wordWrap">
-                          <bool>true</bool>
-                         </property>
-                         <property name="textInteractionFlags">
-                          <set>Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse</set>
-                         </property>
-                        </widget>
-                       </item>
-                       <item row="8" column="1">
-                        <widget class="QDoubleSpinBox" name="doubleSpinBox_opacity_odom_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="2" column="2">
-                        <widget class="QLabel" name="label_273">
-                         <property name="text">
-                          <string>Scan downsampling step size.</string>
-                         </property>
-                         <property name="wordWrap">
-                          <bool>true</bool>
-                         </property>
-                         <property name="textInteractionFlags">
-                          <set>Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse</set>
-                         </property>
-                        </widget>
-                       </item>
                        <item row="3" column="0">
                         <widget class="QDoubleSpinBox" name="doubleSpinBox_voxelSizeScan">
                          <property name="suffix">
@@ -1329,33 +1458,7 @@ Show a yellow background when the number of odometry inliers goes under this thr
                          </property>
                         </widget>
                        </item>
-                       <item row="0" column="1">
+                       <item row="6" column="2">
-                        <widget class="QLabel" name="label_356">
-                         <property name="text">
-                          <string>Odometry</string>
-                         </property>
-                         <property name="wordWrap">
-                          <bool>true</bool>
-                         </property>
-                         <property name="textInteractionFlags">
-                          <set>Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse</set>
-                         </property>
-                        </widget>
-                       </item>
-                       <item row="0" column="0">
-                        <widget class="QLabel" name="label_359">
-                         <property name="text">
-                          <string>Map</string>
-                         </property>
-                         <property name="wordWrap">
-                          <bool>true</bool>
-                         </property>
-                         <property name="textInteractionFlags">
-                          <set>Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse</set>
-                         </property>
-                        </widget>
-                       </item>
-                       <item row="4" column="2">
                         <widget class="QLabel" name="label_367">
                          <property name="text">
                           <string>Ceiling filtering height (0=Disabled). This is done in /map frame.</string>
@@ -1368,7 +1471,7 @@ Show a yellow background when the number of odometry inliers goes under this thr
                          </property>
                         </widget>
                        </item>
-                       <item row="5" column="2">
+                       <item row="7" column="2">
                         <widget class="QLabel" name="label_368">
                          <property name="text">
                           <string>Floor filtering height (0=Disabled). This is done in /map frame.</string>
@@ -1381,29 +1484,7 @@ Show a yellow background when the number of odometry inliers goes under this thr
                          </property>
                         </widget>
                        </item>
-                       <item row="4" column="0">
+                       <item row="7" column="0">
-                        <widget class="QDoubleSpinBox" name="doubleSpinBox_ceilingFilterHeight_scan">
-                         <property name="suffix">
-                          <string> m</string>
-                         </property>
-                         <property name="decimals">
-                          <number>3</number>
-                         </property>
-                         <property name="minimum">
-                          <double>-10.000000000000000</double>
-                         </property>
-                         <property name="maximum">
-                          <double>10.000000000000000</double>
-                         </property>
-                         <property name="singleStep">
-                          <double>0.100000000000000</double>
-                         </property>
-                         <property name="value">
-                          <double>0.000000000000000</double>
-                         </property>
-                        </widget>
-                       </item>
-                       <item row="5" column="0">
                         <widget class="QDoubleSpinBox" name="doubleSpinBox_floorFilterHeight_scan">
                          <property name="suffix">
                           <string> m</string>
@@ -1425,20 +1506,7 @@ Show a yellow background when the number of odometry inliers goes under this thr
                          </property>
                         </widget>
                        </item>
-                       <item row="6" column="2">
+                       <item row="8" column="0">
-                        <widget class="QLabel" name="label_241">
-                         <property name="text">
-                          <string>Normal K search. If not 0, normals will be computed and added to created cloud for visualization (keys  7, 8 and 9).</string>
-                         </property>
-                         <property name="wordWrap">
-                          <bool>true</bool>
-                         </property>
-                         <property name="textInteractionFlags">
-                          <set>Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse</set>
-                         </property>
-                        </widget>
-                       </item>
-                       <item row="6" column="0">
                         <widget class="QSpinBox" name="spinBox_normalKSearch_scan">
                          <property name="minimum">
                           <number>0</number>
@@ -1451,10 +1519,23 @@ Show a yellow background when the number of odometry inliers goes under this thr
                          </property>
                         </widget>
                        </item>
-                       <item row="7" column="2">
+                       <item row="9" column="0">
-                        <widget class="QLabel" name="label_428">
+                        <widget class="QDoubleSpinBox" name="doubleSpinBox_normalRadiusSearch_scan">
+                         <property name="singleStep">
+                          <double>0.010000000000000</double>
+                         </property>
+                        </widget>
+                       </item>
+                       <item row="10" column="0">
+                        <widget class="QSpinBox" name="spinBox_colorSchemeScan"/>
+                       </item>
+                       <item row="10" column="1">
+                        <widget class="QSpinBox" name="spinBox_colorSchemeScan_odom"/>
+                       </item>
+                       <item row="5" column="2">
+                        <widget class="QLabel" name="label_461">
                          <property name="text">
-                          <string>Normal radius search. If not 0, normals will be computed and added to created cloud for visualization (keys  7, 8 and 9).</string>
+                          <string>Minimum range.</string>
                          </property>
                          <property name="wordWrap">
                           <bool>true</bool>
@@ -1464,10 +1545,33 @@ Show a yellow background when the number of odometry inliers goes under this thr
                          </property>
                         </widget>
                        </item>
-                       <item row="7" column="0">
+                       <item row="5" column="0">
-                        <widget class="QDoubleSpinBox" name="doubleSpinBox_normalRadiusSearch_scan">
+                        <widget class="QDoubleSpinBox" name="doubleSpinBox_minRange">
-                         <property name="singleStep">
+                         <property name="suffix">
-                          <double>0.010000000000000</double>
+                          <string> m</string>
+                         </property>
+                         <property name="maximum">
+                          <double>999.000000000000000</double>
+                         </property>
+                        </widget>
+                       </item>
+                       <item row="4" column="1">
+                        <widget class="QDoubleSpinBox" name="doubleSpinBox_maxRange_odom">
+                         <property name="suffix">
+                          <string> m</string>
+                         </property>
+                         <property name="maximum">
+                          <double>999.000000000000000</double>
+                         </property>
+                        </widget>
+                       </item>
+                       <item row="5" column="1">
+                        <widget class="QDoubleSpinBox" name="doubleSpinBox_minRange_odom">
+                         <property name="suffix">
+                          <string> m</string>
+                         </property>
+                         <property name="maximum">
+                          <double>999.000000000000000</double>
                          </property>
                         </widget>
                        </item>