3D Rendering: Added floor/ceiling filtering and normal estimation for scans

corelib/include/rtabmap/core/util3d_filtering.h

@@ -135,6 +135,12 @@ pcl::PointCloud<pcl::PointXYZRGB>::Ptr RTABMAP_EXP passThrough(
 		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,
+		float min,
+		float max,
+		bool negative = false);
 
 pcl::IndicesPtr RTABMAP_EXP cropBox(
 		const pcl::PointCloud<pcl::PointXYZ>::Ptr & cloud,

corelib/src/util3d_filtering.cpp

@@ -341,6 +341,26 @@ pcl::PointCloud<pcl::PointXYZRGB>::Ptr passThrough(
 	return output;
 }
 
+pcl::PointCloud<pcl::PointNormal>::Ptr passThrough(
+		const pcl::PointCloud<pcl::PointNormal>::Ptr & cloud,
+		const std::string & axis,
+		float min,
+		float max,
+		bool negative)
+{
+	UASSERT(max > min);
+	UASSERT(axis.compare("x") == 0 || axis.compare("y") == 0 || axis.compare("z") == 0);
+
+	pcl::PointCloud<pcl::PointNormal>::Ptr output(new pcl::PointCloud<pcl::PointNormal>);
+	pcl::PassThrough<pcl::PointNormal> filter;
+	filter.setNegative(negative);
+	filter.setFilterFieldName(axis);
+	filter.setFilterLimits(min, max);
+	filter.setInputCloud(cloud);
+	filter.filter(*output);
+	return output;
+}
+
 pcl::IndicesPtr cropBox(
 		const pcl::PointCloud<pcl::PointXYZ>::Ptr & cloud,
 		const pcl::IndicesPtr & indices,

guilib/include/rtabmap/gui/PreferencesDialog.h

@@ -163,6 +163,9 @@ public:
 	double getCeilingFilteringHeight() const;
 	double getFloorFilteringHeight() const;
 	int getNormalKSearch() const;
+	double getScanCeilingFilteringHeight() const;
+	double getScanFloorFilteringHeight() const;
+	int getScanNormalKSearch() const;
 	bool isCloudsShown(int index) const;      // 0=map, 1=odom
 	bool isOctomapUpdated() const;
 	bool isOctomapShown() const;

guilib/src/MainWindow.cpp

@@ -2863,6 +2863,24 @@ void MainWindow::createAndAddScanToMap(int nodeId, const Transform & pose, int m
 			{
 				cloud = util3d::voxelize(cloud, _preferencesDialog->getCloudVoxelSizeScan(0));
 			}
+
+			// Do ceiling/floor filtering
+			if(cloud->size() &&
+			   (_preferencesDialog->getScanFloorFilteringHeight() != 0.0 ||
+			   _preferencesDialog->getScanCeilingFilteringHeight() != 0.0))
+			{
+				// perform in /map frame
+				pcl::PointCloud<pcl::PointNormal>::Ptr cloudTransformed = util3d::transformPointCloud(cloud, 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
+				cloud = util3d::transformPointCloud(cloudTransformed, pose.inverse());
+			}
+
 			QColor color = Qt::gray;
 			if(mapId >= 0)
 			{
@@ -2895,37 +2913,92 @@ void MainWindow::createAndAddScanToMap(int nodeId, const Transform & pose, int m
 		{
 			pcl::PointCloud<pcl::PointXYZ>::Ptr cloud;
 			cloud = util3d::laserScanToPointCloud(scan, iter->sensorData().laserScanInfo().localTransform());
+			bool filtered = false;
 			if(_preferencesDialog->getCloudVoxelSizeScan(0) > 0.0)
 			{
 				cloud = util3d::voxelize(cloud, _preferencesDialog->getCloudVoxelSizeScan(0));
+				filtered = true;
 			}
+
+			// Do ceiling/floor filtering
+			if(scan.channels() > 2 && // don't filter 2D scans
+				cloud->size() &&
+			   (_preferencesDialog->getScanFloorFilteringHeight() != 0.0 ||
+			   _preferencesDialog->getScanCeilingFilteringHeight() != 0.0))
+			{
+				// perform in /map frame
+				pcl::PointCloud<pcl::PointXYZ>::Ptr cloudTransformed = util3d::transformPointCloud(cloud, 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
+				cloud = util3d::transformPointCloud(cloudTransformed, pose.inverse());
+				filtered = true;
+			}
+
+			pcl::PointCloud<pcl::PointNormal>::Ptr cloudWithNormals;
+			if(scan.channels() > 2 && // don't compute normals for 2D scans
+				cloud->size() &&
+			   _preferencesDialog->getScanNormalKSearch() > 0)
+			{
+				pcl::PointCloud<pcl::Normal>::Ptr normals = util3d::computeNormals(cloud, _preferencesDialog->getScanNormalKSearch());
+				cloudWithNormals.reset(new pcl::PointCloud<pcl::PointNormal>);
+				pcl::concatenateFields(*cloud, *normals, *cloudWithNormals);
+				filtered = true;
+			}
+
 			QColor color = Qt::gray;
 			if(mapId >= 0)
 			{
 				color = (Qt::GlobalColor)(mapId+3 % 12 + 7 );
 			}
-			if(!_cloudViewer->addCloud(scanName, cloud, pose, color))
+			if(cloudWithNormals.get())
 			{
-				UERROR("Adding cloud %d to viewer failed!", nodeId);
+				if(!_cloudViewer->addCloud(scanName, cloudWithNormals, pose, color))
+				{
+					UERROR("Adding cloud %d to viewer failed!", nodeId);
+				}
+				else
+				{
+					if(nodeId > 0)
+					{
+						//reconvert the voxelized cloud
+						scan = util3d::laserScanFromPointCloud(*cloudWithNormals);
+						_createdScans.insert(std::make_pair(nodeId, scan)); // keep scan in base_link frame
+					}
+
+					_cloudViewer->setCloudOpacity(scanName, _preferencesDialog->getScanOpacity(0));
+					_cloudViewer->setCloudPointSize(scanName, _preferencesDialog->getScanPointSize(0));
+				}
 			}
 			else
 			{
-				if(nodeId > 0)
+				if(!_cloudViewer->addCloud(scanName, cloud, pose, color))
 				{
-					if(_preferencesDialog->getCloudVoxelSizeScan(0) > 0.0)
+					UERROR("Adding cloud %d to viewer failed!", nodeId);
-					{
-						//reconvert the voxelized cloud
-						scan = util3d::laserScanFromPointCloud(*cloud);
-					}
-					else
-					{
-						scan = util3d::transformLaserScan(scan, iter->sensorData().laserScanInfo().localTransform());
-					}
-					_createdScans.insert(std::make_pair(nodeId, scan)); // keep scan in base_link frame
 				}
+				else
+				{
+					if(nodeId > 0)
+					{
+						if(filtered)
+						{
+							//reconvert the voxelized cloud
+							scan = util3d::laserScanFromPointCloud(*cloud);
+						}
+						else
+						{
+							scan = util3d::transformLaserScan(scan, iter->sensorData().laserScanInfo().localTransform());
+						}
+						_createdScans.insert(std::make_pair(nodeId, scan)); // keep scan in base_link frame
+					}
 
-				_cloudViewer->setCloudOpacity(scanName, _preferencesDialog->getScanOpacity(0));
+					_cloudViewer->setCloudOpacity(scanName, _preferencesDialog->getScanOpacity(0));
-				_cloudViewer->setCloudPointSize(scanName, _preferencesDialog->getScanPointSize(0));
+					_cloudViewer->setCloudPointSize(scanName, _preferencesDialog->getScanPointSize(0));
+				}
 			}
 		}
 	}

guilib/src/PreferencesDialog.cpp

@@ -393,6 +393,9 @@ PreferencesDialog::PreferencesDialog(QWidget * parent) :
 	connect(_ui->doubleSpinBox_ceilingFilterHeight, SIGNAL(valueChanged(double)), this, SLOT(makeObsoleteCloudRenderingPanel()));
 	connect(_ui->doubleSpinBox_floorFilterHeight, SIGNAL(valueChanged(double)), this, SLOT(makeObsoleteCloudRenderingPanel()));
 	connect(_ui->spinBox_normalKSearch, SIGNAL(valueChanged(int)), this, SLOT(makeObsoleteCloudRenderingPanel()));
+	connect(_ui->doubleSpinBox_ceilingFilterHeight_scan, SIGNAL(valueChanged(double)), this, SLOT(makeObsoleteCloudRenderingPanel()));
+	connect(_ui->doubleSpinBox_floorFilterHeight_scan, SIGNAL(valueChanged(double)), this, SLOT(makeObsoleteCloudRenderingPanel()));
+	connect(_ui->spinBox_normalKSearch_scan, SIGNAL(valueChanged(int)), this, SLOT(makeObsoleteCloudRenderingPanel()));
 
 	connect(_ui->checkBox_showGraphs, SIGNAL(stateChanged(int)), this, SLOT(makeObsoleteCloudRenderingPanel()));
 	connect(_ui->checkBox_showFrustums, SIGNAL(stateChanged(int)), this, SLOT(makeObsoleteCloudRenderingPanel()));
@@ -1282,13 +1285,16 @@ void PreferencesDialog::resetSettings(QGroupBox * groupBox)
 
 		_ui->doubleSpinBox_ceilingFilterHeight->setValue(0);
 		_ui->doubleSpinBox_floorFilterHeight->setValue(0);
+		_ui->spinBox_normalKSearch->setValue(10);
+
+		_ui->doubleSpinBox_ceilingFilterHeight_scan->setValue(0);
+		_ui->doubleSpinBox_floorFilterHeight_scan->setValue(0);
+		_ui->spinBox_normalKSearch_scan->setValue(0);
 
 		_ui->checkBox_showGraphs->setChecked(true);
 		_ui->checkBox_showFrustums->setChecked(false);
 		_ui->checkBox_showLabels->setChecked(false);
 
-		_ui->spinBox_normalKSearch->setValue(10);
-
 		_ui->doubleSpinBox_mesh_angleTolerance->setValue(15.0);
 		_ui->groupBox_organized->setChecked(false);
 #if PCL_VERSION_COMPARE(>=, 1, 7, 2)
@@ -1674,6 +1680,9 @@ void PreferencesDialog::readGuiSettings(const QString & filePath)
 	_ui->doubleSpinBox_ceilingFilterHeight->setValue(settings.value("cloudCeilingHeight", _ui->doubleSpinBox_ceilingFilterHeight->value()).toDouble());
 	_ui->doubleSpinBox_floorFilterHeight->setValue(settings.value("cloudFloorHeight", _ui->doubleSpinBox_floorFilterHeight->value()).toDouble());
 	_ui->spinBox_normalKSearch->setValue(settings.value("normalKSearch", _ui->spinBox_normalKSearch->value()).toInt());
+	_ui->doubleSpinBox_ceilingFilterHeight_scan->setValue(settings.value("scanCeilingHeight", _ui->doubleSpinBox_ceilingFilterHeight_scan->value()).toDouble());
+	_ui->doubleSpinBox_floorFilterHeight_scan->setValue(settings.value("scanFloorHeight", _ui->doubleSpinBox_floorFilterHeight_scan->value()).toDouble());
+	_ui->spinBox_normalKSearch_scan->setValue(settings.value("scanNormalKSearch", _ui->spinBox_normalKSearch_scan->value()).toInt());
 
 	_ui->checkBox_showGraphs->setChecked(settings.value("showGraphs", _ui->checkBox_showGraphs->isChecked()).toBool());
 	_ui->checkBox_showFrustums->setChecked(settings.value("showFrustums", _ui->checkBox_showFrustums->isChecked()).toBool());
@@ -2056,7 +2065,10 @@ void PreferencesDialog::writeGuiSettings(const QString & filePath) const
 	settings.setValue("cloudNoiseMinNeighbors", _ui->spinBox_noiseMinNeighbors->value());
 	settings.setValue("cloudCeilingHeight",     _ui->doubleSpinBox_ceilingFilterHeight->value());
 	settings.setValue("cloudFloorHeight",       _ui->doubleSpinBox_floorFilterHeight->value());
-	settings.setValue("normalKSearch",           _ui->spinBox_normalKSearch->value());
+	settings.setValue("normalKSearch",          _ui->spinBox_normalKSearch->value());
+	settings.setValue("scanCeilingHeight",      _ui->doubleSpinBox_ceilingFilterHeight_scan->value());
+	settings.setValue("scanFloorHeight",        _ui->doubleSpinBox_floorFilterHeight_scan->value());
+	settings.setValue("scanNormalKSearch",      _ui->spinBox_normalKSearch_scan->value());
 
 	settings.setValue("showGraphs", _ui->checkBox_showGraphs->isChecked());
 	settings.setValue("showFrustums", _ui->checkBox_showFrustums->isChecked());
@@ -4101,6 +4113,18 @@ int PreferencesDialog::getNormalKSearch() const
 {
 	return _ui->spinBox_normalKSearch->value();
 }
+double PreferencesDialog::getScanCeilingFilteringHeight() const
+{
+	return _ui->doubleSpinBox_ceilingFilterHeight_scan->value();
+}
+double PreferencesDialog::getScanFloorFilteringHeight() const
+{
+	return _ui->doubleSpinBox_floorFilterHeight_scan->value();
+}
+int PreferencesDialog::getScanNormalKSearch() const
+{
+	return _ui->spinBox_normalKSearch_scan->value();
+}
 
 bool PreferencesDialog::isGraphsShown() const
 {

guilib/src/ui/preferencesDialog.ui

@@ -63,9 +63,9 @@
           <property name="geometry">
            <rect>
             <x>0</x>
-            <y>-373</y>
+            <y>0</y>
-            <width>673</width>
+            <width>678</width>
-            <height>2718</height>
+            <height>2701</height>
            </rect>
           </property>
           <layout class="QVBoxLayout" name="verticalLayout_16">
@@ -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">
@@ -1084,8 +1084,18 @@ Show a yellow background when the number of odometry inliers goes under this thr
                       <property name="title">
                        <string>Laser Scan</string>
                       </property>
-                      <layout class="QGridLayout" name="gridLayout_79">
+                      <layout class="QGridLayout" name="gridLayout_79" columnstretch="0,0,1">
-                       <item row="5" column="2">
+                       <item row="8" column="1">
+                        <widget class="QSpinBox" name="spinBox_ptsize_odom_scan">
+                         <property name="minimum">
+                          <number>1</number>
+                         </property>
+                         <property name="maximum">
+                          <number>64</number>
+                         </property>
+                        </widget>
+                       </item>
+                       <item row="8" column="2">
                         <widget class="QLabel" name="label_158">
                          <property name="text">
                           <string>Scan point size (1..64).</string>
@@ -1127,17 +1137,7 @@ Show a yellow background when the number of odometry inliers goes under this thr
                          </property>
                         </widget>
                        </item>
-                       <item row="5" column="1">
+                       <item row="7" column="2">
-                        <widget class="QSpinBox" name="spinBox_ptsize_odom_scan">
-                         <property name="minimum">
-                          <number>1</number>
-                         </property>
-                         <property name="maximum">
-                          <number>64</number>
-                         </property>
-                        </widget>
-                       </item>
-                       <item row="4" column="2">
                         <widget class="QLabel" name="label_156">
                          <property name="text">
                           <string>Scan opacity.</string>
@@ -1163,7 +1163,7 @@ Show a yellow background when the number of odometry inliers goes under this thr
                          </property>
                         </widget>
                        </item>
-                       <item row="5" column="0">
+                       <item row="8" column="0">
                         <widget class="QSpinBox" name="spinBox_ptsize_scan">
                          <property name="minimum">
                           <number>1</number>
@@ -1173,6 +1173,16 @@ Show a yellow background when the number of odometry inliers goes under this thr
                          </property>
                         </widget>
                        </item>
+                       <item row="2" column="1">
+                        <widget class="QSpinBox" name="spinBox_downsamplingScan_odom">
+                         <property name="minimum">
+                          <number>1</number>
+                         </property>
+                         <property name="maximum">
+                          <number>9999</number>
+                         </property>
+                        </widget>
+                       </item>
                        <item row="1" column="1">
                         <widget class="QCheckBox" name="checkBox_showOdomScans">
                          <property name="text">
@@ -1193,17 +1203,7 @@ Show a yellow background when the number of odometry inliers goes under this thr
                          </property>
                         </widget>
                        </item>
-                       <item row="2" column="1">
+                       <item row="7" column="0">
-                        <widget class="QSpinBox" name="spinBox_downsamplingScan_odom">
-                         <property name="minimum">
-                          <number>1</number>
-                         </property>
-                         <property name="maximum">
-                          <number>9999</number>
-                         </property>
-                        </widget>
-                       </item>
-                       <item row="4" column="0">
                         <widget class="QDoubleSpinBox" name="doubleSpinBox_opacity_scan">
                          <property name="suffix">
                           <string/>
@@ -1235,7 +1235,7 @@ Show a yellow background when the number of odometry inliers goes under this thr
                          </property>
                         </widget>
                        </item>
-                       <item row="4" column="1">
+                       <item row="7" column="1">
                         <widget class="QDoubleSpinBox" name="doubleSpinBox_opacity_odom_scan">
                          <property name="suffix">
                           <string/>
@@ -1312,6 +1312,102 @@ Show a yellow background when the number of odometry inliers goes under this thr
                          </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>
+                         </property>
+                         <property name="wordWrap">
+                          <bool>true</bool>
+                         </property>
+                         <property name="textInteractionFlags">
+                          <set>Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse</set>
+                         </property>
+                        </widget>
+                       </item>
+                       <item row="5" column="2">
+                        <widget class="QLabel" name="label_368">
+                         <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="4" 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>
+                         </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="6" 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="6" column="0">
+                        <widget class="QSpinBox" name="spinBox_normalKSearch_scan">
+                         <property name="minimum">
+                          <number>0</number>
+                         </property>
+                         <property name="maximum">
+                          <number>1000</number>
+                         </property>
+                         <property name="value">
+                          <number>10</number>
+                         </property>
+                        </widget>
+                       </item>
                       </layout>
                      </widget>
                     </item>