Texturing: using depth image to filter wrongly projected textures

corelib/include/rtabmap/core/util3d_surface.h

@@ -137,7 +137,10 @@ pcl::TextureMesh::Ptr RTABMAP_EXP createTextureMesh(
 		const pcl::PolygonMesh::Ptr & mesh,
 		const std::map<int, Transform> & poses,
 		const std::map<int, CameraModel> & cameraModels,
+		const std::map<int, cv::Mat> & cameraDepths,
 		float maxDistance = 0.0f, // max camera distance to polygon to apply texture
+		float maxDepthError = 0.0f, // maximum depth error between reprojected mesh and depth image to texture a face (-1=disabled, 0=edge length is used)
+		float maxAngle = 0.0f, // maximum angle between camera and face (0=disabled)
 		int minClusterSize = 50, // minimum size of polygons clusters textured
 		const std::vector<float> & roiRatios = std::vector<float>(), // [left, right, top, bottom] region of interest (in ratios) of the image projected.
 		const ProgressState * state = 0,
@@ -146,7 +149,10 @@ pcl::TextureMesh::Ptr RTABMAP_EXP createTextureMesh(
 		const pcl::PolygonMesh::Ptr & mesh,
 		const std::map<int, Transform> & poses,
 		const std::map<int, std::vector<CameraModel> > & cameraModels,
+		const std::map<int, cv::Mat> & cameraDepths,
 		float maxDistance = 0.0f, // max camera distance to polygon to apply texture
+		float maxDepthError = 0.0f, // maximum depth error between reprojected mesh and depth image to texture a face (-1=disabled, 0=edge length is used)
+		float maxAngle = 0.0f, // maximum angle between camera and face (0=disabled)
 		int minClusterSize = 50, // minimum size of polygons clusters textured
 		const std::vector<float> & roiRatios = std::vector<float>(), // [left, right, top, bottom] region of interest (in ratios) of the image projected.
 		const ProgressState * state = 0,

corelib/src/pcl18/surface/impl/texture_mapping.hpp

@@ -1013,12 +1013,16 @@ class FaceInfo
 {
 public:
 	FaceInfo(float d,
+			float a,
+			float edge,
 			bool facingCam,
 			const pcl::PointXY & uv1,
 			const pcl::PointXY & uv2,
 			const pcl::PointXY & uv3,
 			const pcl::PointXY & center) :
 				distance(d),
+				angle(a),
+				longestEdgeSqrd(edge),
 				facingTheCam(facingCam),
 				uv_coord1(uv1),
 				uv_coord2(uv2),
@@ -1026,6 +1030,8 @@ public:
 				uv_center(center)
 	{}
 	float distance;
+	float angle;
+	float longestEdgeSqrd;
 	bool facingTheCam;
 	pcl::PointXY uv_coord1;
 	pcl::PointXY uv_coord2;
@@ -1116,10 +1122,36 @@ pcl::TextureMapping<PointInT>::textureMeshwithMultipleCameras2 (
 				float angle = normal.dot(Eigen::Vector3f(0.0f,0.0f,1.0f));
 				bool facingTheCam = angle>0.0f;
 				float distanceToCam = std::min(std::min(pt0.z, pt1.z), pt2.z);
+				float angleToCam = 0.0f;
+				Eigen::Vector3f e0 = Eigen::Vector3f(
+						pt1.x - pt0.x,
+						pt1.y - pt0.y,
+						pt1.z - pt0.z);
+				Eigen::Vector3f e1 = Eigen::Vector3f(
+						pt2.x - pt0.x,
+						pt2.y - pt0.y,
+						pt2.z - pt0.z);
+				Eigen::Vector3f e2 = Eigen::Vector3f(
+						pt2.x - pt1.x,
+						pt2.y - pt1.y,
+						pt2.z - pt1.z);
+				if(facingTheCam && this->max_angle_)
+				{
+					Eigen::Vector3f normal3D;
+					normal3D = e0.cross(e1);
+					angleToCam = pcl::getAngle3D(Eigen::Vector4f(normal3D[0], normal3D[1], normal3D[2], 0.0f), Eigen::Vector4f(0.0f,0.0f,-1.0f,0.0f));
+				}
+
+				// longest edge
+				float e0norm2 = e0[0]*e0[0] + e0[1]*e0[1] + e0[2]*e0[2];
+				float e1norm2 = e1[0]*e1[0] + e1[1]*e1[1] + e1[2]*e1[2];
+				float e2norm2 = e2[0]*e2[0] + e2[1]*e2[1] + e2[2]*e2[2];
+				float longestEdgeSqrd = std::max(std::max(e0norm2, e1norm2), e2norm2);
+
 				pcl::PointXY center;
 				center.x = (uv_coords1.x+uv_coords2.x+uv_coords3.x)/3.0f;
 				center.y = (uv_coords1.y+uv_coords2.y+uv_coords3.y)/3.0f;
-				visibleFaces[current_cam].insert(visibleFaces[current_cam].end(), std::make_pair(idx_face, FaceInfo(distanceToCam, facingTheCam, uv_coords1, uv_coords2, uv_coords3, center)));
+				visibleFaces[current_cam].insert(visibleFaces[current_cam].end(), std::make_pair(idx_face, FaceInfo(distanceToCam, angleToCam, longestEdgeSqrd, facingTheCam, uv_coords1, uv_coords2, uv_coords3, center)));
 				sortedVisibleFaces.insert(std::make_pair(distanceToCam, idx_face));
 				visibilityIndices[oi] = idx_face;
 				++oi;
@@ -1278,16 +1310,72 @@ pcl::TextureMapping<PointInT>::textureMeshwithMultipleCameras2 (
 		pcl::Vertices & face = faces[idx_face];
 
 		int cameraIndex = -1;
-		float closestDistanceToCam = std::numeric_limits<float>::max();
+		float smallestWeight = std::numeric_limits<float>::max();
-		float closestDistanceToCenter = std::numeric_limits<float>::max();
+		bool depthSet = false;
 		pcl::PointXY uv_coords[3];
 		for (std::list<int>::iterator camIter = faceCameras[idx_face].begin(); camIter!=faceCameras[idx_face].end(); ++camIter)
 		{
 			int current_cam = *camIter;
 			std::map<int, FaceInfo>::iterator iter = visibleFaces[current_cam].find(idx_face);
 			UASSERT(iter != visibleFaces[current_cam].end());
-			if (iter->second.facingTheCam)
+			if (iter->second.facingTheCam && (max_angle_ <=0.0f || iter->second.angle <= max_angle_))
 			{
+				float distanceToCam = iter->second.distance;
+				float vx = (iter->second.uv_coord1.x+iter->second.uv_coord2.x+ iter->second.uv_coord3.x)/3.0f-0.5f;
+				float vy = (iter->second.uv_coord1.y+iter->second.uv_coord2.y+ iter->second.uv_coord3.y)/3.0f-0.5f;
+				float distanceToCenter = vx*vx+vy*vy;
+
+				cv::Mat depth = cameras[current_cam].depth;
+				bool currentDepthSet = false;
+				float maxDepthError = max_depth_error_==0.0f?std::sqrt(iter->second.longestEdgeSqrd) : max_depth_error_;
+				if(!cameras[current_cam].depth.empty() && maxDepthError > 0.0f)
+				{
+					float d1 = depth.type() == CV_32FC1?
+							depth.at<float>((1.0f-iter->second.uv_coord1.y)*depth.rows, iter->second.uv_coord1.x*depth.cols):
+							float(depth.at<unsigned short>((1.0f-iter->second.uv_coord1.y)*depth.rows, iter->second.uv_coord1.x*depth.cols))/1000.0f;
+					float d2 = depth.type() == CV_32FC1?
+							depth.at<float>((1.0f-iter->second.uv_coord2.y)*depth.rows, iter->second.uv_coord2.x*depth.cols):
+							float(depth.at<unsigned short>((1.0f-iter->second.uv_coord2.y)*depth.rows, iter->second.uv_coord2.x*depth.cols))/1000.0f;
+					float d3 = depth.type() == CV_32FC1?
+							depth.at<float>((1.0f-iter->second.uv_coord3.y)*depth.rows, iter->second.uv_coord3.x*depth.cols):
+							float(depth.at<unsigned short>((1.0f-iter->second.uv_coord3.y)*depth.rows, iter->second.uv_coord3.x*depth.cols))/1000.0f;
+					if(d1 <= 0.0f || !std::isfinite(d1) || d2 <= 0.0f || !std::isfinite(d2) || d3 <= 0.0f || !std::isfinite(d3))
+					{
+						if(depthSet)
+						{
+							// ignore pixels with no depth
+							continue;
+						}
+						else if(d1 > 0.0f && std::isfinite(d1) && fabs(d1 - distanceToCam) > maxDepthError)
+						{
+							// ignore pixels with too much depth error
+							continue;
+						}
+						else if(d2 > 0.0f && std::isfinite(d2) && fabs(d2 - distanceToCam) > maxDepthError)
+						{
+							// ignore pixels with too much depth error
+							continue;
+						}
+						else if(d3 > 0.0f && std::isfinite(d3) && fabs(d3 - distanceToCam) > maxDepthError)
+						{
+							// ignore pixels with too much depth error
+							continue;
+						}
+						//else it could be a window for which no depth is available on any cameras
+					}
+					else
+					{
+						if(fabs(d1 - distanceToCam) > maxDepthError ||
+							fabs(d2 - distanceToCam) > maxDepthError ||
+							fabs(d3 - distanceToCam) > maxDepthError)
+						{
+							// ignore pixels with too much depth error
+							continue;
+						}
+						currentDepthSet = true;
+					}
+				}
+
 				if(vertexToPixels)
 				{
 					vertexToPixels->at(face.vertices[0]).insert(std::make_pair(current_cam, iter->second.uv_coord1));
@@ -1295,22 +1383,19 @@ pcl::TextureMapping<PointInT>::textureMeshwithMultipleCameras2 (
 					vertexToPixels->at(face.vertices[2]).insert(std::make_pair(current_cam, iter->second.uv_coord3));
 				}
 
-				float distanceToCam = 0;//iter->second.distance;
-				float vx = (iter->second.uv_coord1.x+iter->second.uv_coord2.x+ iter->second.uv_coord3.x)/3.0f-0.5f;
-				float vy = (iter->second.uv_coord1.y+iter->second.uv_coord2.y+ iter->second.uv_coord3.y)/3.0f-0.5f;
-				float distanceToCenter = vx*vx+vy*vy;
-
 				//UDEBUG("Process polygon %d cam =%d distanceToCam=%f", idx_face, current_cam, distanceToCam);
 
-				if(//(distanceToCam <= closestDistanceToCam && distanceToCenter <= closestDistanceToCenter * 1.1) ||
+				if(distanceToCenter <= smallestWeight || (!depthSet && currentDepthSet))
-				   (distanceToCam <= closestDistanceToCam * 2 && distanceToCenter <= closestDistanceToCenter))
 				{
 					cameraIndex = current_cam;
-					closestDistanceToCam = distanceToCam;
+					smallestWeight = distanceToCenter;
-					closestDistanceToCenter = distanceToCenter;
 					uv_coords[0] = iter->second.uv_coord1;
 					uv_coords[1] = iter->second.uv_coord2;
 					uv_coords[2] = iter->second.uv_coord3;
+					if(!depthSet && currentDepthSet)
+					{
+						depthSet = true;
+					}
 				}
 			}
 		}
@@ -1432,12 +1517,14 @@ pcl::TextureMapping<PointInT>::getPointUVCoordinates(const PointInT &pt, const C
 			}
 		}
 
-		// original code of PCL inverted y
-		UV_coordinates.y = 1.0f - UV_coordinates.y;
-
 		// point is visible!
 		if (UV_coordinates.x >= 0.0 && UV_coordinates.x <= 1.0 && UV_coordinates.y >= 0.0 && UV_coordinates.y <= 1.0)
-			return (true); // point was visible by the camera
+		{
+			// point is visible by the camera
+			// original code of PCL inverted y
+			UV_coordinates.y = 1.0f - UV_coordinates.y;
+			return (true);
+		}
 	}
 
 	// point is NOT visible by the camera

corelib/src/pcl18/surface/texture_mapping.h

@@ -78,6 +78,7 @@ namespace pcl
       double width;
       std::string texture_file;
       std::vector<double> roi; // [x, y, width, height]
+      cv::Mat depth;
 
       EIGEN_MAKE_ALIGNED_OPERATOR_NEW
     };
@@ -120,7 +121,7 @@ namespace pcl
 
       /** \brief Constructor. */
       TextureMapping () :
-        f_ (), vector_field_ (), tex_files_ (), tex_material_ (), max_distance_(0.0f), min_cluster_size_(50)
+        f_ (), vector_field_ (), tex_files_ (), tex_material_ (), max_distance_(0.0f), max_depth_error_(0.0f), max_angle_(0.0f), min_cluster_size_(50)
       {
       }
 
@@ -175,6 +176,18 @@ namespace pcl
     	  max_distance_ = maxDistance;
       }
 
+      inline void
+	  setMaxDepthError(float maxDepthError)
+	  {
+    	  max_depth_error_ = maxDepthError;
+	  }
+
+      inline void
+	  setMaxAngle(float maxAngle)
+	  {
+    	  max_angle_ = maxAngle;
+	  }
+
       inline void
 	  setMinClusterSize(int size)
       {
@@ -371,6 +384,12 @@ namespace pcl
       /** \brief maximum distance between camera and polygon to apply a texture */
       float max_distance_;
 
+      /** \brief maximum depth error between projected point and corresponding depth of the camera to apply a texture */
+      float max_depth_error_;
+
+      /** \brief maximum angle (rad) between camera and polygon to apply a texture */
+      float max_angle_;
+
       /** \brief Remove texture from small polygon clusters */
       int min_cluster_size_;
 

corelib/src/util3d_surface.cpp

@@ -574,6 +574,7 @@ pcl::PolygonMesh::Ptr createMesh(
 pcl::texture_mapping::CameraVector createTextureCameras(
 		const std::map<int, Transform> & poses,
 		const std::map<int, std::vector<CameraModel> > & cameraModels,
+		const std::map<int, cv::Mat> & cameraDepths,
 		const std::vector<float> & roiRatios)
 {
 	UASSERT_MSG(poses.size() == cameraModels.size(), uFormat("%d vs %d", (int)poses.size(), (int)cameraModels.size()).c_str());
@@ -585,6 +586,8 @@ pcl::texture_mapping::CameraVector createTextureCameras(
 	{
 		UASSERT(poseIter->first == modelIter->first);
 
+		std::map<int, cv::Mat>::const_iterator depthIter = cameraDepths.find(poseIter->first);
+
 		// for each sub camera
 		for(unsigned int i=0; i<modelIter->second.size(); ++i)
 		{
@@ -621,6 +624,15 @@ pcl::texture_mapping::CameraVector createTextureCameras(
 				cam.roi[2] = cam.width * (1.0 - roiRatios[1]) - cam.roi[0]; // right -> width
 				cam.roi[3] = cam.height * (1.0 - roiRatios[3]) - cam.roi[1]; // bottom -> height
 			}
+
+			if(depthIter != cameraDepths.end() && !depthIter->second.empty())
+			{
+				UASSERT(depthIter->second.type() == CV_32FC1 || depthIter->second.type() == CV_16UC1);
+				UASSERT(depthIter->second.cols % modelIter->second.size() == 0);
+				int subWidth = depthIter->second.cols/(modelIter->second.size());
+				cam.depth = cv::Mat(depthIter->second, cv::Range(0, depthIter->second.rows), cv::Range(subWidth*i, subWidth*(i+1)));
+			}
+
 			cameras.push_back(cam);
 		}
 	}
@@ -631,7 +643,10 @@ pcl::TextureMesh::Ptr createTextureMesh(
 		const pcl::PolygonMesh::Ptr & mesh,
 		const std::map<int, Transform> & poses,
 		const std::map<int, CameraModel> & cameraModels,
+		const std::map<int, cv::Mat> & cameraDepths,
 		float maxDistance,
+		float maxDepthError,
+		float maxAngle,
 		int minClusterSize,
 		const std::vector<float> & roiRatios,
 		const ProgressState * state,
@@ -649,7 +664,10 @@ pcl::TextureMesh::Ptr createTextureMesh(
 			mesh,
 			poses,
 			cameraSubModels,
+			cameraDepths,
 			maxDistance,
+			maxDepthError,
+			maxAngle,
 			minClusterSize,
 			roiRatios,
 			state,
@@ -660,7 +678,10 @@ pcl::TextureMesh::Ptr createTextureMesh(
 		const pcl::PolygonMesh::Ptr & mesh,
 		const std::map<int, Transform> & poses,
 		const std::map<int, std::vector<CameraModel> > & cameraModels,
+		const std::map<int, cv::Mat> & cameraDepths,
 		float maxDistance,
+		float maxDepthError,
+		float maxAngle,
 		int minClusterSize,
 		const std::vector<float> & roiRatios,
 		const ProgressState * state,
@@ -680,6 +701,7 @@ pcl::TextureMesh::Ptr createTextureMesh(
 	pcl::texture_mapping::CameraVector cameras = createTextureCameras(
 			poses,
 			cameraModels,
+			cameraDepths,
 			roiRatios);
 
 	// Create materials for each texture (and one extra for occluded faces)
@@ -722,6 +744,11 @@ pcl::TextureMesh::Ptr createTextureMesh(
 	// Texture by projection
 	pcl::TextureMapping<pcl::PointXYZ> tm; // TextureMapping object that will perform the sort
 	tm.setMaxDistance(maxDistance);
+	tm.setMaxAngle(maxAngle);
+	if(maxDepthError > 0.0f)
+	{
+		tm.setMaxDepthError(maxDepthError);
+	}
 	tm.setMinClusterSize(minClusterSize);
 	if(tm.textureMeshwithMultipleCameras2(*textureMesh, cameras, state, vertexToPixels))
 	{

guilib/src/ExportCloudsDialog.cpp

@@ -170,6 +170,8 @@ ExportCloudsDialog::ExportCloudsDialog(QWidget *parent) :
 	connect(_ui->comboBox_meshingTextureSize, SIGNAL(currentIndexChanged(int)), this, SIGNAL(configChanged()));
 	connect(_ui->spinBox_mesh_maxTextures, SIGNAL(valueChanged(int)), this, SIGNAL(configChanged()));
 	connect(_ui->doubleSpinBox_meshingTextureMaxDistance, SIGNAL(valueChanged(double)), this, SIGNAL(configChanged()));
+	connect(_ui->doubleSpinBox_meshingTextureMaxDepthError, SIGNAL(valueChanged(double)), this, SIGNAL(configChanged()));
+	connect(_ui->doubleSpinBox_meshingTextureMaxAngle, SIGNAL(valueChanged(double)), this, SIGNAL(configChanged()));
 	connect(_ui->spinBox_mesh_minTextureClusterSize, SIGNAL(valueChanged(int)), this, SIGNAL(configChanged()));
 	connect(_ui->lineEdit_meshingTextureRoiRatios, SIGNAL(textChanged(const QString &)), this, SIGNAL(configChanged()));
 	connect(_ui->checkBox_cameraFilter, SIGNAL(stateChanged(int)), this, SIGNAL(configChanged()));
@@ -310,6 +312,8 @@ void ExportCloudsDialog::saveSettings(QSettings & settings, const QString & grou
 	settings.setValue("mesh_textureSize", _ui->comboBox_meshingTextureSize->currentIndex());
 	settings.setValue("mesh_textureMaxCount", _ui->spinBox_mesh_maxTextures->value());
 	settings.setValue("mesh_textureMaxDistance", _ui->doubleSpinBox_meshingTextureMaxDistance->value());
+	settings.setValue("mesh_textureMaxDepthError", _ui->doubleSpinBox_meshingTextureMaxDepthError->value());
+	settings.setValue("mesh_textureMaxAngle", _ui->doubleSpinBox_meshingTextureMaxAngle->value());
 	settings.setValue("mesh_textureMinCluster", _ui->spinBox_mesh_minTextureClusterSize->value());
 	settings.setValue("mesh_textureRoiRatios", _ui->lineEdit_meshingTextureRoiRatios->text());
 	settings.setValue("mesh_textureCameraFiltering", _ui->checkBox_cameraFilter->isChecked());
@@ -421,6 +425,8 @@ void ExportCloudsDialog::loadSettings(QSettings & settings, const QString & grou
 	_ui->comboBox_meshingTextureSize->setCurrentIndex(settings.value("mesh_textureSize", _ui->comboBox_meshingTextureSize->currentIndex()).toInt());
 	_ui->spinBox_mesh_maxTextures->setValue(settings.value("mesh_textureMaxCount", _ui->spinBox_mesh_maxTextures->value()).toInt());
 	_ui->doubleSpinBox_meshingTextureMaxDistance->setValue(settings.value("mesh_textureMaxDistance", _ui->doubleSpinBox_meshingTextureMaxDistance->value()).toDouble());
+	_ui->doubleSpinBox_meshingTextureMaxDepthError->setValue(settings.value("mesh_textureMaxDepthError", _ui->doubleSpinBox_meshingTextureMaxDepthError->value()).toDouble());
+	_ui->doubleSpinBox_meshingTextureMaxAngle->setValue(settings.value("mesh_textureMaxAngle", _ui->doubleSpinBox_meshingTextureMaxAngle->value()).toDouble());
 	_ui->spinBox_mesh_minTextureClusterSize->setValue(settings.value("mesh_textureMinCluster", _ui->spinBox_mesh_minTextureClusterSize->value()).toDouble());
 	_ui->lineEdit_meshingTextureRoiRatios->setText(settings.value("mesh_textureRoiRatios", _ui->lineEdit_meshingTextureRoiRatios->text()).toString());
 	_ui->checkBox_cameraFilter->setChecked(settings.value("mesh_textureCameraFiltering", _ui->checkBox_cameraFilter->isChecked()).toBool());
@@ -532,6 +538,8 @@ void ExportCloudsDialog::restoreDefaults()
 	_ui->comboBox_meshingTextureSize->setCurrentIndex(5); // 4096
 	_ui->spinBox_mesh_maxTextures->setValue(1);
 	_ui->doubleSpinBox_meshingTextureMaxDistance->setValue(3.0);
+	_ui->doubleSpinBox_meshingTextureMaxDepthError->setValue(0.0);
+	_ui->doubleSpinBox_meshingTextureMaxAngle->setValue(0.0);
 	_ui->spinBox_mesh_minTextureClusterSize->setValue(50);
 	_ui->lineEdit_meshingTextureRoiRatios->setText("0.0 0.0 0.0 0.0");
 	_ui->checkBox_cameraFilter->setChecked(false);
@@ -1928,6 +1936,7 @@ bool ExportCloudsDialog::getExportedClouds(
 				}
 				std::map<int, Transform> cameraPoses;
 				std::map<int, std::vector<CameraModel> > cameraModels;
+				std::map<int, cv::Mat > cameraDepths;
 				for(std::map<int, Transform>::iterator jter=cameras.begin(); jter!=cameras.end(); ++jter)
 				{
 					if(validCameras.find(jter->first) != validCameras.end())
@@ -1947,8 +1956,10 @@ bool ExportCloudsDialog::getExportedClouds(
 							_dbDriver->getCalibration(jter->first, models, stereoModel);
 						}
 
+						bool stereo=false;
 						if(stereoModel.isValidForProjection())
 						{
+							stereo = true;
 							models.clear();
 							models.push_back(stereoModel.left());
 						}
@@ -1956,8 +1967,11 @@ bool ExportCloudsDialog::getExportedClouds(
 						{
 							models.clear();
 						}
+
 						if(!jter->second.isNull() && models.size())
 						{
+							cv::Mat depth;
+							bool getDepth = !stereo && _ui->doubleSpinBox_meshingTextureMaxDepthError->value() >= 0.0f;
 							if(models[0].imageWidth() == 0 || models[0].imageHeight() == 0)
 							{
 								// we are using an old database format (image size not saved in calibrations), we should
@@ -1965,13 +1979,13 @@ bool ExportCloudsDialog::getExportedClouds(
 								cv::Mat img;
 								if(cacheHasCompressedImage)
 								{
-									cachedSignatures.find(jter->first)->sensorData().uncompressDataConst(&img, 0);
+									cachedSignatures.find(jter->first)->sensorData().uncompressDataConst(&img, getDepth?&depth:0);
 								}
 								else if(_dbDriver)
 								{
 									SensorData data;
 									_dbDriver->getNodeData(jter->first, data, true, false, false, false);
-									data.uncompressDataConst(&img, 0);
+									data.uncompressDataConst(&img, getDepth?&depth:0);
 								}
 								cv::Size imageSize = img.size();
 								imageSize.width /= models.size();
@@ -1979,13 +1993,30 @@ bool ExportCloudsDialog::getExportedClouds(
 								{
 									models[i].setImageSize(imageSize);
 								}
-
+							}
+							else
+							{
+								// get just the depth
+								if(cacheHasCompressedImage)
+								{
+									cachedSignatures.find(jter->first)->sensorData().uncompressDataConst(0, getDepth?&depth:0);
+								}
+								else if(_dbDriver)
+								{
+									SensorData data;
+									_dbDriver->getNodeData(jter->first, data, true, false, false, false);
+									data.uncompressDataConst(0, getDepth?&depth:0);
+								}
 							}
 
 							if(models[0].imageWidth() != 0 && models[0].imageHeight() != 0)
 							{
 								cameraPoses.insert(std::make_pair(jter->first, jter->second));
 								cameraModels.insert(std::make_pair(jter->first, models));
+								if(!depth.empty())
+								{
+									cameraDepths.insert(std::make_pair(jter->first, depth));
+								}
 							}
 						}
 					}
@@ -2058,6 +2089,7 @@ bool ExportCloudsDialog::getExportedClouds(
 								if(cameraPoses.find(modelIter->first)==cameraPoses.end())
 								{
 									cameraModels.erase(modelIter++);
+									cameraDepths.erase(modelIter->first);
 								}
 								else
 								{
@@ -2114,7 +2146,10 @@ bool ExportCloudsDialog::getExportedClouds(
 								iter->second,
 								cameraPoses,
 								cameraModels,
+								cameraDepths,
 								_ui->doubleSpinBox_meshingTextureMaxDistance->value(),
+								_ui->doubleSpinBox_meshingTextureMaxDepthError->value(),
+								_ui->doubleSpinBox_meshingTextureMaxAngle->value()*M_PI/180.0,
 								_ui->spinBox_mesh_minTextureClusterSize->value(),
 								roiRatios,
 								&texturingState,
@@ -3097,7 +3132,7 @@ std::vector<cv::Mat> ExportCloudsDialog::mergeTextures(
 					}
 				}
 			}
-			else if(mesh.tex_polygons[i].size())
+			else if(mesh.tex_polygons[i].size() && mesh.tex_materials[i].tex_file.compare("occluded")!=0)
 			{
 				UWARN("Failed parsing texture file name: %s", mesh.tex_materials[i].tex_file.c_str());
 			}
@@ -3278,9 +3313,16 @@ std::vector<cv::Mat> ExportCloudsDialog::mergeTextures(
 
 						for(int i=0; i<num_images; ++i)
 						{
-							for(int j=i+1; j<num_images; ++j)
+							for(int j=i; j<num_images; ++j)
 							{
-								if(N(i, j))
+								if(i == j)
+								{
+									if(N(i,j) == 0)
+									{
+										N(i,j) = 1;
+									}
+								}
+								else if(N(i, j))
 								{
 									I(i, j) /= N(i, j);
 									I(j, i) /= N(j, i);

guilib/src/ui/exportCloudsDialog.ui

@@ -23,9 +23,9 @@
       <property name="geometry">
        <rect>
         <x>0</x>
-        <y>-1412</y>
+        <y>-2146</y>
         <width>773</width>
-        <height>3953</height>
+        <height>4045</height>
        </rect>
       </property>
       <layout class="QVBoxLayout" name="verticalLayout_13">
@@ -1486,7 +1486,111 @@ Guidelines: 4 times the voxel size, 0.025 for voxel=0.</string>
             <layout class="QVBoxLayout" name="verticalLayout_16">
              <item>
               <layout class="QGridLayout" name="gridLayout_15" columnstretch="0,1">
-               <item row="7" column="1">
+               <item row="12" column="0">
+                <widget class="QSpinBox" name="spinBox_textureBrightnessContrastRatioHigh">
+                 <property name="suffix">
+                  <string> %</string>
+                 </property>
+                 <property name="minimum">
+                  <number>0</number>
+                 </property>
+                 <property name="maximum">
+                  <number>49</number>
+                 </property>
+                </widget>
+               </item>
+               <item row="12" column="1">
+                <widget class="QLabel" name="label_meshingTextureSize_6">
+                 <property name="text">
+                  <string>Brightness and contrast balance high ratio. Only used when textures are merged.</string>
+                 </property>
+                 <property name="wordWrap">
+                  <bool>true</bool>
+                 </property>
+                </widget>
+               </item>
+               <item row="10" column="0">
+                <widget class="QComboBox" name="comboBox_blendingDecimation">
+                 <item>
+                  <property name="text">
+                   <string>Auto</string>
+                  </property>
+                 </item>
+                 <item>
+                  <property name="text">
+                   <string>1</string>
+                  </property>
+                 </item>
+                 <item>
+                  <property name="text">
+                   <string>2</string>
+                  </property>
+                 </item>
+                 <item>
+                  <property name="text">
+                   <string>4</string>
+                  </property>
+                 </item>
+                 <item>
+                  <property name="text">
+                   <string>8</string>
+                  </property>
+                 </item>
+                 <item>
+                  <property name="text">
+                   <string>16</string>
+                  </property>
+                 </item>
+                 <item>
+                  <property name="text">
+                   <string>32</string>
+                  </property>
+                 </item>
+                 <item>
+                  <property name="text">
+                   <string>64</string>
+                  </property>
+                 </item>
+                </widget>
+               </item>
+               <item row="9" column="0">
+                <widget class="QCheckBox" name="checkBox_blending">
+                 <property name="text">
+                  <string/>
+                 </property>
+                </widget>
+               </item>
+               <item row="10" column="1">
+                <widget class="QLabel" name="label_exposureFusion_3">
+                 <property name="text">
+                  <string>Blending decimation. </string>
+                 </property>
+                 <property name="wordWrap">
+                  <bool>true</bool>
+                 </property>
+                </widget>
+               </item>
+               <item row="3" column="1">
+                <widget class="QLabel" name="label_meshingTextureSize_7">
+                 <property name="text">
+                  <string>ROI ratios [left right top bottom] between 0 and 1. This can be used to ignore black borders of RGB images caused by calibration. </string>
+                 </property>
+                 <property name="wordWrap">
+                  <bool>true</bool>
+                 </property>
+                </widget>
+               </item>
+               <item row="2" column="1">
+                <widget class="QLabel" name="label_meshingTextureSize_8">
+                 <property name="text">
+                  <string>Maximum output textures.</string>
+                 </property>
+                 <property name="wordWrap">
+                  <bool>true</bool>
+                 </property>
+                </widget>
+               </item>
+               <item row="9" column="1">
                 <widget class="QLabel" name="label_exposureFusion_2">
                  <property name="text">
                   <string>Blending. Only used with dense reconstruction flavor and if clouds are assembled.</string>
@@ -1528,7 +1632,7 @@ Guidelines: 4 times the voxel size, 0.025 for voxel=0.</string>
                  </property>
                 </widget>
                </item>
-               <item row="9" column="1">
+               <item row="11" column="1">
                 <widget class="QLabel" name="label_meshingTextureSize_5">
                  <property name="text">
                   <string>Brightness and contrast balance low ratio. Only used when textures are merged.</string>
@@ -1562,29 +1666,6 @@ Guidelines: 4 times the voxel size, 0.025 for voxel=0.</string>
                  </item>
                 </widget>
                </item>
-               <item row="10" column="0">
-                <widget class="QSpinBox" name="spinBox_textureBrightnessContrastRatioHigh">
-                 <property name="suffix">
-                  <string> %</string>
-                 </property>
-                 <property name="minimum">
-                  <number>0</number>
-                 </property>
-                 <property name="maximum">
-                  <number>49</number>
-                 </property>
-                </widget>
-               </item>
-               <item row="10" column="1">
-                <widget class="QLabel" name="label_meshingTextureSize_6">
-                 <property name="text">
-                  <string>Brightness and contrast balance high ratio. Only used when textures are merged.</string>
-                 </property>
-                 <property name="wordWrap">
-                  <bool>true</bool>
-                 </property>
-                </widget>
-               </item>
                <item row="1" column="0">
                 <widget class="QComboBox" name="comboBox_meshingTextureSize">
                  <item>
@@ -1634,14 +1715,14 @@ Guidelines: 4 times the voxel size, 0.025 for voxel=0.</string>
                  </item>
                 </widget>
                </item>
-               <item row="12" column="0">
+               <item row="14" column="0">
                 <widget class="QCheckBox" name="checkBox_exposureFusion">
                  <property name="text">
                   <string/>
                  </property>
                 </widget>
                </item>
-               <item row="5" column="1">
+               <item row="7" column="1">
                 <widget class="QLabel" name="label_meshingTextureSize_4">
                  <property name="text">
                   <string>Minimum polygon cluster size for texturing. This removes textures applied on sparse polygons. Only used with dense reconstruction flavor.</string>
@@ -1654,50 +1735,6 @@ Guidelines: 4 times the voxel size, 0.025 for voxel=0.</string>
                <item row="3" column="0">
                 <widget class="QLineEdit" name="lineEdit_meshingTextureRoiRatios"/>
                </item>
-               <item row="8" column="0">
-                <widget class="QComboBox" name="comboBox_blendingDecimation">
-                 <item>
-                  <property name="text">
-                   <string>Auto</string>
-                  </property>
-                 </item>
-                 <item>
-                  <property name="text">
-                   <string>1</string>
-                  </property>
-                 </item>
-                 <item>
-                  <property name="text">
-                   <string>2</string>
-                  </property>
-                 </item>
-                 <item>
-                  <property name="text">
-                   <string>4</string>
-                  </property>
-                 </item>
-                 <item>
-                  <property name="text">
-                   <string>8</string>
-                  </property>
-                 </item>
-                 <item>
-                  <property name="text">
-                   <string>16</string>
-                  </property>
-                 </item>
-                 <item>
-                  <property name="text">
-                   <string>32</string>
-                  </property>
-                 </item>
-                 <item>
-                  <property name="text">
-                   <string>64</string>
-                  </property>
-                 </item>
-                </widget>
-               </item>
                <item row="1" column="1">
                 <widget class="QLabel" name="label_meshingTextureSize">
                  <property name="text">
@@ -1708,21 +1745,14 @@ Guidelines: 4 times the voxel size, 0.025 for voxel=0.</string>
                  </property>
                 </widget>
                </item>
-               <item row="7" column="0">
+               <item row="8" column="0">
-                <widget class="QCheckBox" name="checkBox_blending">
-                 <property name="text">
-                  <string/>
-                 </property>
-                </widget>
-               </item>
-               <item row="6" column="0">
                 <widget class="QCheckBox" name="checkBox_cameraFilter">
                  <property name="text">
                   <string/>
                  </property>
                 </widget>
                </item>
-               <item row="9" column="0">
+               <item row="11" column="0">
                 <widget class="QSpinBox" name="spinBox_textureBrightnessContrastRatioLow">
                  <property name="suffix">
                   <string> %</string>
@@ -1735,7 +1765,7 @@ Guidelines: 4 times the voxel size, 0.025 for voxel=0.</string>
                  </property>
                 </widget>
                </item>
-               <item row="5" column="0">
+               <item row="7" column="0">
                 <widget class="QSpinBox" name="spinBox_mesh_minTextureClusterSize">
                  <property name="minimum">
                   <number>0</number>
@@ -1746,16 +1776,6 @@ Guidelines: 4 times the voxel size, 0.025 for voxel=0.</string>
                 </widget>
                </item>
                <item row="8" column="1">
-                <widget class="QLabel" name="label_exposureFusion_3">
-                 <property name="text">
-                  <string>Blending decimation. </string>
-                 </property>
-                 <property name="wordWrap">
-                  <bool>true</bool>
-                 </property>
-                </widget>
-               </item>
-               <item row="6" column="1">
                 <widget class="QLabel" name="label_meshingTextureSize_3">
                  <property name="text">
                   <string>Camera filtering. By comparing poses in the same area, only one camera in a fixed radius and angle is used for texturing.</string>
@@ -1765,7 +1785,7 @@ Guidelines: 4 times the voxel size, 0.025 for voxel=0.</string>
                  </property>
                 </widget>
                </item>
-               <item row="12" column="1">
+               <item row="14" column="1">
                 <widget class="QLabel" name="label_exposureFusion">
                  <property name="text">
                   <string>Exposure fusion. Only used when textures are merged and brightness/contrast balance ratios are used (merging original texture + 1 for each ratios).</string>
@@ -1775,16 +1795,6 @@ Guidelines: 4 times the voxel size, 0.025 for voxel=0.</string>
                  </property>
                 </widget>
                </item>
-               <item row="3" column="1">
-                <widget class="QLabel" name="label_meshingTextureSize_7">
-                 <property name="text">
-                  <string>ROI ratios [left right top bottom] between 0 and 1. This can be used to ignore black borders of RGB images caused by calibration. </string>
-                 </property>
-                 <property name="wordWrap">
-                  <bool>true</bool>
-                 </property>
-                </widget>
-               </item>
                <item row="2" column="0">
                 <widget class="QSpinBox" name="spinBox_mesh_maxTextures">
                  <property name="minimum">
@@ -1795,16 +1805,70 @@ Guidelines: 4 times the voxel size, 0.025 for voxel=0.</string>
                  </property>
                 </widget>
                </item>
-               <item row="2" column="1">
+               <item row="5" column="1">
-                <widget class="QLabel" name="label_meshingTextureSize_8">
+                <widget class="QLabel" name="label_meshingTextureSize_9">
                  <property name="text">
-                  <string>Maximum output textures.</string>
+                  <string>Maximum depth error between reconstructed mesh projection in camera and corresponding value in depth image to apply texturing (negative means disabled, 0 means that longest edge length of the face is used as maximum error).</string>
                  </property>
                  <property name="wordWrap">
                   <bool>true</bool>
                  </property>
                 </widget>
                </item>
+               <item row="5" column="0">
+                <widget class="QDoubleSpinBox" name="doubleSpinBox_meshingTextureMaxDepthError">
+                 <property name="suffix">
+                  <string> m</string>
+                 </property>
+                 <property name="decimals">
+                  <number>2</number>
+                 </property>
+                 <property name="minimum">
+                  <double>-1.000000000000000</double>
+                 </property>
+                 <property name="maximum">
+                  <double>99.000000000000000</double>
+                 </property>
+                 <property name="singleStep">
+                  <double>0.050000000000000</double>
+                 </property>
+                 <property name="value">
+                  <double>0.000000000000000</double>
+                 </property>
+                </widget>
+               </item>
+               <item row="6" column="1">
+                <widget class="QLabel" name="label_meshingTextureSize_10">
+                 <property name="text">
+                  <string>Maximum angle between camera and face to apply texture (0 means disabled).</string>
+                 </property>
+                 <property name="wordWrap">
+                  <bool>true</bool>
+                 </property>
+                </widget>
+               </item>
+               <item row="6" column="0">
+                <widget class="QDoubleSpinBox" name="doubleSpinBox_meshingTextureMaxAngle">
+                 <property name="suffix">
+                  <string> deg</string>
+                 </property>
+                 <property name="decimals">
+                  <number>0</number>
+                 </property>
+                 <property name="minimum">
+                  <double>0.000000000000000</double>
+                 </property>
+                 <property name="maximum">
+                  <double>180.000000000000000</double>
+                 </property>
+                 <property name="singleStep">
+                  <double>1.000000000000000</double>
+                 </property>
+                 <property name="value">
+                  <double>0.000000000000000</double>
+                 </property>
+                </widget>
+               </item>
               </layout>
              </item>
              <item>