Merge branch 'ios_feb_2025_updates' of github.com:introlab/rtabmap into ios_feb_2025_updates

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

@@ -151,7 +151,10 @@ void denseMeshPostProcessing(
 				coloredPts.at(i) = false;
 			}
 		}
-		pcl::toPCLPointCloud2(*coloredCloud, mesh->cloud);
+		if(coloredOutput) {
+			pcl::toPCLPointCloud2(*coloredCloud, mesh->cloud);
+			hasColors = true;
+		}
 
 		// remove polygons with no color
 		if(cleanMesh)
@@ -177,7 +180,6 @@ void denseMeshPostProcessing(
 			filteredPolygons.resize(oi);
 			mesh->polygons = filteredPolygons;
 		}
-		hasColors = true;
 	}
 
 	if(minClusterSize)
@@ -238,11 +240,11 @@ void denseMeshPostProcessing(
 		if(progressState) progressState->callback(uFormat("Filtered %d polygons.", before-(int)mesh->polygons.size()));
 	}
 
-	// compute normals for the mesh if not already here, add also white color if colored output is required
+	// compute normals for the mesh if not already here
-	if(!hasNormals || (!hasColors && coloredOutput))
+	if(!hasNormals)
 	{
 		// use polygons
-		if(hasColors || coloredOutput)
+		if(hasColors)
 		{
 			pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr cloud (new pcl::PointCloud<pcl::PointXYZRGBNormal>);
 			pcl::fromPCLPointCloud2(mesh->cloud, *cloud);
@@ -269,18 +271,9 @@ void denseMeshPostProcessing(
 				// flat normal (per face)
 				for(unsigned int j=0; j<v.vertices.size(); ++j)
 				{
-					if(!hasNormals)
+					cloud->at(v.vertices[j]).normal_x = normal[0];
-					{
+					cloud->at(v.vertices[j]).normal_y = normal[1];
-						cloud->at(v.vertices[j]).normal_x = normal[0];
+					cloud->at(v.vertices[j]).normal_z = normal[2];
-						cloud->at(v.vertices[j]).normal_y = normal[1];
-						cloud->at(v.vertices[j]).normal_z = normal[2];
-					}
-					if(!hasColors)
-					{
-						cloud->at(v.vertices[j]).r = 255;
-						cloud->at(v.vertices[j]).g = 255;
-						cloud->at(v.vertices[j]).b = 255;
-					}
 				}
 			}
 			pcl::toPCLPointCloud2 (*cloud, mesh->cloud);

corelib/include/rtabmap/core/util3d_surface.h

@@ -225,6 +225,7 @@ cv::Mat RTABMAP_CORE_EXPORT mergeTextures(
 		bool exposureFusion = false,         //Exposure fusion can be used only with OpenCV3
 		const ProgressState * state = 0,
 		unsigned char blankValue = 255,      //Gray value for blank polygons (without texture)
+		bool clearVertexColorUnderTexture = true,
 		std::map<int, std::map<int, cv::Vec4d> > * gains = 0, // <Camera ID, Camera Sub Index (multi-cameras), gains Gray-R-G-B>
 		std::map<int, std::map<int, cv::Mat> > * blendingGains = 0, // <Camera ID, Camera Sub Index (multi-cameras), gains>
 		std::pair<float, float> * contrastValues = 0); // Alpha/beta contrast values
@@ -247,6 +248,7 @@ cv::Mat RTABMAP_CORE_EXPORT mergeTextures(
 		bool exposureFusion = false,         //Exposure fusion can be used only with OpenCV3
 		const ProgressState * state = 0,
 		unsigned char blankValue = 255,      //Gray value for blank polygons (without texture)
+		bool clearVertexColorUnderTexture = true,
 		std::map<int, std::map<int, cv::Vec4d> > * gains = 0, // <Camera ID, Camera Sub Index (multi-cameras), gains Gray-R-G-B>
 		std::map<int, std::map<int, cv::Mat> > * blendingGains = 0, // <Camera ID, Camera Sub Index (multi-cameras), gains>
 		std::pair<float, float> * contrastValues = 0); // Alpha/beta contrast values
@@ -586,6 +588,15 @@ bool intersectRayMesh(
 		Eigen::Vector3f & normal,
 		int & index);
 
+int RTABMAP_CORE_EXPORT saveOBJFile(
+	const std::string &file_name,
+    const pcl::TextureMesh &tex_mesh,
+	unsigned precision = 5);
+
+int RTABMAP_CORE_EXPORT saveOBJFile(
+	const std::string &file_name,
+	const pcl::PolygonMesh &mesh,
+	unsigned precision = 5);
 
 } // namespace util3d
 } // namespace rtabmap

corelib/src/util3d_surface.cpp

@@ -1454,6 +1454,7 @@ cv::Mat mergeTextures(
 		bool exposureFusion,
 		const ProgressState * state,
 		unsigned char blankValue,
+		bool clearVertexColorUnderTexture,
 		std::map<int, std::map<int, cv::Vec4d> > * gains,
 		std::map<int, std::map<int, cv::Mat> > * blendingGains,
 		std::pair<float, float> * contrastValues)
@@ -1483,6 +1484,7 @@ cv::Mat mergeTextures(
 			exposureFusion,
 			state,
 			blankValue,
+			clearVertexColorUnderTexture,
 			gains,
 			blendingGains,
 			contrastValues);
@@ -1506,6 +1508,7 @@ cv::Mat mergeTextures(
 		bool exposureFusion,
 		const ProgressState * state,
 		unsigned char blankValue,
+		bool clearVertexColorUnderTexture,
 		std::map<int, std::map<int, cv::Vec4d> > * gainsOut,
 		std::map<int, std::map<int, cv::Mat> > * blendingGainsOut,
 		std::pair<float, float> * contrastValuesOut)
@@ -2186,6 +2189,70 @@ cv::Mat mergeTextures(
 					if(state) state->callback(uFormat("Brightness and contrast auto %fs", timer.ticks()));
 				}
 			}
+			
+			// Cloud color
+			if(clearVertexColorUnderTexture)
+			{
+				int colorOffset = 0;
+				for(unsigned int i=0; i<mesh.cloud.fields.size(); ++i)
+				{
+					if(mesh.cloud.fields[i].name.compare("rgb") == 0)
+					{
+						colorOffset = mesh.cloud.fields[i].offset;
+						break;
+					}
+				}
+				if(colorOffset>0)
+				{
+					pcl::IndicesPtr notTexturedVertexIndices(new pcl::Indices());
+					UASSERT(mesh.tex_coordinates.size() == mesh.tex_polygons.size());
+					for(size_t t=0; t<mesh.tex_polygons.size(); ++t)
+					{
+						int pixelIndex = 0;
+						for(size_t p=0; p<mesh.tex_polygons[t].size(); ++p)
+						{
+							// only clear polygon color if all 3 vertices have valid tex coordinates.
+							bool valid = true;
+							for(size_t v=0; v<mesh.tex_polygons[t][p].vertices.size() && valid; ++v)
+							{
+								UASSERT(pixelIndex+v < mesh.tex_coordinates[t].size());
+								const Eigen::Vector2f & uv = mesh.tex_coordinates[t][pixelIndex+v];
+								if(uv[0] == -1 || uv[1] == -1)
+								{
+									valid = false;
+								}
+							}
+							if(!valid)
+							{
+								for(size_t v=0; v<mesh.tex_polygons[t][p].vertices.size(); ++v)
+								{
+									int vertex = mesh.tex_polygons[t][p].vertices[v];
+									notTexturedVertexIndices->push_back(vertex);
+								}
+							}
+							pixelIndex+=mesh.tex_polygons[t][p].vertices.size();
+						}
+					}
+					// Set up the full indices set
+					pcl::Indices full_indices (mesh.cloud.width * mesh.cloud.height);
+					for (size_t fii = 0; fii < full_indices.size(); ++fii)  // fii = full indices iterator
+						full_indices[fii] = fii;
+
+					// Set up the sorted input indices
+					std::sort (notTexturedVertexIndices->begin (), notTexturedVertexIndices->end ());
+
+					// Store the difference in indices
+					pcl::Indices texturedVertexIndices;
+					std::set_difference (full_indices.begin (), full_indices.end (), notTexturedVertexIndices->begin (), notTexturedVertexIndices->end (), std::inserter (texturedVertexIndices, texturedVertexIndices.begin ()));
+
+					for(size_t i=0; i<texturedVertexIndices.size(); ++i)
+					{
+						std::uint32_t white = 0xffffff;
+						UASSERT(texturedVertexIndices[i] * mesh.cloud.point_step + colorOffset < mesh.cloud.data.size());
+						memcpy(&mesh.cloud.data.data()[texturedVertexIndices[i] * mesh.cloud.point_step + colorOffset], reinterpret_cast<float*>(&white), sizeof(float));
+					}
+				}
+			}
 		}
 	}
 	UDEBUG("globalTextures=%d", globalTextures.cols?globalTextures.cols / globalTextures.rows:0);
