From 07b36e6bca6296224d60cd8e5f0d853fb7841ce9 Mon Sep 17 00:00:00 2001
From: wmayer <wmayer@users.sourceforge.net>
Date: Thu, 31 Dec 2015 15:36:16 +0100
Subject: [PATCH] + extended ply reader based on pcl to read-in attached
 properties

---
 src/Mod/Points/App/PointsAlgos.cpp | 164 +++++++++++++++++++++++++++++
 src/Mod/Points/App/PointsAlgos.h   |  39 +++++--
 2 files changed, 196 insertions(+), 7 deletions(-)

diff --git a/src/Mod/Points/App/PointsAlgos.cpp b/src/Mod/Points/App/PointsAlgos.cpp
index 942d91ce9..33b7e87bc 100644
--- a/src/Mod/Points/App/PointsAlgos.cpp
+++ b/src/Mod/Points/App/PointsAlgos.cpp
@@ -29,6 +29,11 @@
 # include <sstream>
 #endif
 
+#ifdef HAVE_PCL_IO
+# include <pcl/io/ply_io.h>
+# include <pcl/point_types.h>
+#endif
+
 
 #include "PointsAlgos.h"
 #include "Points.h"
@@ -111,3 +116,162 @@ void PointsAlgos::LoadAscii(PointKernel &points, const char *FileName)
     if (LineCnt < (int)points.size())
         points.erase(LineCnt, points.size());
 }
