From 2b92820f6e587e81afaafe49030a73c1425fbfe8 Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Bartek=20=C5=81ukawski?= <bwmn.peter@gmail.com>
Date: Thu, 17 Oct 2024 18:48:26 +0200
Subject: [PATCH] Add specialized instantiations for the MLS algorithm

https://github.com/roboticslab-uc3m/vision/issues/112
---
 libraries/YarpCloudUtils/CMakeLists.txt       |  2 +-
 .../YarpCloudUtils-pcl-traits.hpp             | 35 +++++++++++++++++++
 .../YarpCloudUtils/YarpCloudUtils-pcl.cpp     |  6 ++--
 3 files changed, 40 insertions(+), 3 deletions(-)

diff --git a/libraries/YarpCloudUtils/CMakeLists.txt b/libraries/YarpCloudUtils/CMakeLists.txt
index b8ca8bde..ca91222d 100644
--- a/libraries/YarpCloudUtils/CMakeLists.txt
+++ b/libraries/YarpCloudUtils/CMakeLists.txt
@@ -37,7 +37,7 @@ if(ENABLE_YarpCloudUtils)
         target_link_libraries(YarpCloudUtils PRIVATE YARP::YARP_pcl
                                                      ${PCL_LIBRARIES})
 
-        target_compile_definitions(YarpCloudUtils PUBLIC YCU_HAVE_PCL PCL_NO_PRECOMPILE)
+        target_compile_definitions(YarpCloudUtils PUBLIC YCU_HAVE_PCL)
     endif()
 
     add_library(ROBOTICSLAB::YarpCloudUtils ALIAS YarpCloudUtils)
diff --git a/libraries/YarpCloudUtils/YarpCloudUtils-pcl-traits.hpp b/libraries/YarpCloudUtils/YarpCloudUtils-pcl-traits.hpp
index b74d3d5d..48614a32 100644
--- a/libraries/YarpCloudUtils/YarpCloudUtils-pcl-traits.hpp
+++ b/libraries/YarpCloudUtils/YarpCloudUtils-pcl-traits.hpp
@@ -59,6 +59,11 @@ struct pcl_xyzi_types_tag {}; // XYZI(+Normal)
 
 struct pcl_normal_types_tag {};
 
+// special cases for pcl::MovingLeastSquares, see:
+// https://github.com/PointCloudLibrary/pcl/pull/5764
+struct pcl_mls_types_tag : public pcl_all_xyz_types_tag {};
+struct pcl_mls_normal_types_tag : public pcl_normal_types_tag {};
+
 // map PCL type according to selected tag
 
 template <typename T, typename tag>
@@ -130,6 +135,36 @@ template <>
 struct pcl_decay<pcl::InterestPoint, pcl_normal_types_tag>
 { typedef pcl::PointNormal type; };
 
+// mappings for pcl::MovingLeastSquares (special case, less precompiled instantiations)
+
+template <typename T>
+struct pcl_decay<T, pcl_mls_types_tag>
+{ typedef T type; };
+
+template <>
+struct pcl_decay<pcl::InterestPoint, pcl_mls_types_tag>
+{ typedef pcl::PointXYZ type; };
+
+template <typename T>
+struct pcl_decay<T, pcl_mls_normal_types_tag>
+{ typedef T type; };
+
+template <>
+struct pcl_decay<pcl::PointXYZ, pcl_mls_normal_types_tag>
+{ typedef pcl::PointNormal type; };
+
+template <>
+struct pcl_decay<pcl::PointXYZRGB, pcl_mls_normal_types_tag>
+{ typedef pcl::PointXYZRGBNormal type; };
+
+template <>
+struct pcl_decay<pcl::PointXYZI, pcl_mls_normal_types_tag>
+{ typedef pcl::PointNormal type; };
+
+template <>
+struct pcl_decay<pcl::InterestPoint, pcl_mls_normal_types_tag>
+{ typedef pcl::PointNormal type; };
+
 // register allowed conversions
 
 template <typename T1, typename T2>
diff --git a/libraries/YarpCloudUtils/YarpCloudUtils-pcl.cpp b/libraries/YarpCloudUtils/YarpCloudUtils-pcl.cpp
index 1edf7db1..5803d662 100644
--- a/libraries/YarpCloudUtils/YarpCloudUtils-pcl.cpp
+++ b/libraries/YarpCloudUtils/YarpCloudUtils-pcl.cpp
@@ -67,6 +67,8 @@ namespace
         using rgb_t = typename pcl_decay<T, pcl_rgb_types_tag>::type;
         using xyzi_t = typename pcl_decay<T, pcl_xyzi_types_tag>::type;
         using normal_t = typename pcl_decay<T, pcl_normal_types_tag>::type;
+        using mls_t = typename pcl_decay<T, pcl_mls_types_tag>::type;
+        using mls_normal_t = typename pcl_decay<T, pcl_mls_normal_types_tag>::type;
 
         if (!options.check("algorithm"))
         {
@@ -142,9 +144,9 @@ namespace
             break;
         case "MovingLeastSquares"_hash:
             if (options.check("computeNormals"), yarp::os::Value(false).asBool())
-                doMovingLeastSquares<any_xyz_t, normal_t>(prev.getCloud<any_xyz_t>(), curr.setCloud<normal_t>(), options);
+                doMovingLeastSquares<mls_t, mls_normal_t>(prev.getCloud<mls_t>(), curr.setCloud<mls_normal_t>(), options);
             else
-                doMovingLeastSquares<any_xyz_t>(prev.getCloud<any_xyz_t>(), curr.setCloud<any_xyz_t>(), options);
+                doMovingLeastSquares<mls_t>(prev.getCloud<mls_t>(), curr.setCloud<mls_t>(), options);
             break;
         case "NormalEstimation"_hash:
             pcl::copyPointCloud(*prev.getCloud<any_xyz_t>(), *curr.setCloud<normal_t>());