@@ -3881,6 +3948,359 @@ bool intersectRayTriangle(
     return true;                    // I is in T
 }
 
+/**
+ * This is a modified copy of https://github.com/PointCloudLibrary/pcl/blob/pcl-1.12.1/io/src/obj_io.cpp
+ * with added color on each vertex if provided
+ */
+int saveOBJFile(
+	const std::string &file_name,
+	const pcl::TextureMesh &tex_mesh,
+	unsigned precision)
+{
+  if (tex_mesh.cloud.data.empty ())
+  {
+    UERROR ("Input point cloud has no data!\n");
+    return (-1);
+  }
+  // Open file
+  std::ofstream fs;
+  fs.precision (precision);
+  fs.open (file_name.c_str ());
+
+  // Define material file
+  std::string mtl_file_name = file_name.substr (0, file_name.find_last_of ('.')) + ".mtl";
+  // Strip path for "mtllib" command
+  std::string mtl_file_name_nopath = mtl_file_name;
+  mtl_file_name_nopath.erase (0, mtl_file_name.find_last_of ('/') + 1);
+
+  /* Write 3D information */
+  // number of points
+  unsigned nr_points  = tex_mesh.cloud.width * tex_mesh.cloud.height;
+  unsigned point_size = static_cast<unsigned> (tex_mesh.cloud.data.size () / nr_points);
+
+  // mesh size
+  unsigned nr_meshes = static_cast<unsigned> (tex_mesh.tex_polygons.size ());
+  // number of faces for header
+  unsigned nr_faces = 0;
+  for (unsigned m = 0; m < nr_meshes; ++m)
+    nr_faces += static_cast<unsigned> (tex_mesh.tex_polygons[m].size ());
+
+  // Write the header information
+  fs << "####" << '\n';
+  fs << "# OBJ dataFile simple version. File name: " << file_name << '\n';
+  fs << "# Vertices: " << nr_points << '\n';
+  fs << "# Faces: " <<nr_faces << '\n';
+  fs << "# Material information:" << '\n';
+  fs << "mtllib " << mtl_file_name_nopath << '\n';
+  fs << "####" << '\n';
+
+  // Write vertex coordinates
+  fs << "# Vertices" << '\n';
+  for (unsigned i = 0; i < nr_points; ++i)
+  {
+    int xyz = 0;
+    // "v" just be written one
+    bool v_written = false;
+    for (std::size_t d = 0; d < tex_mesh.cloud.fields.size (); ++d)
+    {
+      // adding vertex
+      if ((tex_mesh.cloud.fields[d].datatype == pcl::PCLPointField::FLOAT32) && (
+          tex_mesh.cloud.fields[d].name == "x" ||
+          tex_mesh.cloud.fields[d].name == "y" ||
+          tex_mesh.cloud.fields[d].name == "z"))
+      {
+        if (!v_written)
+        {
+           // write vertices beginning with v
+          fs << "v ";
+          v_written = true;
+        }
+        float value;
+        memcpy (&value, &tex_mesh.cloud.data[i * point_size + tex_mesh.cloud.fields[d].offset], sizeof (float));
+        fs << value;
+        if (++xyz == 3)
+          continue;
+        fs << " ";
+      }
+	  else if(tex_mesh.cloud.fields[d].datatype == pcl::PCLPointField::FLOAT32 &&
+          	  tex_mesh.cloud.fields[d].name == "rgb")
+	  {
+		std::uint32_t rgb = *reinterpret_cast<const int*>(&tex_mesh.cloud.data[i * point_size + tex_mesh.cloud.fields[d].offset]);
+		std::uint8_t r = (rgb >> 16) & 0x0000ff;
+		std::uint8_t g = (rgb >> 8)  & 0x0000ff;
+		std::uint8_t b = (rgb)       & 0x0000ff;
+		fs << " " << float(r)/255.0f << " " << float(g)/255.0f << " " << float(b)/255.0f;
+		break;
+	  }
+    }
+    if (xyz != 3)
+    {
+      UERROR ("Input point cloud has no XYZ data!\n");
+      return (-2);
+    }
+    fs << '\n';
+  }
+  fs << "# "<< nr_points <<" vertices" << '\n';
+
+  // Write vertex normals
+  for (unsigned i = 0; i < nr_points; ++i)
+  {
+    int xyz = 0;
+    // "vn" just be written one
+    bool v_written = false;
+    for (std::size_t d = 0; d < tex_mesh.cloud.fields.size (); ++d)
+    {
+      // adding vertex
+      if ((tex_mesh.cloud.fields[d].datatype == pcl::PCLPointField::FLOAT32) && (
+          tex_mesh.cloud.fields[d].name == "normal_x" ||
+          tex_mesh.cloud.fields[d].name == "normal_y" ||
+          tex_mesh.cloud.fields[d].name == "normal_z"))
+      {
+    	  if (!v_written)
+    	  {
+          // write vertices beginning with vn
+          fs << "vn ";
+          v_written = true;
+    	  }
+        float value;
+        memcpy (&value, &tex_mesh.cloud.data[i * point_size + tex_mesh.cloud.fields[d].offset], sizeof (float));
+        fs << value;
+        if (++xyz == 3)
+          break;
+        fs << " ";
+      }
+    }
+    if (xyz != 3)
+    {
+      UERROR ("Input point cloud has no normals!\n");
+      return (-2);
+    }
+    fs << '\n';
+  }
+  // Write vertex texture with "vt" (adding latter)
+
+  for (unsigned m = 0; m < nr_meshes; ++m)
+  {
+    fs << "# " << tex_mesh.tex_coordinates[m].size() << " vertex textures in submesh " << m <<  '\n';
+    for (const auto &coordinate : tex_mesh.tex_coordinates[m])
+    {
+      fs << "vt ";
+      fs <<  coordinate[0] << " " << coordinate[1] << '\n';
+    }
+  }
+
+  unsigned f_idx = 0;
+
+  // int idx_vt =0;
+  for (unsigned m = 0; m < nr_meshes; ++m)
+  {
+    if (m > 0) f_idx += static_cast<unsigned> (tex_mesh.tex_polygons[m-1].size ());
+
+    fs << "# The material will be used for mesh " << m << '\n';
+    fs << "usemtl " <<  tex_mesh.tex_materials[m].tex_name << '\n';
+    fs << "# Faces" << '\n';
+
+    for (std::size_t i = 0; i < tex_mesh.tex_polygons[m].size(); ++i)
+    {
+      // Write faces with "f"
+      fs << "f";
+      // There's one UV per vertex per face, i.e., the same vertex can have
+      // different UV depending on the face.
+      for (std::size_t j = 0; j < tex_mesh.tex_polygons[m][i].vertices.size (); ++j)
+      {
+        std::uint32_t idx = tex_mesh.tex_polygons[m][i].vertices[j] + 1;
+        fs << " " << idx
+           << "/" << tex_mesh.tex_polygons[m][i].vertices.size () * (i+f_idx) +j+1
+           << "/" << idx; // vertex index in obj file format starting with 1
+      }
+      fs << '\n';
+    }
+    fs << "# "<< tex_mesh.tex_polygons[m].size() << " faces in mesh " << m << '\n';
+  }
+  fs << "# End of File" << std::flush;
+
+  // Close obj file
+  fs.close ();
+
+  /* Write material definition for OBJ file*/
+  // Open file
+
+  std::ofstream m_fs;
+  m_fs.precision (precision);
+  m_fs.open (mtl_file_name.c_str ());
+
+  // default
+  m_fs << "#" << '\n';
+  m_fs << "# Wavefront material file" << '\n';
+  m_fs << "#" << '\n';
+  for(unsigned m = 0; m < nr_meshes; ++m)
+  {
+    m_fs << "newmtl " << tex_mesh.tex_materials[m].tex_name << '\n';
+    m_fs << "Ka "<< tex_mesh.tex_materials[m].tex_Ka.r << " " << tex_mesh.tex_materials[m].tex_Ka.g << " " << tex_mesh.tex_materials[m].tex_Ka.b << '\n'; // defines the ambient color of the material to be (r,g,b).
+    m_fs << "Kd "<< tex_mesh.tex_materials[m].tex_Kd.r << " " << tex_mesh.tex_materials[m].tex_Kd.g << " " << tex_mesh.tex_materials[m].tex_Kd.b << '\n'; // defines the diffuse color of the material to be (r,g,b).
+    m_fs << "Ks "<< tex_mesh.tex_materials[m].tex_Ks.r << " " << tex_mesh.tex_materials[m].tex_Ks.g << " " << tex_mesh.tex_materials[m].tex_Ks.b << '\n'; // defines the specular color of the material to be (r,g,b). This color shows up in highlights.
+    m_fs << "d " << tex_mesh.tex_materials[m].tex_d << '\n'; // defines the transparency of the material to be alpha.
+    m_fs << "Ns "<< tex_mesh.tex_materials[m].tex_Ns  << '\n'; // defines the shininess of the material to be s.
+    m_fs << "illum "<< tex_mesh.tex_materials[m].tex_illum << '\n'; // denotes the illumination model used by the material.
+                                            // illum = 1 indicates a flat material with no specular highlights, so the value of Ks is not used.
+                                            // illum = 2 denotes the presence of specular highlights, and so a specification for Ks is required.
+    m_fs << "map_Kd " << tex_mesh.tex_materials[m].tex_file << '\n';
+    m_fs << "###" << '\n';
+  }
+  m_fs.close ();
+  return (0);
+}
+
+/**
+ * This is a modified copy of https://github.com/PointCloudLibrary/pcl/blob/pcl-1.12.1/io/src/obj_io.cpp
+ * with added color on each vertex if provided
+ */
+int saveOBJFile(
+	const std::string &file_name,
+	const pcl::PolygonMesh &mesh,
+	unsigned precision)
+{
+  if (mesh.cloud.data.empty ())
+  {
+    UERROR ("Input point cloud has no data!\n");
+    return (-1);
+  }
+  // Open file
+  std::ofstream fs;
+  fs.precision (precision);
+  fs.open (file_name.c_str ());
+
+  /* Write 3D information */
+  // number of points
+  int nr_points  = mesh.cloud.width * mesh.cloud.height;
+  // point size
+  unsigned point_size = static_cast<unsigned> (mesh.cloud.data.size () / nr_points);
+  // number of faces for header
+  unsigned nr_faces = static_cast<unsigned> (mesh.polygons.size ());
+  // Do we have vertices normals?
+  int normal_index = getFieldIndex (mesh.cloud, "normal_x");
+
+  // Write the header information
+  fs << "####" << '\n';
+  fs << "# OBJ dataFile simple version. File name: " << file_name << '\n';
+  fs << "# Vertices: " << nr_points << '\n';
+  if (normal_index != -1)
+    fs << "# Vertices normals : " << nr_points << '\n';
+  fs << "# Faces: " <<nr_faces << '\n';
+  fs << "####" << '\n';
+
+  // Write vertex coordinates
+  fs << "# List of Vertices, with (x,y,z) coordinates, w is optional." << '\n';
+  for (int i = 0; i < nr_points; ++i)
+  {
+    int xyz = 0;
+    for (std::size_t d = 0; d < mesh.cloud.fields.size (); ++d)
+    {
+      // adding vertex
+      if ((mesh.cloud.fields[d].datatype == pcl::PCLPointField::FLOAT32) && (
+          mesh.cloud.fields[d].name == "x" ||
+          mesh.cloud.fields[d].name == "y" ||
+          mesh.cloud.fields[d].name == "z"))
+      {
+        if (mesh.cloud.fields[d].name == "x")
+           // write vertices beginning with v
+          fs << "v ";
+
+        float value;
+        memcpy (&value, &mesh.cloud.data[i * point_size + mesh.cloud.fields[d].offset], sizeof (float));
+        fs << value;
+        if (++xyz == 3)
+          continue;
+        fs << " ";
+      }
+	  else if(mesh.cloud.fields[d].datatype == pcl::PCLPointField::FLOAT32 &&
+          	  mesh.cloud.fields[d].name == "rgb")
+	  {
+		std::uint32_t rgb = *reinterpret_cast<const int*>(&mesh.cloud.data[i * point_size + mesh.cloud.fields[d].offset]);
+		std::uint8_t r = (rgb >> 16) & 0x0000ff;
+		std::uint8_t g = (rgb >> 8)  & 0x0000ff;
+		std::uint8_t b = (rgb)       & 0x0000ff;
+		fs << " " << float(r)/255.0f << " " << float(g)/255.0f << " " << float(b)/255.0f;
+		break;
+	  }
+    }
+    if (xyz != 3)
+    {
+      UERROR ("Input point cloud has no XYZ data!\n");
+      return (-2);
+    }
+    fs << '\n';
+  }
+
+  fs << "# "<< nr_points <<" vertices" << '\n';
+
+  if(normal_index != -1)
+  {
+    fs << "# Normals in (x,y,z) form; normals might not be unit." <<  '\n';
+    // Write vertex normals
+    for (int i = 0; i < nr_points; ++i)
+    {
+      int nxyz = 0;
+      for (std::size_t d = 0; d < mesh.cloud.fields.size (); ++d)
+      {
+        // adding vertex
+        if ((mesh.cloud.fields[d].datatype == pcl::PCLPointField::FLOAT32) && (
+              mesh.cloud.fields[d].name == "normal_x" ||
+              mesh.cloud.fields[d].name == "normal_y" ||
+              mesh.cloud.fields[d].name == "normal_z"))
+        {
+          if (mesh.cloud.fields[d].name == "normal_x")
+            // write vertices beginning with vn
+            fs << "vn ";
+
+          float value;
+          memcpy (&value, &mesh.cloud.data[i * point_size + mesh.cloud.fields[d].offset], sizeof (float));
+          fs << value;
+          if (++nxyz == 3)
+            break;
+          fs << " ";
+        }
+      }
+      if (nxyz != 3)
+      {
+        UERROR ("Input point cloud has no normals!\n");
+        return (-2);
+      }
+      fs << '\n';
+    }
+
+    fs << "# "<< nr_points <<" vertices normals" << '\n';
+  }
+
+  fs << "# Face Definitions" << '\n';
+  // Write down faces
+  if(normal_index == -1)
+  {
+    for(unsigned i = 0; i < nr_faces; i++)
+    {
+      fs << "f ";      
+      for (std::size_t j = 0; j < mesh.polygons[i].vertices.size () - 1; ++j)
+        fs << mesh.polygons[i].vertices[j] + 1 << " ";
+      fs << mesh.polygons[i].vertices.back() + 1 << '\n';
+    }
+  }
+  else
+  {
+    for(unsigned i = 0; i < nr_faces; i++)
+    {
+      fs << "f ";
+      for (std::size_t j = 0; j < mesh.polygons[i].vertices.size () - 1; ++j)
+        fs << mesh.polygons[i].vertices[j] + 1 << "//" << mesh.polygons[i].vertices[j] + 1 << " ";
+      fs << mesh.polygons[i].vertices.back() + 1 << "//" << mesh.polygons[i].vertices.back() + 1 << '\n';
+    }
+  }
+  fs << "# End of File" << std::endl;
+
+  // Close obj file
+  fs.close ();
+  return 0;
+}
+
 }
 
 }

