Added OBJ texture policy option (keep color on textureless polygons)

app/android/jni/RTABMapApp.cpp

@@ -1467,6 +1467,24 @@ int RTABMapApp::Render()
                     optMesh_.normals.reset(new pcl::PointCloud<pcl::Normal>);
 					pcl::fromPCLPointCloud2(optTextureMesh_->cloud, *optMesh_.cloud);
 					pcl::fromPCLPointCloud2(optTextureMesh_->cloud, *optMesh_.normals);
+                    bool hasColors = false;
+                    for(unsigned int i=0; i<optTextureMesh_->cloud.fields.size(); ++i)
+                    {
+                        if(optTextureMesh_->cloud.fields[i].name.compare("rgb") == 0)
+                        {
+                            hasColors = true;
+                            break;
+                        }
+                    }
+                    if(!hasColors)
+                    {
+                        std::uint8_t r = 255, g = 255, b = 255;    // White
+                        std::uint32_t rgb = ((std::uint32_t)r << 16 | (std::uint32_t)g << 8 | (std::uint32_t)b);
+                        for(size_t i=0; i<optMesh_.cloud->size(); ++i)
+                        {
+                            optMesh_.cloud->at(i).rgb = *reinterpret_cast<float*>(&rgb);
+                        }
+                    }
                     optMesh_.polygons = optTextureMesh_->tex_polygons[0];
 					if(optTextureMesh_->tex_coordinates.size())
 					{
@@ -3267,6 +3285,7 @@ bool RTABMapApp::exportMesh(
 		int optimizedMinClusterSize,
 		float optimizedMaxTextureDistance,
 		int optimizedMinTextureClusterSize,
+        int textureVertexColorPolicy,
 		bool blockRendering)
 {
 	// make sure createdMeshes_ is not modified while exporting! We don't
@@ -3555,7 +3574,7 @@ bool RTABMapApp::exportMesh(
 									0,
 									mergedClouds,
 									optimizedColorRadius,
-									textureSize == 0,
+                                    !(textureSize > 0 && textureVertexColorPolicy == 0),
 									optimizedCleanWhitePolygons,
 									optimizedMinClusterSize);
 
@@ -3797,7 +3816,9 @@ bool RTABMapApp::exportMesh(
 							textureCount,
 							vertexToPixels,
 							true, 10.0f, true ,true, 0, 0, 0, false,
-							&progressionStatus_);
+							&progressionStatus_,
+                        	255,
+                            textureVertexColorPolicy == 1);
                     LOGI("Merging %d textures... globalTextures=%dx%d", (int)textureMesh->tex_materials.size(),
                          globalTextures.cols, globalTextures.rows);
 				}
@@ -3838,9 +3859,28 @@ bool RTABMapApp::exportMesh(
 				}
 				else if(textureMesh->tex_materials.size())
 				{
-					pcl::PointCloud<pcl::PointNormal>::Ptr cloud(new pcl::PointCloud<pcl::PointNormal>);
+                    bool hasColors = false;
-					pcl::fromPCLPointCloud2(textureMesh->cloud, *cloud);
+                    for(unsigned int i=0; i<textureMesh->cloud.fields.size(); ++i)
-					cv::Mat cloudMat = rtabmap::compressData2(rtabmap::util3d::laserScanFromPointCloud(*cloud, rtabmap::Transform(), false).data()); // for database
+                    {
+                        if(textureMesh->cloud.fields[i].name.compare("rgb") == 0)
+                        {
+                            hasColors = true;
+                            break;
+                        }
+                    }
+                    cv::Mat cloudMat;
+                    if(hasColors)
+                    {
+                        pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZRGBNormal>);
+                        pcl::fromPCLPointCloud2(textureMesh->cloud, *cloud);
+                        cloudMat = rtabmap::compressData2(rtabmap::util3d::laserScanFromPointCloud(*cloud, rtabmap::Transform(), false).data()); // for database
+                    }
+                    else
+                    {
+                        pcl::PointCloud<pcl::PointNormal>::Ptr cloud(new pcl::PointCloud<pcl::PointNormal>);
+                        pcl::fromPCLPointCloud2(textureMesh->cloud, *cloud);
+                        cloudMat = rtabmap::compressData2(rtabmap::util3d::laserScanFromPointCloud(*cloud, rtabmap::Transform(), false).data()); // for database
+                    }
 
 					// save in database
 					std::vector<std::vector<std::vector<RTABMAP_PCL_INDEX> > > polygons(textureMesh->tex_polygons.size());
@@ -4243,7 +4283,7 @@ bool RTABMapApp::writeExportedMesh(const std::string & directory, const std::str
 				totalPolygons += textureMesh->tex_polygons[i].size();
 			}
 			LOGI("Saving obj (%d vertices, %d polygons) to %s.", (int)textureMesh->cloud.data.size()/textureMesh->cloud.point_step, totalPolygons, filePath.c_str());
