From 18a47ef53dedd42bf729441f32fb759a9bb7b6a7 Mon Sep 17 00:00:00 2001
From: Nikolaas Steenbergen <nikolaas.steenbergen@gmail.com>
Date: Wed, 21 Aug 2024 17:33:49 +0200
Subject: [PATCH] Add color for sampling from mesh  (#6842)

* First version, colors are not sampled correctly

* Fixed uv lookup

* Apply style

* Add poissant sampling

* Add changelog entry

* Add triangle_material_id

* apply style
---
 CHANGELOG.md                         |  2 +-
 cpp/open3d/geometry/TriangleMesh.cpp | 35 ++++++++++++++++++++++++----
 2 files changed, 31 insertions(+), 6 deletions(-)

diff --git a/CHANGELOG.md b/CHANGELOG.md
index 829d890ef53..d032cdedbf4 100644
--- a/CHANGELOG.md
+++ b/CHANGELOG.md
@@ -1,5 +1,5 @@
 ## Main
-
+-   Fix TriangleMesh::SamplePointsUniformly and TriangleMesh::SamplePointsPoissonDisk now sampling colors from mesh if available (PR #6842)
 -   Fix TriangleMesh::SamplePointsUniformly not sampling triangle meshes uniformly (PR #6653)
 -   Fix tensor based TSDF integration example.
 -   Use GLIBCXX_USE_CXX11_ABI=ON by default
diff --git a/cpp/open3d/geometry/TriangleMesh.cpp b/cpp/open3d/geometry/TriangleMesh.cpp
index 7078a675c4f..c73fc3490e8 100644
--- a/cpp/open3d/geometry/TriangleMesh.cpp
+++ b/cpp/open3d/geometry/TriangleMesh.cpp
@@ -443,6 +443,10 @@ std::shared_ptr<PointCloud> TriangleMesh::SamplePointsUniformlyImpl(
     // sample point cloud
     bool has_vert_normal = HasVertexNormals();
     bool has_vert_color = HasVertexColors();
+    bool has_textures_ = HasTextures();
+    bool has_triangle_uvs_ = HasTriangleUvs();
+    bool has_triangle_material_ids_ = HasTriangleMaterialIds();
+
     utility::random::UniformRealGenerator<double> uniform_generator(0.0, 1.0);
     auto pcd = std::make_shared<PointCloud>();
     pcd->points_.resize(number_of_points);
@@ -452,10 +456,9 @@ std::shared_ptr<PointCloud> TriangleMesh::SamplePointsUniformlyImpl(
     if (use_triangle_normal && !HasTriangleNormals()) {
         ComputeTriangleNormals(true);
     }
-    if (has_vert_color) {
+    if (has_vert_color || (has_textures_ && has_triangle_uvs_)) {
         pcd->colors_.resize(number_of_points);
     }
-
     for (size_t point_idx = 0; point_idx < number_of_points; ++point_idx) {
         double r1 = uniform_generator();
         double r2 = uniform_generator();
@@ -475,13 +478,33 @@ std::shared_ptr<PointCloud> TriangleMesh::SamplePointsUniformlyImpl(
         if (use_triangle_normal) {
             pcd->normals_[point_idx] = triangle_normals_[tidx];
         }
-        if (has_vert_color) {
+        // if there is no texture, sample from vertex color
+        if (has_vert_color && !has_textures_ && !has_triangle_uvs_) {
             pcd->colors_[point_idx] = a * vertex_colors_[triangle(0)] +
                                       b * vertex_colors_[triangle(1)] +
                                       c * vertex_colors_[triangle(2)];
         }
+        // if there is a texture, sample from texture instead
+        if (has_textures_ && has_triangle_uvs_ && has_triangle_material_ids_) {
+            Eigen::Vector2d uv = a * triangle_uvs_[3 * tidx] +
+                                 b * triangle_uvs_[3 * tidx + 1] +
+                                 c * triangle_uvs_[3 * tidx + 2];
+            int material_id = triangle_material_ids_[tidx];
+            int w = textures_[material_id].width_;
+            int h = textures_[material_id].height_;
+
+            pcd->colors_[point_idx] = Eigen::Vector3d(
+                    (double)*(textures_[material_id].PointerAt<uint8_t>(
+                            uv(0) * w, uv(1) * h, 0)) /
+                            255,
+                    (double)*(textures_[material_id].PointerAt<uint8_t>(
+                            uv(0) * w, uv(1) * h, 1)) /
+                            255,
+                    (double)*(textures_[material_id].PointerAt<uint8_t>(
+                            uv(0) * w, uv(1) * h, 2)) /
+                            255);
+        }
     }
-
     return pcd;
 }
 
@@ -621,6 +644,8 @@ std::shared_ptr<PointCloud> TriangleMesh::SamplePointsPoissonDisk(
     // update pcl
     bool has_vert_normal = pcl->HasNormals();
     bool has_vert_color = pcl->HasColors();
+    bool has_textures_ = HasTextures();
+    bool has_triangle_uvs_ = HasTriangleUvs();
     int next_free = 0;
     for (size_t idx = 0; idx < pcl->points_.size(); ++idx) {
         if (!deleted[idx]) {
@@ -628,7 +653,7 @@ std::shared_ptr<PointCloud> TriangleMesh::SamplePointsPoissonDisk(
             if (has_vert_normal) {
                 pcl->normals_[next_free] = pcl->normals_[idx];
             }
-            if (has_vert_color) {
+            if (has_vert_color || (has_textures_ && has_triangle_uvs_)) {
                 pcl->colors_[next_free] = pcl->colors_[idx];
             }
             next_free++;