guilib/include/rtabmap/gui/CloudViewer.h

@@ -350,6 +350,7 @@ public:
 	void setLighting(bool on);
 	void setShading(bool on);
 	void setEdgeVisibility(bool visible);
+	void setScalarVisibility(bool visible);
 	void setInteractorLayer(int layer);
 
 	bool isBackfaceCulling() const;
@@ -459,6 +460,7 @@ private:
     QAction * _aSetLighting;
     QAction * _aSetFlatShading;
     QAction * _aSetEdgeVisibility;
+	QAction * _aSetScalarVisibility;
     QAction * _aBackfaceCulling;
     QAction * _aPolygonPicking;
     QMenu * _menu;

guilib/include/rtabmap/gui/CloudViewerInteractorStyle.h

@@ -32,6 +32,7 @@ public:
 protected:
 	virtual void OnMouseMove();
 	virtual void OnLeftButtonDown();
+	virtual void OnRightButtonDown();
 
 protected:
 	friend class CloudViewer;

guilib/src/CloudViewer.cpp

@@ -80,6 +80,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #include <vtkObjectFactory.h>
 #include <vtkQuad.h>
 #include <vtkWarpScalar.h>
+#include <vtkUnsignedCharArray.h>
 #include <opencv/vtkImageMatSource.h>
 
 #if VTK_MAJOR_VERSION >= 7
