Added viewpoint to laser scan 2D ray tracing

corelib/include/rtabmap/core/util3d_mapping.h

@@ -43,8 +43,17 @@ namespace rtabmap
 namespace util3d
 {
 
+RTABMAP_DEPRECATED(void RTABMAP_EXP occupancy2DFromLaserScan(
+		const cv::Mat & scan, // in /base_link frame
+		cv::Mat & ground,
+		cv::Mat & obstacles,
+		float cellSize,
+		bool unknownSpaceFilled = false,
+		float scanMaxRange = 0.0f), "Use interface with \"viewpoint\" parameter to make sure the ray tracing origin is from the sensor and not the base.");
+
 void RTABMAP_EXP occupancy2DFromLaserScan(
-		const cv::Mat & scan,
+		const cv::Mat & scan, // in /base_link frame
+		const cv::Point3f & viewpoint, // /base_link -> /base_scan
 		cv::Mat & ground,
 		cv::Mat & obstacles,
 		float cellSize,
@@ -61,8 +70,18 @@ cv::Mat RTABMAP_EXP create2DMapFromOccupancyLocalMaps(
 		bool erode = false,
 		float footprintRadius = 0.0f);
 
+RTABMAP_DEPRECATED(cv::Mat RTABMAP_EXP create2DMap(const std::map<int, Transform> & poses,
+		const std::map<int, pcl::PointCloud<pcl::PointXYZ>::Ptr > & scans, // in /base_link frame
+		float cellSize,
+		bool unknownSpaceFilled,
+		float & xMin,
+		float & yMin,
+		float minMapSize = 0.0f,
+		float scanMaxRange = 0.0f), "Use interface with \"viewpoints\" parameter to make sure the ray tracing origin is from the sensor and not the base.");
+
 cv::Mat RTABMAP_EXP create2DMap(const std::map<int, Transform> & poses,
-		const std::map<int, pcl::PointCloud<pcl::PointXYZ>::Ptr > & scans,
+		const std::map<int, pcl::PointCloud<pcl::PointXYZ>::Ptr > & scans, // in /base_link frame
+		const std::map<int, cv::Point3f > & viewpoints, // /base_link -> /base_scan
 		float cellSize,
 		bool unknownSpaceFilled,
 		float & xMin,

corelib/src/OccupancyGrid.cpp

@@ -201,8 +201,14 @@ void OccupancyGrid::createLocalMap(
 	{
 		UDEBUG("2D laser scan");
 		//2D
+		viewPoint = cv::Point3f(
+				node.sensorData().laserScanInfo().localTransform().x(),
+				node.sensorData().laserScanInfo().localTransform().y(),
+				node.sensorData().laserScanInfo().localTransform().z());
+
 		util3d::occupancy2DFromLaserScan(
 				util3d::transformLaserScan(node.sensorData().laserScanRaw(), node.sensorData().laserScanInfo().localTransform()),
+				viewPoint,
 				ground,
 				obstacles,
 				cellSize_,
@@ -331,6 +337,7 @@ void OccupancyGrid::createLocalMap(
 						obstacles = cv::Mat();
 						util3d::occupancy2DFromLaserScan(
 								laserScan,
+								viewPoint,
 								ground,
 								obstacles,
 								cellSize_,

corelib/src/util3d_mapping.cpp

@@ -45,6 +45,7 @@ namespace rtabmap
 
 namespace util3d
 {
+
 void occupancy2DFromLaserScan(
 		const cv::Mat & scan,
 		cv::Mat & ground,
@@ -52,6 +53,26 @@ void occupancy2DFromLaserScan(
 		float cellSize,
 		bool unknownSpaceFilled,
 		float scanMaxRange)
+{
+	cv::Point3f viewpoint(0,0,0);
+	occupancy2DFromLaserScan(
+			scan,
+			viewpoint,
+			ground,
+			obstacles,
+			cellSize,
+			unknownSpaceFilled,
+			scanMaxRange);
+}
+
+void occupancy2DFromLaserScan(
+		const cv::Mat & scan,
+		const cv::Point3f & viewpoint,
+		cv::Mat & ground,
+		cv::Mat & obstacles,
+		float cellSize,
+		bool unknownSpaceFilled,
+		float scanMaxRange)
 {
 	if(scan.empty())
 	{
@@ -67,8 +88,11 @@ void occupancy2DFromLaserScan(
 	std::map<int, pcl::PointCloud<pcl::PointXYZ>::Ptr> scans;
 	scans.insert(std::make_pair(1, obstaclesCloud));
 
+	std::map<int, cv::Point3f> viewpoints;
+	viewpoints.insert(std::make_pair(1, viewpoint));
+
 	float xMin, yMin;
-	cv::Mat map8S = create2DMap(poses, scans, cellSize, unknownSpaceFilled, xMin, yMin, 0.0f, scanMaxRange);
+	cv::Mat map8S = create2DMap(poses, scans, viewpoints, cellSize, unknownSpaceFilled, xMin, yMin, 0.0f, scanMaxRange);
 
 	// If input ground has already values, add them to map
 	if(ground.rows == 1 && ground.cols>0 && ground.type() == CV_32FC2)
@@ -482,6 +506,43 @@ cv::Mat create2DMap(const std::map<int, Transform> & poses,
 		float & yMin,
 		float minMapSize,
 		float scanMaxRange)
+{
+	std::map<int, cv::Point3f > viewpoints;
+	return create2DMap(poses,
+			scans,
+			viewpoints,
+			cellSize,
+			unknownSpaceFilled,
+			xMin,
+			yMin,
+			minMapSize,
+			scanMaxRange);
+}
+
+/**
+ * Create 2d Occupancy grid (CV_8S)
+ * -1 = unknown
+ * 0 = empty space
+ * 100 = obstacle
+ * @param poses
+ * @param scans
+ * @param viewpoints
+ * @param cellSize m
+ * @param unknownSpaceFilled if false no fill, otherwise a virtual laser sweeps the unknown space from each pose (stopping on detected obstacle)
+ * @param xMin
+ * @param yMin
+ * @param minMapSize minimum map size in meters
+ * @param scanMaxRange laser scan maximum range, would be set if unknownSpaceFilled=true
+ */
+cv::Mat create2DMap(const std::map<int, Transform> & poses,
+		const std::map<int, pcl::PointCloud<pcl::PointXYZ>::Ptr > & scans,
+		const std::map<int, cv::Point3f > & viewpoints,
+		float cellSize,
+		bool unknownSpaceFilled,
+		float & xMin,
+		float & yMin,
+		float minMapSize,
+		float scanMaxRange)
 {
 	UDEBUG("poses=%d, scans = %d scanMaxRange=%f", poses.size(), scans.size(), scanMaxRange);
 	std::map<int, pcl::PointCloud<pcl::PointXYZ>::Ptr > localScans;
@@ -494,15 +555,23 @@ cv::Mat create2DMap(const std::map<int, Transform> & poses,
 	}
 	for(std::map<int, Transform>::const_iterator iter = poses.begin(); iter!=poses.end(); ++iter)
 	{
-		if(uContains(scans, iter->first) && scans.at(iter->first)->size())
+		std::map<int, pcl::PointCloud<pcl::PointXYZ>::Ptr >::const_iterator jter=scans.find(iter->first);
+		if(jter!=scans.end() && jter->second->size())
 		{
 			UASSERT(!iter->second.isNull());
-			pcl::PointCloud<pcl::PointXYZ>::Ptr cloud = util3d::transformPointCloud(scans.at(iter->first), iter->second);
+			pcl::PointCloud<pcl::PointXYZ>::Ptr cloud = util3d::transformPointCloud(jter->second, iter->second);
 			pcl::PointXYZ min, max;
 			pcl::getMinMax3D(*cloud, min, max);
 			minMax.push_back(min);
 			minMax.push_back(max);
 			minMax.push_back(pcl::PointXYZ(iter->second.x(), iter->second.y(), iter->second.z()));
+
+			std::map<int, cv::Point3f>::const_iterator kter=viewpoints.find(iter->first);
+			if(kter!=viewpoints.end())
+			{
+				minMax.push_back(pcl::PointXYZ(iter->second.x()+kter->second.x, iter->second.y()+kter->second.y, iter->second.z()+kter->second.z));
+			}
+
 			localScans.insert(std::make_pair(iter->first, cloud));
 		}
 	}
@@ -531,7 +600,13 @@ cv::Mat create2DMap(const std::map<int, Transform> & poses,
 		for(std::map<int, pcl::PointCloud<pcl::PointXYZ>::Ptr >::iterator iter = localScans.begin(); iter!=localScans.end(); ++iter)
 		{
 			const Transform & pose = poses.at(iter->first);
-			cv::Point2i start((pose.x()-xMin)/cellSize + 0.5f, (pose.y()-yMin)/cellSize + 0.5f);
+			cv::Point3f viewpoint(0,0,0);
+			std::map<int, cv::Point3f>::const_iterator kter=viewpoints.find(iter->first);
+			if(kter!=viewpoints.end())
+			{
+				viewpoint = kter->second;
+			}
+			cv::Point2i start(((pose.x()+viewpoint.x)-xMin)/cellSize + 0.5f, ((pose.y()+viewpoint.y)-yMin)/cellSize + 0.5f);
 			for(unsigned int i=0; i<iter->second->size(); ++i)
 			{
 				cv::Point2i end((iter->second->points[i].x-xMin)/cellSize, (iter->second->points[i].y-yMin)/cellSize);
@@ -557,7 +632,13 @@ cv::Mat create2DMap(const std::map<int, Transform> & poses,
 					if(scanMaxRange > cellSize)
 					{
 						const Transform & pose = poses.at(iter->first);
-						cv::Point2i start((pose.x()-xMin)/cellSize + 0.5f, (pose.y()-yMin)/cellSize + 0.5f);
+						cv::Point3f viewpoint(0,0,0);
+						std::map<int, cv::Point3f>::const_iterator kter=viewpoints.find(iter->first);
+						if(kter!=viewpoints.end())
+						{
+							viewpoint = kter->second;
+						}
+						cv::Point2i start(((pose.x()+viewpoint.x)-xMin)/cellSize + 0.5f, ((pose.y()+viewpoint.y)-yMin)/cellSize + 0.5f);
 
 						//UWARN("maxLength = %f", maxLength);
 						//rotate counterclockwise from the first point until we pass the last point
@@ -565,8 +646,8 @@ cv::Mat create2DMap(const std::map<int, Transform> & poses,
 						cv::Mat rotation = (cv::Mat_<float>(2,2) << cos(a), -sin(a),
 																	 sin(a), cos(a));
 						cv::Mat origin(2,1,CV_32F), endFirst(2,1,CV_32F), endLast(2,1,CV_32F);
-						origin.at<float>(0) = pose.x();
+						origin.at<float>(0) = pose.x()+viewpoint.x;
-						origin.at<float>(1) = pose.y();
+						origin.at<float>(1) = pose.y()+viewpoint.y;
 						pcl::PointXYZ ptFirst = iter->second->points[0];
 						pcl::PointXYZ ptLast = iter->second->points[iter->second->points.size()-1];
 						//if(ptFirst.y > ptLast.y)

guilib/include/rtabmap/gui/PreferencesDialog.h

@@ -299,6 +299,7 @@ private slots:
 	void updateKpROI();
 	void updateStereoDisparityVisibility();
 	void useOdomFeatures();
+	void useGridProjRayTracing();
 	void changeWorkingDirectory();
 	void changeDictionaryPath();
 	void readSettingsEnd();

guilib/src/PreferencesDialog.cpp

@@ -832,6 +832,7 @@ PreferencesDialog::PreferencesDialog(QWidget * parent) :
 	_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());
+	connect(_ui->checkBox_grid_projRayTracing, SIGNAL(stateChanged(int)), this, SLOT(useGridProjRayTracing()));
 	_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());
@@ -3750,6 +3751,22 @@ void PreferencesDialog::useOdomFeatures()
 	}
 }
 
+
+void PreferencesDialog::useGridProjRayTracing()
+{
+	if(this->isVisible() && _ui->checkBox_grid_projRayTracing->isChecked() && _ui->groupBox_grid_3d->isChecked())
+	{
+		int r = QMessageBox::question(this, tr("Using ray tracing for 2D projection..."),
+				tr("Currently the 3D occupancy grid parameter is checked, but 2D ray tracing "
+				  "only works with 2D occupancy grids. Do you want to uncheck 3D occupancy grid?"), QMessageBox::Yes | QMessageBox::No, QMessageBox::Yes);
+
+		if(r == QMessageBox::Yes)
+		{
+			_ui->groupBox_grid_3d->setChecked(false);
+		}
+	}
+}
+
 void PreferencesDialog::changeWorkingDirectory()
 {
 	QString directory = QFileDialog::getExistingDirectory(this, tr("Working directory"), _ui->lineEdit_workingDirectory->text());