Merge pull request #1286 from PDAL/loganbyers-pcdwriter_upgrades

PCD Writer updates #1206

plugins/pcl/PCLConversions.hpp

@@ -130,7 +130,8 @@ void PCDtoPDAL(CloudT &cloud, PointViewPtr view)
  * Converts PDAL data to PCD format.
  */
 template <typename CloudT>
-void PDALtoPCD(PointViewPtr view, CloudT &cloud, BOX3D const& bounds)
+void PDALtoPCD(PointViewPtr view, CloudT &cloud, BOX3D const& bounds,
+               double const& scale_x, double const& scale_y, double const& scale_z)
 {
     typedef typename pcl::traits::fieldList<typename CloudT::PointType>::type
         FieldList;
@@ -149,9 +150,9 @@ void PDALtoPCD(PointViewPtr view, CloudT &cloud, BOX3D const& bounds)
             double zd = view->getFieldAs<double>(Dimension::Id::Z, i) - bounds.minz;
 
             typename CloudT::PointType p = cloud.points[i];
-            p.x = (float)xd;
-            p.y = (float)yd;
-            p.z = (float)zd;
+            p.x = (float) (xd / scale_x);
+            p.y = (float) (yd / scale_y);
+            p.z = (float) (zd / scale_z);
             cloud.points[i] = p;
         }
     }
@@ -189,6 +190,18 @@ void PDALtoPCD(PointViewPtr view, CloudT &cloud, BOX3D const& bounds)
     }
 }
 
+/**
+ * \brief Convert PDAL point cloud to PCD.
+ *
+ * Converts PDAL data to PCD format.
+ */
+template <typename CloudT>
+void PDALtoPCD(PointViewPtr view, CloudT &cloud, BOX3D const& bounds)
+{
+    PDALtoPCD(view, cloud, bounds, 1.0, 1.0, 1.0);
+}
+
+
 
 template <typename CloudT>
 void PDALtoPCD(PointViewPtr view, CloudT &cloud)

plugins/pcl/io/PcdWriter.cpp

@@ -53,33 +53,53 @@ namespace pdal
 static PluginInfo const s_info = PluginInfo(
     "writers.pcd",
     "Write data in the Point Cloud Library (PCL) format.",
-    "http://pdal.io/stages/writers.pclvisualizer.html" );
+    "http://pdal.io/stages/writers.pcd.html" );
 
 CREATE_SHARED_PLUGIN(1, 0, PcdWriter, Writer, s_info)
 
 std::string PcdWriter::getName() const { return s_info.name; }
 
+
 void PcdWriter::addArgs(ProgramArgs& args)
 {
-    args.add("filename", "Output filename", m_filename).setPositional();
-    args.add("compression", "Whether compressed output should be written",
-        m_compressed);
-}
 
+    std::string compression;
+    args.add("filename", "Filename to write PCD file to", m_filename);
+    args.add("compression","Level of PCD compression to use (ascii, binary, compressed)", compression, "ascii");
+    args.add("xyz", "Write only XYZ dimensions?", m_xyz, false);
+    args.add("subtract_minimum", "Set origin to minimum of XYZ dimension", m_subtract_minimum, true);
+    args.add("offset_x", "Offset to be subtracted from XYZ position", m_offset_x, 0.0);
+    args.add("offset_y", "Offset to be subtracted from XYZ position", m_offset_y, 0.0);
+    args.add("offset_z", "Offset to be subtracted from XYZ position", m_offset_z, 0.0);
+    args.add("scale_x", "Scale to divide from XYZ dimension", m_scale_x, 1.0);
+    args.add("scale_y", "Scale to divide from XYZ dimension", m_scale_y, 1.0);
+    args.add("scale_z", "Scale to divide from XYZ dimension", m_scale_z, 1.0);
 
-void PcdWriter::write(const PointViewPtr view)
-{
-    pcl::PointCloud<XYZIRGBA>::Ptr cloud(new pcl::PointCloud<XYZIRGBA>);
-    BOX3D buffer_bounds;
-    view->calculateBounds(buffer_bounds);
-    pclsupport::PDALtoPCD(view, *cloud, buffer_bounds);
+    if (compression == "binary")
+    {
+      m_compression = 1;
+    }
+    else if (compression == "compressed")
+    {
+      m_compression = 2;
+    }
+    else  // including "ascii"
+    {
+      m_compression = 0;
+    }
 
-    pcl::PCDWriter w;
+}
 
-    if (m_compressed)
-        w.writeBinaryCompressed<XYZIRGBA>(m_filename, *cloud);
+void PcdWriter::write(const PointViewPtr view)
+{
+    if (m_xyz)
+    {
+        writeView<pcl::PointCloud<pcl::PointXYZ> >(view);
+    }
     else
-        w.writeASCII<XYZIRGBA>(m_filename, *cloud);
+    {
+        writeView<pcl::PointCloud<XYZIRGBA> >(view);
+    }
 }
 
 

plugins/pcl/io/PcdWriter.hpp

@@ -38,6 +38,11 @@
 #include <pdal/util/FileUtils.hpp>
 #include <pdal/StageFactory.hpp>
 
+#include "PCLConversions.hpp"
+
+#include <pcl/io/pcd_io.h>
+#include <pcl/io/impl/pcd_io.hpp>
+
 #include <vector>
 #include <string>
 
@@ -58,11 +63,51 @@ class PDAL_DLL PcdWriter : public Writer
     virtual void addArgs(ProgramArgs& args);
     virtual void write(const PointViewPtr view);
 
+    template<typename CloudT>
+    inline void writeView(const PointViewPtr view); // implemented in header
+
     std::string m_filename;
-    bool m_compressed;
+    uint8_t m_compression;
+    bool m_xyz;
+    bool m_subtract_minimum;
+    double m_offset_x;
+    double m_offset_y;
+    double m_offset_z;
+    double m_scale_x;
+    double m_scale_y;
+    double m_scale_z;
 
     PcdWriter& operator=(const PcdWriter&); // not implemented
     PcdWriter(const PcdWriter&); // not implemented
 };
 
+
+template<typename CloudT>
+void PcdWriter::writeView(const PointViewPtr view)
+{
+    typedef typename CloudT::PointType PointT;
+    typename CloudT::Ptr cloud(new CloudT);
+    BOX3D bounds;
+    if (m_subtract_minimum)
+    {
+        view->calculateBounds(bounds);
+        bounds.grow(bounds.minx + m_offset_x,
+                    bounds.miny + m_offset_y,
+                    bounds.minz + m_offset_z);
+    }
+    else
+    {
+        bounds.grow(m_offset_x, m_offset_y, m_offset_z);
+    }
+    pclsupport::PDALtoPCD(view, *cloud, bounds, m_scale_x, m_scale_y, m_scale_z);
+    pcl::PCDWriter w;
+    switch (m_compression)
+    {
+        case 0 : w.writeASCII<PointT>(m_filename, *cloud); break;
+        case 1 : w.writeBinary<PointT>(m_filename, *cloud); break;
+        case 2 : w.writeBinaryCompressed<PointT>(m_filename, *cloud); break;
+    }
+}
+
+
 } // namespaces