@@ -96,6 +97,11 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #include <rtabmap/core/global_map/OctoMap.h>
 #endif
 
+// For compatibility with new VTK generic data arrays.
+#ifdef vtkGenericDataArray_h
+#define InsertNextTupleValue InsertNextTypedTuple
+#endif
+
 namespace rtabmap {
 
 CloudViewer::CloudViewer(QWidget *parent, CloudViewerInteractorStyle * style) :
@@ -124,6 +130,7 @@ CloudViewer::CloudViewer(QWidget *parent, CloudViewerInteractorStyle * style) :
 		_aSetLighting(0),
 		_aSetFlatShading(0),
 		_aSetEdgeVisibility(0),
+		_aSetScalarVisibility(0),
 		_aBackfaceCulling(0),
 		_menu(0),
 		_trajectory(new pcl::PointCloud<pcl::PointXYZ>),
@@ -347,6 +354,9 @@ void CloudViewer::createMenu()
 	_aSetEdgeVisibility = new QAction("Show edges", this);
 	_aSetEdgeVisibility->setCheckable(true);
 	_aSetEdgeVisibility->setChecked(false);
+	_aSetScalarVisibility = new QAction("Show vertex colors", this);
+	_aSetScalarVisibility->setCheckable(true);
+	_aSetScalarVisibility->setChecked(true);
 	_aBackfaceCulling = new QAction("Backface culling", this);
 	_aBackfaceCulling->setCheckable(true);
 	_aBackfaceCulling->setChecked(true);
@@ -408,6 +418,7 @@ void CloudViewer::createMenu()
 	_menu->addAction(_aSetLighting);
 	_menu->addAction(_aSetFlatShading);
 	_menu->addAction(_aSetEdgeVisibility);
+	_menu->addAction(_aSetScalarVisibility);
 	_menu->addAction(_aBackfaceCulling);
 	_menu->addAction(_aPolygonPicking);
 }