-			success = pcl::io::saveOBJFile(filePath, *textureMesh) == 0;
+			success = rtabmap::util3d::saveOBJFile(filePath, *textureMesh) == 0;
 
 			if(success)
 			{

app/android/jni/RTABMapApp.h

@@ -177,6 +177,7 @@ class RTABMapApp : public UEventsHandler {
 		  int optimizedMinClusterSize,
 		  float optimizedMaxTextureDistance,
 		  int optimizedMinTextureClusterSize,
+          int textureVertexColorPolicy,
 		  bool blockRendering);
   bool postExportation(bool visualize);
   bool writeExportedMesh(const std::string & directory, const std::string & name);

app/android/jni/point_cloud_drawable.cpp

@@ -170,10 +170,12 @@ const std::string kTextureMeshVertexShader =
     "precision highp float;\n"
     "precision mediump int;\n"
     "attribute vec3 aVertex;\n"
+	"attribute vec3 aColor;\n"
     "attribute vec2 aTexCoord;\n"
 
     "uniform mat4 uMVP;\n"
 
+	"varying vec3 vColor;\n"
     "varying vec2 vTexCoord;\n"
     "varying float vLightWeighting;\n"
 
@@ -187,12 +189,14 @@ const std::string kTextureMeshVertexShader =
     "    vTexCoord = aTexCoord;\n"
     "  }\n"
 
+	"  vColor = aColor;\n"
     "  vLightWeighting = 1.0;\n"
     "}\n";
 const std::string kTextureMeshLightingVertexShader =
     "precision highp float;\n"
     "precision mediump int;\n"
     "attribute vec3 aVertex;\n"
+	"attribute vec3 aColor;\n"
 	"attribute vec3 aNormal;\n"
     "attribute vec2 aTexCoord;\n"
 
@@ -200,6 +204,7 @@ const std::string kTextureMeshLightingVertexShader =
     "uniform mat3 uN;\n"
     "uniform vec3 uLightingDirection;\n"
 
+	"varying vec3 vColor;\n"
     "varying vec2 vTexCoord;\n"
     "varying float vLightWeighting;\n"
 
@@ -213,6 +218,7 @@ const std::string kTextureMeshLightingVertexShader =
     "    vTexCoord = aTexCoord;\n"
     "  }\n"
 
+	"  vColor = aColor;\n"
     "  vec3 transformedNormal = uN * aNormal;\n"
     "  vLightWeighting = max(dot(transformedNormal, uLightingDirection)*0.5+0.5, 0.0);\n"
     "  if(vLightWeighting<0.5) \n"
@@ -225,11 +231,12 @@ const std::string kTextureMeshFragmentShader =
 	"uniform float uGainR;\n"
 	"uniform float uGainG;\n"
 	"uniform float uGainB;\n"
+	"varying vec3 vColor;\n"
     "varying vec2 vTexCoord;\n"
 	"varying float vLightWeighting;\n"
 	""
     "void main() {\n"
-    "  vec4 textureColor = texture2D(uTexture, vTexCoord);\n"
+    "  vec4 textureColor = texture2D(uTexture, vTexCoord) * vec4(vColor.z, vColor.y, vColor.x, 1.0);\n"
 	"  gl_FragColor = vec4(textureColor.r * uGainR * vLightWeighting, textureColor.g * uGainG * vLightWeighting, textureColor.b * uGainB * vLightWeighting, textureColor.a);\n"
     "}\n";
 const std::string kTextureMeshBlendingFragmentShader =
@@ -243,11 +250,12 @@ const std::string kTextureMeshBlendingFragmentShader =
 	"uniform vec2 uScreenScale;\n"
 	"uniform float uNearZ;\n"
 	"uniform float uFarZ;\n"
+	"varying vec3 vColor;\n"
     "varying vec2 vTexCoord;\n"
 	"varying float vLightWeighting;\n"
 	""
     "void main() {\n"
-    "  vec4 textureColor = texture2D(uTexture, vTexCoord);\n"
+	"  vec4 textureColor = texture2D(uTexture, vTexCoord) * vec4(vColor.z, vColor.y, vColor.x, 1.0);\n"
     "  float alpha = 1.0;\n"
 	"  vec2 coord = uScreenScale * gl_FragCoord.xy;\n;"
 	"  vec4 depthPacked = texture2D(uDepthTexture, coord);\n"
@@ -1128,11 +1136,9 @@ void PointCloudDrawable::Render(
 				attribute_texture = glGetAttribLocation(program, "aTexCoord");
 				glEnableVertexAttribArray(attribute_texture);
 			}
-			else
+
-			{
+            attribute_color = glGetAttribLocation(program, "aColor");
-				attribute_color = glGetAttribLocation(program, "aColor");
+            glEnableVertexAttribArray(attribute_color);
-				glEnableVertexAttribArray(attribute_color);
-			}
 		}
 		tango_gl::util::CheckGlError("Pointcloud::Render() common");
 
