Moved PointXYZLAB to common point types (#4706)

* moved PointXYZLAB to common point types * fixed SEGFAULT in registration test * fixed format error from CI patch * made XYZRGB2XYZLAB templated and added array header * Update common/include/pcl/point_types_conversion.h * added constraint for XYZRGB2XYZLAB template function Co-authored-by: Kunal Tyagi <tyagi.kunal@live.com> * added constraint for XYZRGB2XYZLAB template function * removed some redundant codes & tidied headers * fixed format error in gicp6d.cpp Co-authored-by: Kunal Tyagi <tyagi.kunal@live.com>
PointCloudLibrary · Apr 26, 2021 · 842ff67 · 842ff67
1 parent 47bd241
commit 842ff67
Show file tree

Hide file tree

Showing 8 changed files with 157 additions and 97 deletions.
diff --git a/common/include/pcl/common/colors.h b/common/include/pcl/common/colors.h
@@ -41,6 +41,7 @@
 #include <pcl/point_types.h>
 
 #include <type_traits>  // for is_floating_point
+#include <array>        // for std::array especially in Clang Darwin and MSVC
 
 namespace pcl
 {

diff --git a/common/include/pcl/common/impl/intensity.hpp b/common/include/pcl/common/impl/intensity.hpp
@@ -276,6 +276,40 @@ namespace pcl
       }
     };
 
+    template<>
+    struct IntensityFieldAccessor<pcl::PointXYZLAB>
+    {
+      inline float
+      operator () (const pcl::PointXYZLAB &p) const
+      {
+        return (p.L);
+      }
+
+      inline void
+      get (const pcl::PointXYZLAB &p, float &intensity) const
+      {
+        intensity = p.L;
+      }
+
+      inline void
+      set (pcl::PointXYZLAB &p, float intensity) const
+      {
+        p.L = intensity;
+      }
+
+      inline void
+      demean (pcl::PointXYZLAB& p, float value) const
+      {
+        p.L -= value;
+      }
+
+      inline void
+      add (pcl::PointXYZLAB& p, float value) const
+      {
+        p.L += value;
+      }
+    };
+
     template<>
     struct IntensityFieldAccessor<pcl::PointXYZHSV>
     {

diff --git a/common/include/pcl/impl/point_types.hpp b/common/include/pcl/impl/point_types.hpp
@@ -68,6 +68,7 @@
   (pcl::PointXYZRGBA)           \
   (pcl::PointXYZRGB)            \
   (pcl::PointXYZRGBL)           \
+  (pcl::PointXYZLAB)            \
   (pcl::PointXYZHSV)            \
   (pcl::PointXY)                \
   (pcl::InterestPoint)          \
@@ -125,6 +126,7 @@
   (pcl::PointXYZRGBA)         \
   (pcl::PointXYZRGB)          \
   (pcl::PointXYZRGBL)         \
+  (pcl::PointXYZLAB)          \
   (pcl::PointXYZHSV)          \
   (pcl::InterestPoint)        \
   (pcl::PointNormal)          \
@@ -690,6 +692,41 @@ namespace pcl
   };
 
 