@@ -1408,10 +1419,20 @@ bool CloudViewer::addTextureMesh (
   // Create points from mesh.cloud
   vtkSmartPointer<vtkPoints> poly_points = vtkSmartPointer<vtkPoints>::New ();
   vtkSmartPointer<vtkUnsignedCharArray> colors = vtkSmartPointer<vtkUnsignedCharArray>::New ();
-  bool has_color = false;
+  colors->SetNumberOfComponents(3);
+  colors->SetName ("Colors");
+  bool hasColors = false;
+  for(unsigned int i=0; i<mesh.cloud.fields.size(); ++i)
+  {
+	if(mesh.cloud.fields[i].name.compare("rgb") == 0)
+	{
+		hasColors = true;
+		break;
+	}
+  }
   vtkSmartPointer<vtkMatrix4x4> transformation = vtkSmartPointer<vtkMatrix4x4>::New ();
   
-    pcl::PointCloud<pcl::PointXYZ>::Ptr cloud (new pcl::PointCloud<pcl::PointXYZ> ());
+    pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud (new pcl::PointCloud<pcl::PointXYZRGB> ());
     pcl::fromPCLPointCloud2 (mesh.cloud, *cloud);
     // no points --> exit
     if (cloud->points.size () == 0)
@@ -1423,8 +1444,17 @@ bool CloudViewer::addTextureMesh (
     poly_points->SetNumberOfPoints (cloud->points.size ());
     for (std::size_t i = 0; i < cloud->points.size (); ++i)
     {
-      const pcl::PointXYZ &p = cloud->points[i];
+      const pcl::PointXYZRGB &p = cloud->points[i];
       poly_points->InsertPoint (i, p.x, p.y, p.z);
+
+	  if(hasColors) {
+	  	unsigned char color[3] = {p.r, p.g, p.b}; 
+#if VTK_MAJOR_VERSION > 7 || (VTK_MAJOR_VERSION==7 && VTK_MINOR_VERSION >= 1)
+	  	colors->InsertNextTypedTuple(color);
+#else
+	  	colors->InsertNextTupleValue(color);
+#endif
+	  }
     }
 
   //create polys from polyMesh.tex_polygons
@@ -1443,8 +1473,9 @@ bool CloudViewer::addTextureMesh (
   vtkSmartPointer<vtkPolyData> polydata = vtkSmartPointer<vtkPolyData>::New();
   polydata->SetPolys (polys);
   polydata->SetPoints (poly_points);
-  if (has_color)
+  if (hasColors) {
     polydata->GetPointData()->SetScalars(colors);
+  }
 
   vtkSmartPointer<vtkPolyDataMapper> mapper = vtkSmartPointer<vtkPolyDataMapper>::New ();
 #if VTK_MAJOR_VERSION < 6
@@ -1524,6 +1555,7 @@ bool CloudViewer::addTextureMesh (
   actor->GetProperty()->SetEdgeVisibility(_aSetEdgeVisibility->isChecked());
   actor->GetProperty()->SetBackfaceCulling(_aBackfaceCulling->isChecked());
   actor->GetProperty()->SetFrontfaceCulling(_frontfaceCulling);
+  actor->GetMapper()->SetScalarVisibility(_aSetScalarVisibility->isChecked());
   return true;
 }
 
@@ -2892,6 +2924,21 @@ void CloudViewer::setEdgeVisibility(bool visible)
 	this->refreshView();
 }
 
+void CloudViewer::setScalarVisibility(bool visible)
+{
+	_aSetScalarVisibility->setChecked(visible);
+	pcl::visualization::ShapeActorMapPtr shapeActorMap = _visualizer->getShapeActorMap();
+	for(pcl::visualization::ShapeActorMap::iterator iter=shapeActorMap->begin(); iter!=shapeActorMap->end(); ++iter)
+	{
+		vtkActor* actor = vtkActor::SafeDownCast (iter->second);
+		if(actor && _addedClouds.contains(iter->first))
+		{
+			actor->GetMapper()->SetScalarVisibility(_aSetScalarVisibility->isChecked());
+		}
+	}
+	this->refreshView();
+}
+
 void CloudViewer::setInteractorLayer(int layer)
 {
 	_visualizer->getRendererCollection()->InitTraversal ();
@@ -3983,6 +4030,10 @@ void CloudViewer::handleAction(QAction * a)
 	{
 		this->setEdgeVisibility(_aSetEdgeVisibility->isChecked());
 	}
+	else if(a == _aSetScalarVisibility)
+	{
+		this->setScalarVisibility(_aSetScalarVisibility->isChecked());
+	}
 	else if(a == _aBackfaceCulling)
 	{
 		this->setBackfaceCulling(_aBackfaceCulling->isChecked(), _frontfaceCulling);

guilib/src/CloudViewerInteractorStyle.cpp

@@ -333,4 +333,9 @@ void CloudViewerInteractorStyle::OnLeftButtonDown()
 	PCLVisualizerInteractorStyle::OnLeftButtonDown();
 }
 
+void CloudViewerInteractorStyle::OnRightButtonDown()
+{
+	// ignore
+}
+
 } /* namespace rtabmap */

guilib/src/ExportCloudsDialog.cpp

@@ -240,6 +240,8 @@ ExportCloudsDialog::ExportCloudsDialog(QWidget *parent) :
 	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_distanceToCamPolicy, SIGNAL(stateChanged(int)), this, SIGNAL(configChanged()));
+	connect(_ui->comboBox_texturingColorPolicy, SIGNAL(currentIndexChanged(int)), this, SIGNAL(configChanged()));
 	connect(_ui->checkBox_cameraFilter, SIGNAL(stateChanged(int)), this, SIGNAL(configChanged()));
 	connect(_ui->checkBox_cameraFilter, SIGNAL(stateChanged(int)), this, SLOT(updateReconstructionFlavor()));
 	connect(_ui->doubleSpinBox_cameraFilterRadius, SIGNAL(valueChanged(double)), this, SIGNAL(configChanged()));
@@ -467,6 +469,7 @@ void ExportCloudsDialog::saveSettings(QSettings & settings, const QString & grou
 	settings.setValue("mesh_textureMinCluster", _ui->spinBox_mesh_minTextureClusterSize->value());
 	settings.setValue("mesh_textureRoiRatios", _ui->lineEdit_meshingTextureRoiRatios->text());
 	settings.setValue("mesh_textureDistanceToCamPolicy", _ui->checkBox_distanceToCamPolicy->isChecked());
+	settings.setValue("mesh_textureVertexColorPolicy", _ui->comboBox_texturingColorPolicy->currentIndex());
 	settings.setValue("mesh_textureCameraFiltering", _ui->checkBox_cameraFilter->isChecked());
 	settings.setValue("mesh_textureCameraFilteringRadius", _ui->doubleSpinBox_cameraFilterRadius->value());
 	settings.setValue("mesh_textureCameraFilteringAngle", _ui->doubleSpinBox_cameraFilterAngle->value());
@@ -651,6 +654,7 @@ void ExportCloudsDialog::loadSettings(QSettings & settings, const QString & grou
 	_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_distanceToCamPolicy->setChecked(settings.value("mesh_textureDistanceToCamPolicy", _ui->checkBox_distanceToCamPolicy->isChecked()).toBool());
+	_ui->comboBox_texturingColorPolicy->setCurrentIndex(settings.value("mesh_textureVertexColorPolicy", _ui->comboBox_texturingColorPolicy->currentIndex()).toInt());
 	_ui->checkBox_cameraFilter->setChecked(settings.value("mesh_textureCameraFiltering", _ui->checkBox_cameraFilter->isChecked()).toBool());
 	_ui->doubleSpinBox_cameraFilterRadius->setValue(settings.value("mesh_textureCameraFilteringRadius", _ui->doubleSpinBox_cameraFilterRadius->value()).toDouble());
 	_ui->doubleSpinBox_cameraFilterAngle->setValue(settings.value("mesh_textureCameraFilteringAngle", _ui->doubleSpinBox_cameraFilterAngle->value()).toDouble());
@@ -832,6 +836,7 @@ void ExportCloudsDialog::restoreDefaults()
 	_ui->spinBox_mesh_minTextureClusterSize->setValue(50);
 	_ui->lineEdit_meshingTextureRoiRatios->setText("0.0 0.0 0.0 0.0");
 	_ui->checkBox_distanceToCamPolicy->setChecked(false);
+	_ui->comboBox_texturingColorPolicy->setCurrentIndex(0);
 	_ui->checkBox_cameraFilter->setChecked(false);
 	_ui->doubleSpinBox_cameraFilterRadius->setValue(0);
 	_ui->doubleSpinBox_cameraFilterAngle->setValue(30);
@@ -1352,7 +1357,10 @@ void ExportCloudsDialog::viewClouds(
 							blendingDecimation,
 							_ui->spinBox_textureBrightnessContrastRatioLow->value(),
 							_ui->spinBox_textureBrightnessContrastRatioHigh->value(),
-							_ui->checkBox_exposureFusion->isEnabled() && _ui->checkBox_exposureFusion->isChecked());
+							_ui->checkBox_exposureFusion->isEnabled() && _ui->checkBox_exposureFusion->isChecked(),
+							0,
+							255,
+							_ui->comboBox_texturingColorPolicy->currentIndex() == 1);
 					if(globalTextures.rows == globalTextures.cols)
 					{
 						globalTexture = globalTextures;
@@ -1362,44 +1370,112 @@ void ExportCloudsDialog::viewClouds(
 				_progressDialog->appendText(tr("Viewing the mesh %1 (%2 polygons)...").arg(iter->first).arg(mesh->tex_polygons.size()?mesh->tex_polygons[0].size():0));
 				_progressDialog->incrementStep();
 
+				bool hasColors = false;
+				for(unsigned int i=0; i<mesh->cloud.fields.size(); ++i)
+				{
+					if(mesh->cloud.fields[i].name.compare("rgb") == 0)
+					{
+						hasColors = true;
+					}
+				}
+
 				// VTK issue:
 				//  tex_coordinates should be linked to points, not
 				//  polygon vertices. Points linked to multiple different TCoords (different textures) should
 				//  be duplicated.
-				for (unsigned int t = 0; t < mesh->tex_coordinates.size(); ++t)
+				if(hasColors)
 				{
-					if(mesh->tex_polygons[t].size())
+					for (unsigned int t = 0; t < mesh->tex_coordinates.size(); ++t)
 					{
-
+						if(mesh->tex_polygons[t].size())
-						pcl::PointCloud<pcl::PointXYZ>::Ptr originalCloud(new pcl::PointCloud<pcl::PointXYZ>);
-						pcl::fromPCLPointCloud2(mesh->cloud, *originalCloud);
-
-						// make a cloud with as many points than polygon vertices
-						unsigned int nPoints = mesh->tex_coordinates[t].size();
-						UASSERT(nPoints == mesh->tex_polygons[t].size()*mesh->tex_polygons[t][0].vertices.size()); // assuming polygon size is constant!
-
-						pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>);
-						cloud->resize(nPoints);
-
-						unsigned int oi = 0;
-						for (unsigned int i = 0; i < mesh->tex_polygons[t].size(); ++i)
 						{
-							pcl::Vertices & vertices = mesh->tex_polygons[t][i];
 
-							for(unsigned int j=0; j<vertices.vertices.size(); ++j)
+							pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr originalCloud(new pcl::PointCloud<pcl::PointXYZRGBNormal>);
+							pcl::fromPCLPointCloud2(mesh->cloud, *originalCloud);
+
+							// make a cloud with as many points than polygon vertices
+							unsigned int nPoints = mesh->tex_coordinates[t].size();
+							UASSERT(nPoints == mesh->tex_polygons[t].size()*mesh->tex_polygons[t][0].vertices.size()); // assuming polygon size is constant!
+
+							pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZRGBNormal>);
+							cloud->resize(nPoints);
+
+							unsigned int oi = 0;
+							size_t texCoordIndex = 0;
+							for (unsigned int i = 0; i < mesh->tex_polygons[t].size(); ++i)
 							{
-								UASSERT(oi < cloud->size());
+								pcl::Vertices & vertices = mesh->tex_polygons[t][i];
-								UASSERT_MSG((int)vertices.vertices[j] < (int)originalCloud->size(), uFormat("%d vs %d", vertices.vertices[j], (int)originalCloud->size()).c_str());
+								bool hasTexture = true;
-								cloud->at(oi) = originalCloud->at(vertices.vertices[j]);
+								for(unsigned int j=0; j<vertices.vertices.size(); ++j)
-								vertices.vertices[j] = oi; // new vertice index
+								{
-								++oi;
+									if(mesh->tex_coordinates[t][texCoordIndex][0] == -1 || mesh->tex_coordinates[t][texCoordIndex][1] == -1)
+									{
+										hasTexture = false;
+										break;
+									}
+								}
+								
+								for(unsigned int j=0; j<vertices.vertices.size(); ++j)
+								{
+									UASSERT(oi < cloud->size());
+									UASSERT_MSG((int)vertices.vertices[j] < (int)originalCloud->size(), uFormat("%d vs %d", vertices.vertices[j], (int)originalCloud->size()).c_str());
+									cloud->at(oi) = originalCloud->at(vertices.vertices[j]);
+									if(hasTexture && _ui->comboBox_texturingColorPolicy->currentIndex() == 1) {
+										cloud->at(oi).r = 255;
+										cloud->at(oi).g = 255;
+										cloud->at(oi).b = 255;
+									}
+
+									vertices.vertices[j] = oi; // new vertice index
+									++oi;
+								}
+								texCoordIndex+=vertices.vertices.size();
 							}
+							pcl::toPCLPointCloud2(*cloud, mesh->cloud);
+						}
+						else
+						{
+							UWARN("No polygons for texture %d of mesh %d?!", t, iter->first);
 						}
-						pcl::toPCLPointCloud2(*cloud, mesh->cloud);
 					}
-					else
+				}
+				else
+				{
+					for (unsigned int t = 0; t < mesh->tex_coordinates.size(); ++t)
 					{
-						UWARN("No polygons for texture %d of mesh %d?!", t, iter->first);
+						if(mesh->tex_polygons[t].size())
+						{
+
+							pcl::PointCloud<pcl::PointNormal>::Ptr originalCloud(new pcl::PointCloud<pcl::PointNormal>);
+							pcl::fromPCLPointCloud2(mesh->cloud, *originalCloud);
+
+							// make a cloud with as many points than polygon vertices
+							unsigned int nPoints = mesh->tex_coordinates[t].size();
+							UASSERT(nPoints == mesh->tex_polygons[t].size()*mesh->tex_polygons[t][0].vertices.size()); // assuming polygon size is constant!
+
+							pcl::PointCloud<pcl::PointNormal>::Ptr cloud(new pcl::PointCloud<pcl::PointNormal>);
+							cloud->resize(nPoints);
+
+							unsigned int oi = 0;
+							for (unsigned int i = 0; i < mesh->tex_polygons[t].size(); ++i)
+							{
+								pcl::Vertices & vertices = mesh->tex_polygons[t][i];
+
+								for(unsigned int j=0; j<vertices.vertices.size(); ++j)
+								{
+									UASSERT(oi < cloud->size());
+									UASSERT_MSG((int)vertices.vertices[j] < (int)originalCloud->size(), uFormat("%d vs %d", vertices.vertices[j], (int)originalCloud->size()).c_str());
+									cloud->at(oi) = originalCloud->at(vertices.vertices[j]);
+									vertices.vertices[j] = oi; // new vertice index
+									++oi;
+								}
+							}
+							pcl::toPCLPointCloud2(*cloud, mesh->cloud);
+						}
+						else
+						{
+							UWARN("No polygons for texture %d of mesh %d?!", t, iter->first);
+						}
 					}
 				}
 
@@ -2682,7 +2758,7 @@ bool ExportCloudsDialog::getExportedClouds(
 								_ui->spinBox_meshMaxPolygons->isEnabled()?_ui->spinBox_meshMaxPolygons->value():0,
 								iter->second,
 								(float)_ui->doubleSpinBox_transferColorRadius->value(),
-								!(_ui->checkBox_textureMapping->isEnabled() && _ui->checkBox_textureMapping->isChecked()),
+								!(_ui->checkBox_textureMapping->isEnabled() && _ui->checkBox_textureMapping->isChecked() && _ui->comboBox_texturingColorPolicy->currentIndex()==0),
 								_ui->checkBox_cleanMesh->isChecked(),
 								_ui->spinBox_mesh_minClusterSize->value(),
 								&texturingState);
@@ -3068,7 +3144,7 @@ bool ExportCloudsDialog::getExportedClouds(
 						_ui->spinBox_meshMaxPolygons->isEnabled()?_ui->spinBox_meshMaxPolygons->value():0,
 						vertices,
 						(float)_ui->doubleSpinBox_transferColorRadius->value(),
-						!(_ui->checkBox_textureMapping->isEnabled() && _ui->checkBox_textureMapping->isChecked()),
+						!(_ui->checkBox_textureMapping->isEnabled() && _ui->checkBox_textureMapping->isChecked() && _ui->comboBox_texturingColorPolicy->currentIndex()==0),
 						_ui->checkBox_cleanMesh->isChecked(),
 						_ui->spinBox_mesh_minClusterSize->value(),
 						&texturingState);
@@ -3140,7 +3216,7 @@ bool ExportCloudsDialog::getExportedClouds(
 						_ui->spinBox_meshMaxPolygons->isEnabled()?_ui->spinBox_meshMaxPolygons->value():0,
 						mergedClouds,
 						(float)_ui->doubleSpinBox_transferColorRadius->value(),
-						!(_ui->checkBox_textureMapping->isEnabled() && _ui->checkBox_textureMapping->isChecked()),
+						!(_ui->checkBox_textureMapping->isEnabled() && _ui->checkBox_textureMapping->isChecked() && _ui->comboBox_texturingColorPolicy->currentIndex()==0),
 						_ui->checkBox_cleanMesh->isChecked(),
 						_ui->spinBox_mesh_minClusterSize->value(),
 						&texturingState);
@@ -4697,7 +4773,8 @@ void ExportCloudsDialog::saveTextureMeshes(
 							_ui->spinBox_textureBrightnessContrastRatioHigh->value(),
 							_ui->checkBox_exposureFusion->isEnabled() && _ui->checkBox_exposureFusion->isChecked(),
 							0,
-							0,
+							255,
+							_ui->comboBox_texturingColorPolicy->currentIndex() == 1,
 							&gains,
 							&blendingGains,
 							&contrastValues);
@@ -4939,7 +5016,10 @@ void ExportCloudsDialog::saveTextureMeshes(
 									blendingDecimation,
 									_ui->spinBox_textureBrightnessContrastRatioLow->value(),
 									_ui->spinBox_textureBrightnessContrastRatioHigh->value(),
-									_ui->checkBox_exposureFusion->isEnabled() && _ui->checkBox_exposureFusion->isChecked());
+									_ui->checkBox_exposureFusion->isEnabled() && _ui->checkBox_exposureFusion->isChecked(),
+									0,
+									255,
+									_ui->comboBox_texturingColorPolicy->currentIndex() == 1);
 						}
 						bool singleTexture = mesh->tex_materials.size() == 1;
 						if(!singleTexture)
