Added parameter "Grid/ProjRayTracing" and renamed "Grid/3DGroundIsObstacle" to "Grid/GroundIsObstacle"

corelib/include/rtabmap/core/OccupancyGrid.h

@@ -103,6 +103,7 @@ private:
 	int noiseFilteringMinNeighbors_;
 	bool scan2dUnknownSpaceFilled_;
 	double scan2dMaxUnknownSpaceFilledRange_;
+	bool projRayTracing_;
 
 	std::map<int, std::pair<cv::Mat, cv::Mat> > cache_;
 	cv::Mat map_;

corelib/include/rtabmap/core/Parameters.h

@@ -492,15 +492,16 @@ class RTABMAP_EXP Parameters
 	RTABMAP_PARAM(Grid, MinClusterSize,          int,    10,      uFormat("[%s=true] Minimum cluster size to project the points.", kGridNormalsSegmentation().c_str()));
 	RTABMAP_PARAM(Grid, FlatObstacleDetected,    bool,   true,    uFormat("[%s=true] Flat obstacles detected.", kGridNormalsSegmentation().c_str()));
 #ifdef RTABMAP_OCTOMAP
-	RTABMAP_PARAM(Grid, 3D,                      bool,   true,    uFormat("A 3D occupancy grid is required if you want an Octomap. Set to false if you want only a 2D map, the cloud will be projected on xy plane. A 2D map can be still generated if checked, but it requires more memory and time to generate it. Ignored if laser scan is 2D and \"%s\" is false.", kGridFromDepth().c_str()));
+	RTABMAP_PARAM(Grid, 3D,                      bool,   true,    uFormat("A 3D occupancy grid is required if you want an OctoMap (3D ray tracing). Set to false if you want only a 2D map, the cloud will be projected on xy plane. A 2D map can be still generated if checked, but it requires more memory and time to generate it. Ignored if laser scan is 2D and \"%s\" is false.", kGridFromDepth().c_str()));
 #else
-	RTABMAP_PARAM(Grid, 3D,                      bool,   false,   uFormat("A 3D occupancy grid is required if you want an Octomap. Set to false if you want only a 2D map, the cloud will be projected on xy plane. A 2D map can be still generated if checked, but it requires more memory and time to generate it. Ignored if laser scan is 2D and \"%s\" is false.", kGridFromDepth().c_str()));
+	RTABMAP_PARAM(Grid, 3D,                      bool,   false,   uFormat("A 3D occupancy grid is required if you want an OctoMap (3D ray tracing). Set to false if you want only a 2D map, the cloud will be projected on xy plane. A 2D map can be still generated if checked, but it requires more memory and time to generate it. Ignored if laser scan is 2D and \"%s\" is false.", kGridFromDepth().c_str()));
 #endif
-	RTABMAP_PARAM(Grid, 3DGroundIsObstacle,      bool,   false,   uFormat("[%s=true] Ground is an obstacle. Use this only if you want an Octomap with ground identified as an obstacle (e.g., with an UAV).", kGrid3D().c_str()));
+	RTABMAP_PARAM(Grid, GroundIsObstacle,        bool,   false,   uFormat("[%s=true] Ground segmentation (%s) is ignored, all points are obstacles. Use this only if you want an OctoMap with ground identified as an obstacle (e.g., with an UAV).", kGrid3D().c_str(), kGridNormalsSegmentation().c_str()));
 	RTABMAP_PARAM(Grid, NoiseFilteringRadius,         float,   0.0,     "Noise filtering radius (0=disabled). Done after segmentation.");
 	RTABMAP_PARAM(Grid, NoiseFilteringMinNeighbors,   int,     5,       "Noise filtering minimum neighbors.");
 	RTABMAP_PARAM(Grid, Scan2dUnknownSpaceFilled,     bool,    false,   "Unknown space filled. Only used with 2D laser scans.");
 	RTABMAP_PARAM(Grid, Scan2dMaxFilledRange,         float,   4.0,     "Unknown space filled maximum range. If 0, the laser scan maximum range is used.");