+
+#ifdef HAVE_PCL_IO
+PlyReader::PlyReader()
+{
+}
+
+PlyReader::~PlyReader()
+{
+}
+
+void PlyReader::clear()
+{
+    intensity.clear();
+    colors.clear();
+    normals.clear();
+}
+
+void PlyReader::read(const std::string& filename)
+{
+    clear();
+
+    // pcl test
+    pcl::PCLPointCloud2 cloud2;
+    Eigen::Vector4f origin;
+    Eigen::Quaternionf orientation;
+    int ply_version;
+    int data_type;
+    unsigned int data_idx;
+    pcl::PLYReader ply;
+    ply.readHeader(filename, cloud2, origin, orientation, ply_version, data_type, data_idx);
+
+    bool hasIntensity = false;
+    bool hasColors = false;
+    bool hasNormals = false;
+    for (size_t i = 0; i < cloud2.fields.size (); ++i) {
+        if (cloud2.fields[i].name == "intensity")
+            hasIntensity = true;
+        if (cloud2.fields[i].name == "normal_x" || cloud2.fields[i].name == "nx")
+            hasNormals = true;
+        if (cloud2.fields[i].name == "normal_y" || cloud2.fields[i].name == "ny")
+            hasNormals = true;
+        if (cloud2.fields[i].name == "normal_z" || cloud2.fields[i].name == "nz")
+            hasNormals = true;
+        if (cloud2.fields[i].name == "red")
+            hasColors = true;
+        if (cloud2.fields[i].name == "green")
+            hasColors = true;
+        if (cloud2.fields[i].name == "blue")
+            hasColors = true;
+        if (cloud2.fields[i].name == "rgb")
+            hasColors = true;
+        if (cloud2.fields[i].name == "rgba")
+            hasColors = true;
+    }
+
+    if (hasNormals && hasColors) {
+        pcl::PointCloud<pcl::PointXYZRGBNormal> cloud_in;
+        pcl::io::loadPLYFile<pcl::PointXYZRGBNormal>(filename, cloud_in);
+        points.reserve(cloud_in.size());
+        colors.reserve(cloud_in.size());
+        normals.reserve(cloud_in.size());
+        for (pcl::PointCloud<pcl::PointXYZRGBNormal>::const_iterator it = cloud_in.begin();it!=cloud_in.end();++it) {
+            points.push_back(Base::Vector3d(it->x,it->y,it->z));
+            colors.push_back(App::Color(it->r/255.0f,it->g/255.0f,it->b/255.0f));
+            normals.push_back(Base::Vector3f(it->normal_x,it->normal_y,it->normal_z));
+        }
+    }
+    else if (hasNormals && hasIntensity) {
+        pcl::PointCloud<pcl::PointXYZINormal> cloud_in;
+        pcl::io::loadPLYFile<pcl::PointXYZINormal>(filename, cloud_in);
+        points.reserve(cloud_in.size());
+        intensity.reserve(cloud_in.size());
+        normals.reserve(cloud_in.size());
+        for (pcl::PointCloud<pcl::PointXYZINormal>::const_iterator it = cloud_in.begin();it!=cloud_in.end();++it) {
+            points.push_back(Base::Vector3d(it->x,it->y,it->z));
+            intensity.push_back(it->intensity);
+            normals.push_back(Base::Vector3f(it->normal_x,it->normal_y,it->normal_z));
+        }
+    }
+    else if (hasColors) {
+        pcl::PointCloud<pcl::PointXYZRGBA> cloud_in;
+        pcl::io::loadPLYFile<pcl::PointXYZRGBA>(filename, cloud_in);
+        points.reserve(cloud_in.size());
+        colors.reserve(cloud_in.size());
+        for (pcl::PointCloud<pcl::PointXYZRGBA>::const_iterator it = cloud_in.begin();it!=cloud_in.end();++it) {
+            points.push_back(Base::Vector3d(it->x,it->y,it->z));
+            colors.push_back(App::Color(it->r/255.0f,it->g/255.0f,it->b/255.0f,it->a/255.0f));
+        }
+    }
+    else if (hasIntensity) {
+        pcl::PointCloud<pcl::PointXYZI> cloud_in;
+        pcl::io::loadPLYFile<pcl::PointXYZI>(filename, cloud_in);
+        points.reserve(cloud_in.size());
+        intensity.reserve(cloud_in.size());
+        for (pcl::PointCloud<pcl::PointXYZI>::const_iterator it = cloud_in.begin();it!=cloud_in.end();++it) {
+            points.push_back(Base::Vector3d(it->x,it->y,it->z));
+            intensity.push_back(it->intensity);
+        }
+    }
+    else if (hasNormals) {
+        pcl::PointCloud<pcl::PointNormal> cloud_in;
+        pcl::io::loadPLYFile<pcl::PointNormal>(filename, cloud_in);
+        points.reserve(cloud_in.size());
+        normals.reserve(cloud_in.size());
+        for (pcl::PointCloud<pcl::PointNormal>::const_iterator it = cloud_in.begin();it!=cloud_in.end();++it) {
+            points.push_back(Base::Vector3d(it->x,it->y,it->z));
+            normals.push_back(Base::Vector3f(it->normal_x,it->normal_y,it->normal_z));
+        }
+    }
+    else {
+        pcl::PointCloud<pcl::PointXYZ> cloud_in;
+        pcl::io::loadPLYFile<pcl::PointXYZ>(filename, cloud_in);
+        points.reserve(cloud_in.size());
+        for (pcl::PointCloud<pcl::PointXYZ>::const_iterator it = cloud_in.begin();it!=cloud_in.end();++it) {
+            points.push_back(Base::Vector3d(it->x,it->y,it->z));
+        }
+    }
+}
+
+const PointKernel& PlyReader::getPoints() const
+{
+    return points;
+}
+
+bool PlyReader::hasProperties() const
+{
+    return (hasIntensities() || hasColors() || hasNormals());
+}
+
+const std::vector<float>& PlyReader::getIntensities() const
+{
+    return intensity;
+}
+
+bool PlyReader::hasIntensities() const
+{
+    return (!intensity.empty());
+}
+
+const std::vector<App::Color>& PlyReader::getColors() const
+{
+    return colors;
+}
+
+bool PlyReader::hasColors() const
+{
+    return (!colors.empty());
+}
+
+const std::vector<Base::Vector3f>& PlyReader::getNormals() const
+{
+    return normals;
+}
+
+bool PlyReader::hasNormals() const
+{
+    return (!normals.empty());
+}
+#endif
diff --git a/src/Mod/Points/App/PointsAlgos.h b/src/Mod/Points/App/PointsAlgos.h
index 119f96bbd..42976e18e 100644
--- a/src/Mod/Points/App/PointsAlgos.h
+++ b/src/Mod/Points/App/PointsAlgos.h
@@ -25,6 +25,7 @@
 #define _PointsAlgos_h_
 
 #include "Points.h"
+#include "Properties.h"
 
 namespace Points
 {
@@ -34,15 +35,39 @@ namespace Points
 class PointsExport PointsAlgos
 {
 public:
-  /** Load a point cloud
-   */
-  static void Load(PointKernel&, const char *FileName);
-  /** Load a point cloud
-   */
-  static void LoadAscii(PointKernel&, const char *FileName);
-
+    /** Load a point cloud
+     */
+    static void Load(PointKernel&, const char *FileName);
+    /** Load a point cloud
+     */
+    static void LoadAscii(PointKernel&, const char *FileName);
 };
 
+#ifdef HAVE_PCL_IO
+class PlyReader
+{
+public:
+    PlyReader();
+    ~PlyReader();
+    void clear();
+    void read(const std::string& filename);
+    const PointKernel& getPoints() const;
+    bool hasProperties() const;
+    const std::vector<float>& getIntensities() const;
+    bool hasIntensities() const;
+    const std::vector<App::Color>& getColors() const;
+    bool hasColors() const;
+    const std::vector<Base::Vector3f>& getNormals() const;
+    bool hasNormals() const;
+
+private:
+    PointKernel points;
+    std::vector<float> intensity;
+    std::vector<App::Color> colors;
+    std::vector<Base::Vector3f> normals;
+};
+#endif
+
 } // namespace Points