@@ -5114,28 +5194,31 @@ void ExportCloudsDialog::saveTextureMeshes(
 	}
 }
 
-    bool ExportCloudsDialog::saveOBJFile(const QString &path, pcl::TextureMesh::Ptr &mesh) const {
+bool ExportCloudsDialog::saveOBJFile(const QString &path, pcl::TextureMesh::Ptr &mesh) const {
-#if PCL_VERSION_COMPARE(>=, 1, 13, 0)
-        mesh->tex_coord_indices = std::vector<std::vector<pcl::Vertices>>();
-        auto nr_meshes = static_cast<unsigned>(mesh->tex_polygons.size());
-        unsigned f_idx = 0;
-        for (unsigned m = 0; m < nr_meshes; m++) {
-            std::vector<pcl::Vertices> ci = mesh->tex_polygons[m];
-            for(std::size_t i = 0; i < ci.size(); i++) {
-                for (std::size_t j = 0; j < ci[i].vertices.size(); j++) {
-                    ci[i].vertices[j] = ci[i].vertices.size() * (i + f_idx) + j;
-                }
-            }
-            mesh->tex_coord_indices.push_back(ci);
-            f_idx += static_cast<unsigned>(mesh->tex_polygons[m].size());
-        }
-#endif
-        return pcl::io::saveOBJFile(path.toStdString(), *mesh) == 0;
-    }
 
-    bool ExportCloudsDialog::saveOBJFile(const QString &path, pcl::PolygonMesh &mesh) const {
+	
-        return pcl::io::saveOBJFile(path.toStdString(), mesh) == 0;
+
-    }
+#if PCL_VERSION_COMPARE(>=, 1, 13, 0)
+	mesh->tex_coord_indices = std::vector<std::vector<pcl::Vertices>>();
+	auto nr_meshes = static_cast<unsigned>(mesh->tex_polygons.size());
+	unsigned f_idx = 0;
+	for (unsigned m = 0; m < nr_meshes; m++) {
+		std::vector<pcl::Vertices> ci = mesh->tex_polygons[m];
+		for(std::size_t i = 0; i < ci.size(); i++) {
+			for (std::size_t j = 0; j < ci[i].vertices.size(); j++) {
+				ci[i].vertices[j] = ci[i].vertices.size() * (i + f_idx) + j;
+			}
+		}
+		mesh->tex_coord_indices.push_back(ci);
+		f_idx += static_cast<unsigned>(mesh->tex_polygons[m].size());
+	}
+#endif
+	return util3d::saveOBJFile(path.toStdString(), *mesh) == 0;
+}
+
+bool ExportCloudsDialog::saveOBJFile(const QString &path, pcl::PolygonMesh &mesh) const {
+	return util3d::saveOBJFile(path.toStdString(), mesh) == 0;
+}
 
 
 }

guilib/src/ui/exportCloudsDialog.ui

@@ -23,9 +23,9 @@
       <property name="geometry">
        <rect>
         <x>0</x>
-        <y>-2995</y>
+        <y>-3199</y>
         <width>885</width>
-        <height>6152</height>
+        <height>6183</height>
        </rect>
       </property>
       <layout class="QVBoxLayout" name="verticalLayout_13">
@@ -2438,20 +2438,106 @@ By Node ID and Camera Index: NodeID*10+CameraIndex</string>
             <layout class="QVBoxLayout" name="verticalLayout_16">
              <item>
               <layout class="QGridLayout" name="gridLayout_15" columnstretch="0,1">