+	RTABMAP_PARAM(Grid, ProjRayTracing,               bool,   false,    uFormat("[%s=false] 2D ray tracing is done for each projected obstacle, filling unknown space between the sensor and obstacles.", kGrid3D().c_str()));
 
 public:
 	virtual ~Parameters();

corelib/include/rtabmap/core/impl/OccupancyGrid.hpp

@@ -92,7 +92,7 @@ typename pcl::PointCloud<PointT>::Ptr OccupancyGrid::segmentCloud(
 
 	if(indices->size())
 	{
-		if(normalsSegmentation_)
+		if(normalsSegmentation_ && !groundIsObstacle_)
 		{
 			UDEBUG("normalKSearch=%d", normalKSearch_);
 			UDEBUG("maxGroundAngle=%f", maxGroundAngle_);

corelib/src/OccupancyGrid.cpp

@@ -59,11 +59,12 @@ OccupancyGrid::OccupancyGrid(const ParametersMap & parameters) :
 	maxGroundHeight_(Parameters::defaultGridMaxGroundHeight()),
 	normalsSegmentation_(Parameters::defaultGridNormalsSegmentation()),
 	grid3D_(Parameters::defaultGrid3D()),
-	groundIsObstacle_(Parameters::defaultGrid3DGroundIsObstacle()),
+	groundIsObstacle_(Parameters::defaultGridGroundIsObstacle()),
 	noiseFilteringRadius_(Parameters::defaultGridNoiseFilteringRadius()),
 	noiseFilteringMinNeighbors_(Parameters::defaultGridNoiseFilteringMinNeighbors()),
 	scan2dUnknownSpaceFilled_(Parameters::defaultGridScan2dUnknownSpaceFilled()),
 	scan2dMaxUnknownSpaceFilledRange_(Parameters::defaultGridScan2dMaxFilledRange()),
+	projRayTracing_(Parameters::defaultGridProjRayTracing()),
 	xMin_(0.0f),
 	yMin_(0.0f)
 {
@@ -124,11 +125,12 @@ void OccupancyGrid::parseParameters(const ParametersMap & parameters)
 	Parameters::parse(parameters, Parameters::kGridFlatObstacleDetected(), flatObstaclesDetected_);
 	Parameters::parse(parameters, Parameters::kGridNormalsSegmentation(), normalsSegmentation_);
 	Parameters::parse(parameters, Parameters::kGrid3D(), grid3D_);
-	Parameters::parse(parameters, Parameters::kGrid3DGroundIsObstacle(), groundIsObstacle_);
+	Parameters::parse(parameters, Parameters::kGridGroundIsObstacle(), groundIsObstacle_);
 	Parameters::parse(parameters, Parameters::kGridNoiseFilteringRadius(), noiseFilteringRadius_);
 	Parameters::parse(parameters, Parameters::kGridNoiseFilteringMinNeighbors(), noiseFilteringMinNeighbors_);
 	Parameters::parse(parameters, Parameters::kGridScan2dUnknownSpaceFilled(), scan2dUnknownSpaceFilled_);
 	Parameters::parse(parameters, Parameters::kGridScan2dMaxFilledRange(), scan2dMaxUnknownSpaceFilledRange_);
+	Parameters::parse(parameters, Parameters::kGridProjRayTracing(), projRayTracing_);
 
 	// convert ROI from string to vector
 	ParametersMap::const_iterator iter;
@@ -322,6 +324,20 @@ void OccupancyGrid::createLocalMap(
 							ground,
 							obstacles,
 							cellSize_);
+
+					if(projRayTracing_)
+					{
+						cv::Mat laserScan = obstacles;
+						ground = cv::Mat();
+						obstacles = cv::Mat();
+						util3d::occupancy2DFromLaserScan(
+								laserScan,
+								ground,
+								obstacles,
+								cellSize_,
+								false, // don't fill unknown space
+								0);
+					}
 				}
 			}
 		}

corelib/src/Parameters.cpp

@@ -224,6 +224,9 @@ const std::map<std::string, std::pair<bool, std::string> > & Parameters::getRemo
 	{
 		// removed parameters
 
+		// 0.12.1
+		removedParameters_.insert(std::make_pair("Grid/3DGroundIsObstacle",       std::make_pair(true,  Parameters::kGridGroundIsObstacle())));
+
 		// 0.11.12
 		removedParameters_.insert(std::make_pair("Optimizer/Slam2D",              std::make_pair(true,  Parameters::kRegForce3DoF())));
 		removedParameters_.insert(std::make_pair("OdomF2M/FixedMapPath",          std::make_pair(false,  "")));

guilib/src/PreferencesDialog.cpp

@@ -823,12 +823,13 @@ PreferencesDialog::PreferencesDialog(QWidget * parent) :
 
 	// Occupancy grid
 	_ui->groupBox_grid_3d->setObjectName(Parameters::kGrid3D().c_str());
-	_ui->checkBox_grid_groundObstacle->setObjectName(Parameters::kGrid3DGroundIsObstacle().c_str());
+	_ui->checkBox_grid_groundObstacle->setObjectName(Parameters::kGridGroundIsObstacle().c_str());
 	_ui->doubleSpinBox_grid_resolution->setObjectName(Parameters::kGridCellSize().c_str());
 	_ui->spinBox_grid_decimation->setObjectName(Parameters::kGridDepthDecimation().c_str());
 	_ui->doubleSpinBox_grid_maxDepth->setObjectName(Parameters::kGridDepthMax().c_str());
 	_ui->doubleSpinBox_grid_minDepth->setObjectName(Parameters::kGridDepthMin().c_str());
 	_ui->lineEdit_grid_roi->setObjectName(Parameters::kGridDepthRoiRatios().c_str());
+	_ui->checkBox_grid_projRayTracing->setObjectName(Parameters::kGridProjRayTracing().c_str());
 	_ui->doubleSpinBox_grid_footprintLength->setObjectName(Parameters::kGridFootprintLength().c_str());
 	_ui->doubleSpinBox_grid_footprintWidth->setObjectName(Parameters::kGridFootprintWidth().c_str());
 	_ui->doubleSpinBox_grid_footprintHeight->setObjectName(Parameters::kGridFootprintHeight().c_str());

guilib/src/ui/preferencesDialog.ui

@@ -63,7 +63,7 @@
           <property name="geometry">
            <rect>
             <x>0</x>
-            <y>0</y>
+            <y>-491</y>
             <width>673</width>
             <height>2649</height>
            </rect>
@@ -86,7 +86,7 @@
                 <enum>QFrame::Raised</enum>
                </property>
                <property name="currentIndex">
-                <number>1</number>
+                <number>15</number>
                </property>
                <widget class="QWidget" name="page_22">
                 <layout class="QVBoxLayout" name="verticalLayout_29" stretch="0,1">
@@ -8857,6 +8857,29 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag
                            </property>
                           </widget>
                          </item>
+                         <item row="4" column="1">
+                          <widget class="QLabel" name="label_363">
+                           <property name="text">
+                            <string>If 3D is not checked below, 2D ray tracing is done for each projected obstacle, filling unknown space between the sensor and obstacles.</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="QCheckBox" name="checkBox_grid_projRayTracing">
+                           <property name="text">
+                            <string/>
+                           </property>
+                           <property name="checked">
+                            <bool>false</bool>
+                           </property>
+                          </widget>
+                         </item>
                         </layout>
                        </item>
                       </layout>
@@ -9060,7 +9083,7 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag
                          <item row="0" column="1">
                           <widget class="QLabel" name="label_octomap_groundObstacle">
                            <property name="text">
-                            <string>Ground is an obstacle. Use this only if you want an Octomap with ground identified as an obstacle (e.g., with an UAV).</string>
+                            <string>Ground segmentation (Normals Segmentation Approach) is ignored, all points are obstacles. Use this only if you want an Octomap with ground identified as an obstacle (e.g., with an UAV).</string>
                            </property>
                            <property name="wordWrap">
                             <bool>true</bool>