+  struct EIGEN_ALIGN16 _PointXYZLAB
+  {
+    PCL_ADD_POINT4D; // this adds the members x,y,z
+    union
+    {
+      struct
+      {
+        float L;
+        float a;
+        float b;
+      };
+      float data_lab[4];
+    };
+    PCL_MAKE_ALIGNED_OPERATOR_NEW
+  };
+
+  PCL_EXPORTS std::ostream& operator << (std::ostream& os, const PointXYZLAB& p);
+  /** \brief A point structure representing Euclidean xyz coordinates, and the CIELAB color.
+    * \ingroup common
+  */
+  struct PointXYZLAB : public _PointXYZLAB
+  {
+    inline PointXYZLAB()
+    {
+      x = y = z = 0.0f;
+      data[3] = 1.0f; // important for homogeneous coordinates
+      L = a = b = 0.0f;
+      data_lab[3] = 0.0f;
+    }
+
+    friend std::ostream& operator << (std::ostream& os, const PointXYZLAB& p);
+    PCL_MAKE_ALIGNED_OPERATOR_NEW
+  };
+
+
   struct EIGEN_ALIGN16 _PointXYZHSV
   {
     PCL_ADD_POINT4D;    // This adds the members x,y,z which can also be accessed using the point (which is float[4])
@@ -1882,6 +1919,16 @@ POINT_CLOUD_REGISTER_POINT_STRUCT (pcl::_PointXYZRGBL,
 )
 POINT_CLOUD_REGISTER_POINT_WRAPPER(pcl::PointXYZRGBL, pcl::_PointXYZRGBL)
 
+POINT_CLOUD_REGISTER_POINT_STRUCT (pcl::_PointXYZLAB,
+    (float, x, x)
+    (float, y, y)
+    (float, z, z)
+    (float, L, L)
+    (float, a, a)
+    (float, b, b)
+)
+POINT_CLOUD_REGISTER_POINT_WRAPPER(pcl::PointXYZLAB, pcl::_PointXYZLAB)
+
 POINT_CLOUD_REGISTER_POINT_STRUCT (pcl::_PointXYZHSV,
     (float, x, x)
     (float, y, y)

diff --git a/common/include/pcl/point_types.h b/common/include/pcl/point_types.h
@@ -111,6 +111,11 @@ namespace pcl
     */
   struct PointXYZRGBL;
 
+  /** \brief Members: float x, y, z, L, a, b
+    * \ingroup common
+    */
+  struct PointXYZLAB;
+
   /** \brief Members: float x, y, z, h, s, v
     * \ingroup common
     */

diff --git a/common/include/pcl/point_types_conversion.h b/common/include/pcl/point_types_conversion.h
@@ -43,6 +43,8 @@
 #include <pcl/point_cloud.h>
 #include <pcl/point_types.h>
 
+#include <pcl/common/colors.h> // for RGB2sRGB_LUT
+
 namespace pcl
 {
   // r,g,b, i values are from 0 to 255
@@ -134,6 +136,57 @@ namespace pcl
     if (out.h < 0.f) out.h += 360.f;
   }
 
+  /** \brief Convert a XYZRGB-based point type to a XYZLAB
+    * \param[in] in the input XYZRGB(XYZRGBA, XYZRGBL, etc.) point
+    * \param[out] out the output XYZLAB point
+    */
+  template <typename PointT, traits::HasColor<PointT> = true>
+  inline void
+  PointXYZRGBtoXYZLAB (const PointT& in,
+                       PointXYZLAB&  out)
+  {
+    out.x = in.x;
+    out.y = in.y;
+    out.z = in.z;
+    out.data[3] = 1.0; // important for homogeneous coordinates
+
+    // convert sRGB to CIELAB
+    // for sRGB   -> CIEXYZ see http://www.easyrgb.com/index.php?X=MATH&H=02#text2
+    // for CIEXYZ -> CIELAB see http://www.easyrgb.com/index.php?X=MATH&H=07#text7
+    // an overview at: https://www.comp.nus.edu.sg/~leowwk/papers/colordiff.pdf
+
+    const auto& sRGB_LUT = RGB2sRGB_LUT<double, 8>();
+
+    const double R = sRGB_LUT[in.r];
+    const double G = sRGB_LUT[in.g];
+    const double B = sRGB_LUT[in.b];
+
+    // linear sRGB -> CIEXYZ, D65 illuminant, observer at 2 degrees
+    const double X = R * 0.4124 + G * 0.3576 + B * 0.1805;
+    const double Y = R * 0.2126 + G * 0.7152 + B * 0.0722;
+    const double Z = R * 0.0193 + G * 0.1192 + B * 0.9505;
+
+    // normalize X, Y, Z with tristimulus values for Xn, Yn, Zn
+    float f[3] = {static_cast<float>(X), static_cast<float>(Y), static_cast<float>(Z)};
+    f[0] /= 0.95047;
+    f[1] /= 1;
+    f[2] /= 1.08883;
+
+    // CIEXYZ -> CIELAB
+    for (int i = 0; i < 3; ++i) {
+      if (f[i] > 0.008856) {
+        f[i] = std::pow(f[i], 1.0 / 3.0);
+      }
+      else {
+        f[i] = 7.787 * f[i] + 16.0 / 116.0;
+      }
+    }
+
+    out.L = 116.0f * f[1] - 16.0f;
+    out.a = 500.0f * (f[0] - f[1]);
+    out.b = 200.0f * (f[1] - f[2]);
+  }
+
   /** \brief Convert a XYZRGBA point type to a XYZHSV
     * \param[in] in the input XYZRGBA point
     * \param[out] out the output XYZHSV point

diff --git a/common/src/point_types.cpp b/common/src/point_types.cpp
@@ -130,6 +130,13 @@ namespace pcl
     return (os);
   }
 
+  std::ostream& 
+  operator << (std::ostream& os, const PointXYZLAB& p)
+  {
+    os << "(" << p.x << "," << p.y << "," << p.z << " - " << p.L << " , " <<  p.a << " , " << p.b << ")";
+    return (os);
+  }
+
   std::ostream& 
   operator << (std::ostream& os, const PointXYZHSV& p)
   {

diff --git a/registration/include/pcl/registration/gicp6d.h b/registration/include/pcl/registration/gicp6d.h
@@ -46,38 +46,6 @@
 #include <pcl/point_representation.h>
 #include <pcl/point_types.h>
 
-namespace pcl {
-struct EIGEN_ALIGN16 _PointXYZLAB {
-  PCL_ADD_POINT4D; // this adds the members x,y,z
-  union {
-    struct {
-      float L;
-      float a;
-      float b;
-    };
-    float data_lab[4];
-  };
-  PCL_MAKE_ALIGNED_OPERATOR_NEW
-};
-
-/** \brief A custom point type for position and CIELAB color value */
-struct PointXYZLAB : public _PointXYZLAB {
-  inline PointXYZLAB()
-  {
-    x = y = z = 0.0f;
-    data[3] = 1.0f; // important for homogeneous coordinates
-    L = a = b = 0.0f;
-    data_lab[3] = 0.0f;
-  }
-};
-} // namespace pcl
-
-// register the custom point type in PCL
-POINT_CLOUD_REGISTER_POINT_STRUCT(
-    pcl::_PointXYZLAB,
-    (float, x, x)(float, y, y)(float, z, z)(float, L, L)(float, a, a)(float, b, b))
-POINT_CLOUD_REGISTER_POINT_WRAPPER(pcl::PointXYZLAB, pcl::_PointXYZLAB)
-
 namespace pcl {
 /** \brief GeneralizedIterativeClosestPoint6D integrates L*a*b* color space information
  * into the Generalized Iterative Closest Point (GICP) algorithm.

diff --git a/registration/src/gicp6d.cpp b/registration/src/gicp6d.cpp
@@ -36,72 +36,11 @@
  *
  */
 
-#include <pcl/common/colors.h> // for RGB2sRGB_LUT, XYZ2LAB_LUT
 #include <pcl/registration/gicp6d.h>
-#include <pcl/memory.h> // for pcl::make_shared
+#include <pcl/memory.h>                 // for pcl::make_shared
+#include <pcl/point_types_conversion.h> // for PointXYZRGBtoXYZLAB
 
 namespace pcl {
-// convert sRGB to CIELAB
-Eigen::Vector3f
-RGB2Lab(const Eigen::Vector3i& colorRGB)
-{
-  // for sRGB   -> CIEXYZ see http://www.easyrgb.com/index.php?X=MATH&H=02#text2
-  // for CIEXYZ -> CIELAB see http://www.easyrgb.com/index.php?X=MATH&H=07#text7
-  // an overview at: https://www.comp.nus.edu.sg/~leowwk/papers/colordiff.pdf
-
-  const auto& sRGB_LUT = RGB2sRGB_LUT<double, 8>();
-
-  const double R = sRGB_LUT[colorRGB[0]];
-  const double G = sRGB_LUT[colorRGB[1]];
-  const double B = sRGB_LUT[colorRGB[2]];
-
-  // linear sRGB -> CIEXYZ, D65 illuminant, observer at 2 degrees
-  const double X = R * 0.4124 + G * 0.3576 + B * 0.1805;
-  const double Y = R * 0.2126 + G * 0.7152 + B * 0.0722;
-  const double Z = R * 0.0193 + G * 0.1192 + B * 0.9505;
-
-  // normalize X, Y, Z with tristimulus values for Xn, Yn, Zn
-  float f[3] = {static_cast<float>(X), static_cast<float>(Y), static_cast<float>(Z)};
-  f[0] /= 0.95047;
-  f[1] /= 1;
-  f[2] /= 1.08883;
-
-  // CIEXYZ -> CIELAB
-  for (int i = 0; i < 3; ++i) {
-    if (f[i] > 0.008856) {
-      f[i] = std::pow(f[i], 1.0 / 3.0);
-    }
-    else {
-      f[i] = 7.787 * f[i] + 16.0 / 116.0;
-    }
-  }
-
-  Eigen::Vector3f colorLab;
-  colorLab[0] = 116.0f * f[1] - 16.0f;
-  colorLab[1] = 500.0f * (f[0] - f[1]);
-  colorLab[2] = 200.0f * (f[1] - f[2]);
-
-  return colorLab;
-}
-
-// convert a PointXYZRGBA cloud to a PointXYZLAB cloud
-void
-convertRGBAToLAB(const PointCloud<pcl::PointXYZRGBA>& in, PointCloud<PointXYZLAB>& out)
-{
-  out.resize(in.size());
-
-  for (std::size_t i = 0; i < in.size(); ++i) {
-    out[i].x = in[i].x;
-    out[i].y = in[i].y;
-    out[i].z = in[i].z;
-    out[i].data[3] = 1.0; // important for homogeneous coordinates
-
-    Eigen::Vector3f lab = RGB2Lab(in[i].getRGBVector3i());
-    out[i].L = lab[0];
-    out[i].a = lab[1];
-    out[i].b = lab[2];
-  }
-}
 
 GeneralizedIterativeClosestPoint6D::GeneralizedIterativeClosestPoint6D(float lab_weight)
 : cloud_lab_(new pcl::PointCloud<PointXYZLAB>)
@@ -121,7 +60,10 @@ GeneralizedIterativeClosestPoint6D::setInputSource(
   GeneralizedIterativeClosestPoint<PointSource, PointTarget>::setInputSource(cloud);
 
   // in addition, convert colors of the cloud to CIELAB
-  convertRGBAToLAB(*cloud, *cloud_lab_);
+  cloud_lab_->resize(cloud->size());
+  for (std::size_t point_idx = 0; point_idx < cloud->size(); ++point_idx) {
+    PointXYZRGBtoXYZLAB((*cloud)[point_idx], (*cloud_lab_)[point_idx]);
+  }
 }
 
 void
@@ -132,7 +74,10 @@ GeneralizedIterativeClosestPoint6D::setInputTarget(
   GeneralizedIterativeClosestPoint<PointSource, PointTarget>::setInputTarget(target);
 
   // in addition, convert colors of the cloud to CIELAB...
-  convertRGBAToLAB(*target, *target_lab_);
+  target_lab_->resize(target->size());
+  for (std::size_t point_idx = 0; point_idx < target->size(); ++point_idx) {
+    PointXYZRGBtoXYZLAB((*target)[point_idx], (*target_lab_)[point_idx]);
+  }
 
   // ...and build 6d-tree
   target_tree_lab_.setInputCloud(target_lab_);