-               <item row="13" column="0">
+               <item row="11" column="0">
-                <widget class="QSpinBox" name="spinBox_textureBrightnessContrastRatioHigh">
+                <widget class="QCheckBox" name="checkBox_blending">
+                 <property name="text">
+                  <string/>
+                 </property>
+                </widget>
+               </item>
+               <item row="10" column="1">
+                <widget class="QLabel" name="label_meshingTextureSize_3">
+                 <property name="text">
+                  <string>Camera filtering. Some criteria to select which cameras are used for texturing.</string>
+                 </property>
+                 <property name="wordWrap">
+                  <bool>true</bool>
+                 </property>
+                </widget>
+               </item>
+               <item row="4" column="0">
+                <widget class="QDoubleSpinBox" name="doubleSpinBox_meshingTextureMaxDistance">
                  <property name="suffix">
-                  <string> %</string>
+                  <string> m</string>
+                 </property>
+                 <property name="decimals">
+                  <number>1</number>
                  </property>
                  <property name="minimum">
-                  <number>0</number>
+                  <double>0.000000000000000</double>
                  </property>
                  <property name="maximum">
-                  <number>49</number>
+                  <double>99.000000000000000</double>
+                 </property>
+                 <property name="singleStep">
+                  <double>1.000000000000000</double>
+                 </property>
+                 <property name="value">
+                  <double>3.000000000000000</double>
+                 </property>
+                </widget>
+               </item>
+               <item row="0" column="1">
+                <widget class="QLabel" name="label_meshingTextureFormat">
+                 <property name="text">
+                  <string>Texture format.</string>
+                 </property>
+                 <property name="wordWrap">
+                  <bool>true</bool>
+                 </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="16" 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>
+                 </property>
+                 <property name="wordWrap">
+                  <bool>true</bool>
                  </property>
                 </widget>
                </item>
                <item row="13" 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>
+                 </property>
+                 <property name="wordWrap">
+                  <bool>true</bool>
+                 </property>
+                </widget>
+               </item>
+               <item row="4" column="1">
+                <widget class="QLabel" name="label_meshingTextureSize_2">
+                 <property name="text">
+                  <string>Maximum distance from the camera for polygons to be textured by this camera (0 means infinite). Only used with dense reconstruction flavor.</string>
+                 </property>
+                 <property name="wordWrap">
+                  <bool>true</bool>
+                 </property>
+                </widget>
+               </item>
+               <item row="8" column="1">
+                <widget class="QLabel" name="label_meshingTextureSize_11">
+                 <property name="text">
+                  <string>Distance to camera policy. The closest camera from a polygon is used to texture the polygon. If disabled, the camera for which the polygon projection is the closest of the image center is used to texture the polygon.</string>
+                 </property>
+                 <property name="wordWrap">
+                  <bool>true</bool>
+                 </property>
+                </widget>
+               </item>
+               <item row="14" 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>
@@ -2461,7 +2547,17 @@ By Node ID and Camera Index: NodeID*10+CameraIndex</string>
                  </property>
                 </widget>
                </item>
-               <item row="11" column="0">
+               <item row="2" column="0">
+                <widget class="QSpinBox" name="spinBox_mesh_maxTextures">
+                 <property name="minimum">
+                  <number>1</number>
+                 </property>
+                 <property name="maximum">
+                  <number>20</number>
+                 </property>
+                </widget>
+               </item>
+               <item row="12" column="0">
                 <widget class="QComboBox" name="comboBox_blendingDecimation">
                  <item>
                   <property name="text">
@@ -2505,23 +2601,93 @@ By Node ID and Camera Index: NodeID*10+CameraIndex</string>
                  </item>
                 </widget>
                </item>
-               <item row="10" column="0">
+               <item row="7" column="1">
-                <widget class="QCheckBox" name="checkBox_blending">
+                <widget class="QLabel" name="label_meshingTextureSize_4">
                  <property name="text">
-                  <string/>
+                  <string>Minimum polygon cluster size for texturing. This removes textures applied on sparse polygons. Only used with dense reconstruction flavor.</string>
+                 </property>
+                 <property name="wordWrap">
+                  <bool>true</bool>
+                 </property>
+                </widget>
+               </item>
+               <item row="14" 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="11" column="1">
-                <widget class="QLabel" name="label_exposureFusion_3">
+                <widget class="QLabel" name="label_exposureFusion_2">
                  <property name="text">
-                  <string>Blending decimation. </string>
+                  <string>Blending. Only used with dense reconstruction flavor and if clouds are assembled.</string>
                  </property>
                  <property name="wordWrap">
                   <bool>true</bool>
                  </property>
                 </widget>
                </item>
+               <item row="17" column="1">
+                <widget class="QLabel" name="label_multiband">
+                 <property name="text">
+                  <string>MultiBand texturing. Only available on export and RTAB-Map should be built with AliceVision support.</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>
+               <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="3" column="1">
                 <widget class="QLabel" name="label_meshingTextureSize_7">
                  <property name="text">
@@ -2532,72 +2698,24 @@ By Node ID and Camera Index: NodeID*10+CameraIndex</string>
                  </property>
                 </widget>
                </item>
-               <item row="2" column="1">
+               <item row="17" column="0">
-                <widget class="QLabel" name="label_meshingTextureSize_8">
+                <widget class="QCheckBox" name="checkBox_multiband">
                  <property name="text">
-                  <string>Maximum output textures.</string>
+                  <string/>
-                 </property>
-                 <property name="wordWrap">
-                  <bool>true</bool>
                  </property>
                 </widget>
                </item>
-               <item row="10" column="1">
+               <item row="16" column="0">
-                <widget class="QLabel" name="label_exposureFusion_2">
+                <widget class="QCheckBox" name="checkBox_exposureFusion">
                  <property name="text">
-                  <string>Blending. Only used with dense reconstruction flavor and if clouds are assembled.</string>
+                  <string/>
-                 </property>
-                 <property name="wordWrap">
-                  <bool>true</bool>
                  </property>
                 </widget>
                </item>
-               <item row="4" column="1">
+               <item row="1" column="1">
-                <widget class="QLabel" name="label_meshingTextureSize_2">
+                <widget class="QLabel" name="label_meshingTextureSize">
                  <property name="text">
-                  <string>Maximum distance from the camera for polygons to be textured by this camera (0 means infinite). Only used with dense reconstruction flavor.</string>
+                  <string>Output texture size. If not set or when clouds are not assembled, all textures are saved separately. Warning: values higher than 2048 may not be compatible with all GPUs.</string>
-                 </property>
-                 <property name="wordWrap">
-                  <bool>true</bool>
-                 </property>
-                </widget>
-               </item>
-               <item row="4" column="0">
-                <widget class="QDoubleSpinBox" name="doubleSpinBox_meshingTextureMaxDistance">
-                 <property name="suffix">
-                  <string> m</string>
-                 </property>
-                 <property name="decimals">
-                  <number>1</number>
-                 </property>
-                 <property name="minimum">
-                  <double>0.000000000000000</double>
-                 </property>
-                 <property name="maximum">
-                  <double>99.000000000000000</double>
-                 </property>
-                 <property name="singleStep">
-                  <double>1.000000000000000</double>
-                 </property>
-                 <property name="value">
-                  <double>3.000000000000000</double>
-                 </property>
-                </widget>
-               </item>
-               <item row="12" 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>
-                 </property>
-                 <property name="wordWrap">
-                  <bool>true</bool>
-                 </property>
-                </widget>
-               </item>
-               <item row="0" column="1">
-                <widget class="QLabel" name="label_meshingTextureFormat">
-                 <property name="text">
-                  <string>Texture format.</string>
                  </property>
                  <property name="wordWrap">
                   <bool>true</bool>
@@ -2618,6 +2736,16 @@ By Node ID and Camera Index: NodeID*10+CameraIndex</string>
                  </item>
                 </widget>
                </item>
+               <item row="7" column="0">
+                <widget class="QSpinBox" name="spinBox_mesh_minTextureClusterSize">
+                 <property name="minimum">
+                  <number>0</number>
+                 </property>
+                 <property name="maximum">
+                  <number>99999</number>
+                 </property>
+                </widget>
+               </item>
                <item row="1" column="0">
                 <widget class="QComboBox" name="comboBox_meshingTextureSize">
                  <item>
@@ -2667,17 +2795,17 @@ By Node ID and Camera Index: NodeID*10+CameraIndex</string>
                  </item>
                 </widget>
                </item>
-               <item row="15" column="0">
+               <item row="8" column="0">
-                <widget class="QCheckBox" name="checkBox_exposureFusion">
+                <widget class="QCheckBox" name="checkBox_distanceToCamPolicy">
                  <property name="text">
                   <string/>
                  </property>
                 </widget>
                </item>