@@ -1144,7 +1150,7 @@ void PointCloudDrawable::Render(
 			{
 				glVertexAttribPointer(attribute_texture, 2, GL_FLOAT, GL_FALSE, (hasNormals_?9:6)*sizeof(GLfloat), (GLvoid*) (4 * sizeof(GLfloat)));
 			}
-			else if(!packDepthToColorChannel)
+			if(!packDepthToColorChannel)
 			{
 				glVertexAttribPointer(attribute_color, 3, GL_UNSIGNED_BYTE, GL_TRUE,  (hasNormals_?9:6)*sizeof(GLfloat), (GLvoid*) (3 * sizeof(GLfloat)));
 			}

app/ios/RTABMapApp/NativeWrapper.cpp

@@ -150,11 +150,12 @@ bool exportMeshNative(
             int optimizedMinClusterSize,
             float optimizedMaxTextureDistance,
             int optimizedMinTextureClusterSize,
+            int textureVertexColorPolicy,
             bool blockRendering)
 {
     if(object)
     {
-        return native(object)->exportMesh(cloudVoxelSize, regenerateCloud, meshing, textureSize, textureCount, normalK, optimized, optimizedVoxelSize, optimizedDepth, optimizedMaxPolygons, optimizedColorRadius, optimizedCleanWhitePolygons, optimizedMinClusterSize, optimizedMaxTextureDistance, optimizedMinTextureClusterSize, blockRendering);
+        return native(object)->exportMesh(cloudVoxelSize, regenerateCloud, meshing, textureSize, textureCount, normalK, optimized, optimizedVoxelSize, optimizedDepth, optimizedMaxPolygons, optimizedColorRadius, optimizedCleanWhitePolygons, optimizedMinClusterSize, optimizedMaxTextureDistance, optimizedMinTextureClusterSize, textureVertexColorPolicy, blockRendering);
     }
     else
     {

app/ios/RTABMapApp/NativeWrapper.hpp

@@ -55,6 +55,7 @@ bool exportMeshNative(
             int optimizedMinClusterSize,
             float optimizedMaxTextureDistance,
             int optimizedMinTextureClusterSize,
+            int textureVertexColorPolicy,
             bool blockRendering);
 bool postExportationNative(const void *object, bool visualize);
 bool writeExportedMeshNative(const void *object, const char * directory, const char * name);

app/ios/RTABMapApp/RTABMap.swift

@@ -197,6 +197,7 @@ class RTABMap {
         optimizedMinClusterSize: Int,
         optimizedMaxTextureDistance: Float,
         optimizedMinTextureClusterSize: Int,
+        textureVertexColorPolicy: Int,
         blockRendering: Bool) -> Bool
     {
        return exportMeshNative(native_rtabmap,
@@ -215,6 +216,7 @@ class RTABMap {
                                     Int32(optimizedMinClusterSize),
                                     optimizedMaxTextureDistance,
                                     Int32(optimizedMinTextureClusterSize),
+                            		Int32(textureVertexColorPolicy),
                                     blockRendering)
     }
     

app/ios/RTABMapApp/ViewController.swift

@@ -1870,6 +1870,7 @@ class ViewController: GLKViewController, ARSessionDelegate, RTABMapObserver, UIP
         let optimizedColorRadius = defaults.float(forKey: "ColorRadius")
         let optimizedCleanWhitePolygons = defaults.bool(forKey: "CleanMesh")
         let optimizedMinClusterSize = defaults.integer(forKey: "PolygonFiltering")
+        let textureVertexColorPolicy = defaults.integer(forKey: "TextureVertexColorPolicy")
         let blockRendering = false
         
         var indicator: UIActivityIndicatorView?
@@ -1924,6 +1925,7 @@ class ViewController: GLKViewController, ARSessionDelegate, RTABMapObserver, UIP
                     optimizedMinClusterSize: optimizedMinClusterSize,
                     optimizedMaxTextureDistance: maxTextureDistance,
                     optimizedMinTextureClusterSize: minTextureClusterSize,
+                    textureVertexColorPolicy: textureVertexColorPolicy,
                     blockRendering: blockRendering)
                 
             }, completion:{

app/ios/Settings.bundle/Assembling.plist

@@ -320,6 +320,34 @@
 			<key>DefaultValue</key>
 			<true/>
 		</dict>
+		<dict>
+			<key>Type</key>
+			<string>PSGroupSpecifier</string>
+			<key>FooterText</key>
+			<string>Standard OBJ format has colorless polygons where there is no texture. This policy can add color to vertex when exporting the OBJ.</string>
+		</dict>
+		<dict>
+			<key>Type</key>
+			<string>PSMultiValueSpecifier</string>
+			<key>Title</key>
+			<string>Texture Vertex Color Policy</string>
+			<key>Key</key>
+			<string>TextureVertexColorPolicy</string>
+			<key>DefaultValue</key>
+			<integer>0</integer>
+			<key>Titles</key>
+			<array>
+				<string>Colorless (standard OBJ)</string>
+				<string>Keep color only on textureless polygons</string>
+				<string>Keep all color</string>
+			</array>
+			<key>Values</key>
+			<array>
+				<integer>0</integer>
+				<integer>1</integer>
+				<integer>2</integer>
+			</array>
+		</dict>
 		<dict>
 			<key>Type</key>
 			<string>PSGroupSpecifier</string>