From 6bf5fa55448e50c5045689d9e86809545a343b76 Mon Sep 17 00:00:00 2001 From: Bradley J Chambers Date: Wed, 26 Feb 2014 19:58:30 +0000 Subject: [PATCH] as referenced in #250, do not apply offsets in conversion to PCD, but pass them along to the PCL Pipeline --- include/pdal/PCLConversions.hpp | 63 +++++++++------------------------ src/filters/PCLBlock.cpp | 22 +++++++++--- 2 files changed, 34 insertions(+), 51 deletions(-) diff --git a/include/pdal/PCLConversions.hpp b/include/pdal/PCLConversions.hpp index e2a3f6b9b1..08f7b0ecb3 100755 --- a/include/pdal/PCLConversions.hpp +++ b/include/pdal/PCLConversions.hpp @@ -76,23 +76,11 @@ void PCDtoPDAL(CloudT &cloud, PointBuffer& data) data.setNumPoints(cloud.points.size()); - typename CloudT::PointType testPoint = cloud.points[0]; - - bool has_x = false; - bool has_y = false; - bool has_z = false; - float x_val = 0.0f; - float y_val = 0.0f; - float z_val = 0.0f; - pcl::for_each_type (pcl::CopyIfFieldExists (testPoint, "x", has_x, x_val)); - pcl::for_each_type (pcl::CopyIfFieldExists (testPoint, "y", has_y, y_val)); - pcl::for_each_type (pcl::CopyIfFieldExists (testPoint, "z", has_z, z_val)); - const pdal::Dimension &dX = buffer_schema.getDimension("X"); const pdal::Dimension &dY = buffer_schema.getDimension("Y"); const pdal::Dimension &dZ = buffer_schema.getDimension("Z"); - if (has_x && has_y && has_z) + if (pcl::traits::has_xyz::value) { boost::uint32_t size = dX.getByteSize(); switch (dX.getInterpretation()) @@ -111,7 +99,8 @@ void PCDtoPDAL(CloudT &cloud, PointBuffer& data) data.setField(dX, i, dX.removeScaling(cloud.points[i].x)); if (size == 4) for (size_t i = 0; i < cloud.points.size(); ++i) - data.setField(dX, i, dX.removeScaling(cloud.points[i].x)); + data.setField(dX, i, cloud.points[i].x/dX.getNumericScale()); + //data.setField(dX, i, dX.removeScaling(cloud.points[i].x)); if (size == 8) for (size_t i = 0; i < cloud.points.size(); ++i) data.setField(dX, i, dX.removeScaling(cloud.points[i].x)); @@ -154,7 +143,8 @@ void PCDtoPDAL(CloudT &cloud, PointBuffer& data) data.setField(dY, i, dY.removeScaling(cloud.points[i].y)); if (size == 4) for (size_t i = 0; i < cloud.points.size(); ++i) - data.setField(dY, i, dY.removeScaling(cloud.points[i].y)); + data.setField(dY, i, cloud.points[i].y/dY.getNumericScale()); + //data.setField(dY, i, dY.removeScaling(cloud.points[i].y)); if (size == 8) for (size_t i = 0; i < cloud.points.size(); ++i) data.setField(dY, i, dY.removeScaling(cloud.points[i].y)); @@ -197,7 +187,8 @@ void PCDtoPDAL(CloudT &cloud, PointBuffer& data) data.setField(dZ, i, dZ.removeScaling(cloud.points[i].z)); if (size == 4) for (size_t i = 0; i < cloud.points.size(); ++i) - data.setField(dZ, i, dZ.removeScaling(cloud.points[i].z)); + data.setField(dZ, i, cloud.points[i].z/dZ.getNumericScale()); + //data.setField(dZ, i, dZ.removeScaling(cloud.points[i].z)); if (size == 8) for (size_t i = 0; i < cloud.points.size(); ++i) data.setField(dZ, i, dZ.removeScaling(cloud.points[i].z)); @@ -227,12 +218,10 @@ void PCDtoPDAL(CloudT &cloud, PointBuffer& data) boost::optional dI = buffer_schema.getDimensionOptional("Intensity"); - bool has_i = false; - float i_val = 0.0f; - pcl::for_each_type (pcl::CopyIfFieldExists (testPoint, "intensity", has_i, i_val)); - - if (has_i && dI) + if (pcl::traits::has_intensity::value && dI) { + bool has_i = true; + for (size_t i = 0; i < cloud.points.size(); ++i) { float v; @@ -264,45 +253,27 @@ void PDALtoPCD(PointBuffer& data, CloudT &cloud) cloud.is_dense = false; cloud.points.resize(cloud.width); - typename CloudT::PointType testPoint = cloud.points[0]; - - bool has_x = false; - bool has_y = false; - bool has_z = false; - float x_val = 0.0f; - float y_val = 0.0f; - float z_val = 0.0f; - pcl::for_each_type (pcl::CopyIfFieldExists (testPoint, "x", has_x, x_val)); - pcl::for_each_type (pcl::CopyIfFieldExists (testPoint, "y", has_y, y_val)); - pcl::for_each_type (pcl::CopyIfFieldExists (testPoint, "z", has_z, z_val)); - const pdal::Dimension &dX = buffer_schema.getDimension("X"); const pdal::Dimension &dY = buffer_schema.getDimension("Y"); const pdal::Dimension &dZ = buffer_schema.getDimension("Z"); - - if (has_x && has_y && has_z) + + if (pcl::traits::has_xyz::value) { for (size_t i = 0; i < cloud.points.size(); ++i) { - float xd = data.applyScaling(dX, i); - float yd = data.applyScaling(dY, i); - float zd = data.applyScaling(dZ, i); + float xd = data.getField(dX, i) * dX.getNumericScale(); + float yd = data.getField(dY, i) * dY.getNumericScale(); + float zd = data.getField(dZ, i) * dZ.getNumericScale(); typename CloudT::PointType p = cloud.points[i]; - pcl::for_each_type (pcl::SetIfFieldExists (p, "x", xd)); - pcl::for_each_type (pcl::SetIfFieldExists (p, "y", yd)); - pcl::for_each_type (pcl::SetIfFieldExists (p, "z", zd)); + p.x = xd; p.y = yd; p.z = zd; cloud.points[i] = p; } } boost::optional dI = buffer_schema.getDimensionOptional("Intensity"); - bool has_i = false; - float i_val = 0.0f; - pcl::for_each_type (pcl::CopyIfFieldExists (testPoint, "intensity", has_i, i_val)); - - if (has_i && dI) + if (pcl::traits::has_intensity::value && dI) { for (size_t i = 0; i < cloud.points.size(); ++i) { diff --git a/src/filters/PCLBlock.cpp b/src/filters/PCLBlock.cpp index ef204e6a0f..82ce39fb85 100644 --- a/src/filters/PCLBlock.cpp +++ b/src/filters/PCLBlock.cpp @@ -58,6 +58,8 @@ namespace filters boost::uint32_t PCLBlock::processBuffer(PointBuffer& srcData, std::string& filename, PointBuffer& dstData) const { + boost::uint32_t numPointsAfterFiltering = srcData.getNumPoints(); + #ifdef PDAL_HAVE_PCL bool logOutput = log()->getLevel() > logDEBUG1; if (logOutput) @@ -113,18 +115,28 @@ PCLBlock::processBuffer(PointBuffer& srcData, std::string& filename, PointBuffer pcl::Pipeline pipeline; pipeline.setInputCloud(cloud); pipeline.setJSON(filename); + pipeline.setOffsets(dX.getNumericOffset(), dY.getNumericOffset(), dZ.getNumericOffset()); pipeline.filter(*cloud_f); - pdal::PCDtoPDAL(*cloud_f, dstData); + if (cloud_f->points.size() > 0) + { + pdal::PCDtoPDAL(*cloud_f, dstData); + + log()->get(logDEBUG2) << cloud->points.size() << " before, " << cloud_f->points.size() << " after" << std::endl; - log()->get(logDEBUG2) << cloud->points.size() << " before, " << cloud_f->points.size() << " after" << std::endl; + log()->get(logDEBUG2) << dstData.getNumPoints() << std::endl; - log()->get(logDEBUG2) << dstData.getNumPoints() << std::endl; + log()->get(logDEBUG2) << dstData.applyScaling(dX, 0) << ", " << dstData.applyScaling(dY, 0) << ", " << dstData.applyScaling(dZ, 0) << std::endl; + } + else + { + log()->get(logDEBUG2) << "Filtered cloud has no points!" << std::endl; + } - log()->get(logDEBUG2) << dstData.applyScaling(dX, 0) << ", " << dstData.applyScaling(dY, 0) << ", " << dstData.applyScaling(dZ, 0) << std::endl; + numPointsAfterFiltering = cloud_f->points.size(); #endif - return srcData.getNumPoints(); + return numPointsAfterFiltering; } PCLBlock::PCLBlock(Stage& prevStage, const Options& options)