-               <item row="7" column="1">
+               <item row="2" column="1">
-                <widget class="QLabel" name="label_meshingTextureSize_4">
+                <widget class="QLabel" name="label_meshingTextureSize_8">
                  <property name="text">
-                  <string>Minimum polygon cluster size for texturing. This removes textures applied on sparse polygons. Only used with dense reconstruction flavor.</string>
+                  <string>Maximum output textures.</string>
                  </property>
                  <property name="wordWrap">
                   <bool>true</bool>
@@ -2687,76 +2815,16 @@ By Node ID and Camera Index: NodeID*10+CameraIndex</string>
                <item row="3" column="0">
                 <widget class="QLineEdit" name="lineEdit_meshingTextureRoiRatios"/>
                </item>
-               <item row="1" column="1">
+               <item row="12" column="1">
-                <widget class="QLabel" name="label_meshingTextureSize">
+                <widget class="QLabel" name="label_exposureFusion_3">
                  <property name="text">
-                  <string>Output texture size. If not set or when clouds are not assembled, all textures are saved separately. Warning: values higher than 2048 may not be compatible with all GPUs.</string>
+                  <string>Blending decimation. </string>
                  </property>
                  <property name="wordWrap">
                   <bool>true</bool>
                  </property>
                 </widget>
                </item>
-               <item row="9" column="0">
-                <widget class="QCheckBox" name="checkBox_cameraFilter">
-                 <property name="text">
-                  <string/>
-                 </property>
-                </widget>
-               </item>
-               <item row="12" column="0">
-                <widget class="QSpinBox" name="spinBox_textureBrightnessContrastRatioLow">
-                 <property name="suffix">
-                  <string> %</string>
-                 </property>
-                 <property name="minimum">
-                  <number>0</number>
-                 </property>
-                 <property name="maximum">
-                  <number>49</number>
-                 </property>
-                </widget>
-               </item>
-               <item row="7" column="0">
-                <widget class="QSpinBox" name="spinBox_mesh_minTextureClusterSize">
-                 <property name="minimum">
-                  <number>0</number>
-                 </property>
-                 <property name="maximum">
-                  <number>99999</number>
-                 </property>
-                </widget>
-               </item>
-               <item row="9" column="1">
-                <widget class="QLabel" name="label_meshingTextureSize_3">
-                 <property name="text">
-                  <string>Camera filtering. Some criteria to select which cameras are used for texturing.</string>
-                 </property>
-                 <property name="wordWrap">
-                  <bool>true</bool>
-                 </property>
-                </widget>
-               </item>
-               <item row="15" 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>
-                 </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">
-                  <number>1</number>
-                 </property>
-                 <property name="maximum">
-                  <number>20</number>
-                 </property>
-                </widget>
-               </item>
                <item row="5" column="1">
                 <widget class="QLabel" name="label_meshingTextureSize_9">
                  <property name="text">
@@ -2767,92 +2835,53 @@ By Node ID and Camera Index: NodeID*10+CameraIndex</string>
                  </property>
                 </widget>
                </item>
-               <item row="5" column="0">
+               <item row="13" column="0">
-                <widget class="QDoubleSpinBox" name="doubleSpinBox_meshingTextureMaxDepthError">
+                <widget class="QSpinBox" name="spinBox_textureBrightnessContrastRatioLow">
                  <property name="suffix">
-                  <string> m</string>
+                  <string> %</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>
+                  <number>49</number>
-                 </property>
-                 <property name="singleStep">
-                  <double>1.000000000000000</double>
-                 </property>
-                 <property name="value">
-                  <double>0.000000000000000</double>
                  </property>
                 </widget>
                </item>
-               <item row="16" column="1">
+               <item row="10" column="0">
-                <widget class="QLabel" name="label_multiband">
+                <widget class="QCheckBox" name="checkBox_cameraFilter">
                  <property name="text">
-                  <string>MultiBand texturing. Only available on export and RTAB-Map should be built with AliceVision support.</string>
+                  <string/>
+                 </property>
+                </widget>
+               </item>
+               <item row="9" column="1">
+                <widget class="QLabel" name="label_texturingVertexColorPolicy">
+                 <property name="text">
+                  <string>Output vertex color policy.</string>
                  </property>
                  <property name="wordWrap">
                   <bool>true</bool>
                  </property>
                 </widget>
                </item>
-               <item row="16" column="0">
+               <item row="9" column="0">
-                <widget class="QCheckBox" name="checkBox_multiband">
+                <widget class="QComboBox" name="comboBox_texturingColorPolicy">
-                 <property name="text">
+                 <item>
-                  <string/>
+                  <property name="text">
-                 </property>
+                   <string>Colorless (standard OBJ)</string>
-                </widget>
+                  </property>
-               </item>
+                 </item>
-               <item row="8" column="1">
+                 <item>
-                <widget class="QLabel" name="label_meshingTextureSize_11">
+                  <property name="text">
-                 <property name="text">
+                   <string>Keep color on textureless polygons only</string>
-                  <string>Distance to camera policy. The closest camera from a polygon is used to texture the polygon. If disabled, the camera for which the polygon projection is the closest of the image center is used to texture the polygon.</string>
+                  </property>
-                 </property>
+                 </item>
-                 <property name="wordWrap">
+                 <item>
-                  <bool>true</bool>
+                  <property name="text">
-                 </property>
+                   <string>Keep all color</string>
-                </widget>
+                  </property>
-               </item>
+                 </item>
-               <item row="8" column="0">
-                <widget class="QCheckBox" name="checkBox_distanceToCamPolicy">
-                 <property name="text">
-                  <string/>
-                 </property>
                 </widget>
                </item>
               </layout>

tools/Export/main.cpp

@@ -85,6 +85,10 @@ void showUsage()
 			"    --texture_angle #     Maximum camera angle for texturing a polygon (default 0 deg: no limit).\n"
 			"    --texture_depth_error # Maximum depth error between reprojected mesh and depth image to texture a face\n"
 			"                                (-1=disabled, 0=edge length is used, default=0).\n"
+			"    --texture_color_policy # How vertices are colored when texturing: \n"
+			"                                 0=colorless (standard OBJ), \n"
+			"                                 1=keep color only on textureless polygons, \n"
+			"                                 2=keep all color.\n"
 			"    --texture_roi_ratios \"# # # #\" Region of interest from images to texture or to color scans. Format\n"
 			"                                         is \"left right top bottom\" (e.g. \"0 0 0 0.1\" means 10%%\n"
 			"                                         of the image bottom not used).\n"
@@ -236,6 +240,7 @@ int main(int argc, char * argv[])
 	double multibandAngleHardthr = 90;
 	bool multibandForceVisible = false;
 	float colorRadius = -1.0f;
+	int textureVertexColorPolicy = 0;
 	bool cloudFromScan = false;
 	bool saveInDb = false;
 	int lowBrightnessGain = 0;
@@ -859,6 +864,18 @@ int main(int argc, char * argv[])
 				showUsage();
 			}
 		}
+		else if(std::strcmp(argv[i], "--texture_color_policy") == 0)
+		{
+			++i;
+			if(i<argc-1)
+			{
+				textureVertexColorPolicy = uStr2Int(argv[i]);
+			}
+			else
+			{
+				showUsage();
+			}
+		}
 		else if(std::strcmp(argv[i], "--scan") == 0)
 		{
 			cloudFromScan = true;
@@ -2306,7 +2323,7 @@ int main(int argc, char * argv[])
 						maxPolygons,
 						cloudToExport,
 						colorRadius,
-						!texture,
+						!(texture && textureVertexColorPolicy == 0),
 						doClean,
 						minCluster);
 				printf("Mesh color transfer... done (%fs).\n", timer.ticks());
@@ -2424,7 +2441,8 @@ int main(int argc, char * argv[])
 								lowBrightnessGain, highBrightnessGain, // low-high brightness/contrast balance
 								false, // exposure fusion
 								0,     // state
-								0,     // blank value (0=black)
+								255,   // blank value (0=white)
+								textureVertexColorPolicy == 1,
 								&gains,
 								&blendingGains,
 								&contrastValues);
@@ -2539,7 +2557,7 @@ int main(int argc, char * argv[])
 										f_idx += static_cast<unsigned>(textureMesh->tex_polygons[m].size());
 									}
 	#endif								
-									success = pcl::io::saveOBJFile(outputPath, *textureMesh) == 0;
+									success = util3d::saveOBJFile(outputPath, *textureMesh) == 0;
 
 									if(success)
 									{