added ray based decimation, slow but perhaps useful

Author: thomas-lowe

raycloudtools/raydecimate/raydecimate.cpp

@@ -23,8 +23,9 @@ void usage(int exit_code = 1)
   std::cout << "raydecimate raycloud 3 cm   - reduces to one end point every 3 cm. A spatially even subsampling" << std::endl;
   std::cout << "raydecimate raycloud 4 rays - reduces to every fourth ray. A temporally even subsampling (if rays are chronological)" << std::endl;
   std::cout << "advanced methods not supported in rayrestore:" << std::endl;
-  std::cout << "raydecimate raycloud 20 cm 64 rays - A maximum of 64 rays per cubic 20 cm. Retains small-scale details compared to spatial decimation" << std::endl;
-  std::cout << "raydecimate raycloud 3 cm/m - reduces to ray ends spaced 3 cm apart for each metre of their length" << std::endl;
+  std::cout << "raydecimate raycloud 20 cm 64 points - A maximum of 64 end points per cubic 20 cm. Retains small-scale details compared to spatial decimation" << std::endl;
+  std::cout << "raydecimate raycloud 20 cm/ray - If all cells overlapping a ray intersect a ray then this way is not added" << std::endl;
+  std::cout << "raydecimate raycloud 3 cm/m - reduces to ray ends spaced 3 cm apart for each metre of their length. Good for maintaining a range of point densities" << std::endl;
   // clang-format off
   exit(exit_code);
 }
@@ -49,20 +50,25 @@ int rayDecimate(int argc, char *argv[])
 {
   ray::FileArgument cloud_file;
   ray::IntArgument num_rays(1, 10000);
+  ray::IntArgument width_for_ray(1, 10000);
   ray::DoubleArgument vox_width(0.01, 100.0);
   ray::DoubleArgument radius_per_length(0.01, 100.0);
-  ray::ValueKeyChoice quantity({ &vox_width, &num_rays, &radius_per_length }, { "cm", "rays", "cm/m" });
-  ray::TextArgument cm("cm"), rays("rays"); 
+  ray::ValueKeyChoice quantity({ &vox_width, &num_rays, &radius_per_length, &width_for_ray }, { "cm", "rays", "cm/m", "cm/ray" });
+  ray::TextArgument cm("cm"), points("points"); 
   bool standard_format = ray::parseCommandLine(argc, argv, { &cloud_file, &quantity });
-  bool double_format = ray::parseCommandLine(argc, argv, { &cloud_file, &vox_width, &cm, &num_rays, &rays });
-  if (!standard_format && !double_format)
+  bool double_format_points = ray::parseCommandLine(argc, argv, { &cloud_file, &vox_width, &cm, &num_rays, &points });
+  if (!standard_format && !double_format_points)
     usage();
 
   bool res = false;
-  if (double_format)
+  if (double_format_points)
   {
     res = ray::decimateSpatioTemporal(cloud_file.nameStub(), vox_width.value(), num_rays.value());
   }
+  else if (quantity.selectedKey() == "cm/ray")
+  {
+    res = ray::decimateRaysSpatial(cloud_file.nameStub(), width_for_ray.value());
+  }
   else if (quantity.selectedKey() == "cm")
   {
     res = ray::decimateSpatial(cloud_file.nameStub(), vox_width.value());

raylib/raydecimation.cpp

@@ -164,6 +164,107 @@ bool decimateSpatioTemporal(const std::string &file_stub, double vox_width, int
   return true;
 }
 
+
+bool decimateRaysSpatial(const std::string &file_stub, double vox_width)
+{
+  ray::CloudWriter writer;
+  if (!writer.begin(file_stub + "_decimated.ply"))
+    return false;
+
+  // By maintaining these buffers below, we avoid almost all memory fragmentation
+  ray::Cloud chunk;
+  std::vector<int64_t> subsample;
+  std::set<Eigen::Vector3i, ray::Vector3iLess> voxel_set;
+
+  auto decimate = [&](std::vector<Eigen::Vector3d> &starts, std::vector<Eigen::Vector3d> &ends,
+                      std::vector<double> &times, std::vector<ray::RGBA> &colours) 
+  {
+    double width = 0.01 * vox_width;
+    subsample.clear();
+    for (int i = 0; i < (int)ends.size(); i++)
+    {
+      // #define END_FIRST // by traversing from end to start we match the existence of the ray more to the free space near the end point. In building1 test start first found more ray locations
+      #if defined END_FIRST
+      Eigen::Vector3d dir = starts[i] - ends[i];
+      const Eigen::Vector3d source = ends[i] / width;
+      const Eigen::Vector3d target = starts[i] / width;
+      #else
+      Eigen::Vector3d dir = ends[i] - starts[i];
+      const Eigen::Vector3d source = starts[i] / width;
+      const Eigen::Vector3d target = ends[i] / width;
+      #endif
+      const double length = dir.norm();
+      for (int a = 0; a<3; a++)
+      {
+        if (dir[a] == 0.0)
+        {
+          dir[a] = 1e-10; // prevent division by 0
+        }
+      }
+      const double eps = 1e-9;  // to stay away from edge cases
+      const double maxDist = (target - source).norm();
+
+      // cached values to speed up the loop below
+      Eigen::Vector3i adds;
+      Eigen::Vector3d offsets;
+      for (int k = 0; k < 3; ++k)
+      {
+        if (dir[k] > 0.0)
+        {
+          adds[k] = 1;
+          offsets[k] = 0.5;
+        }
+        else
+        {
+          adds[k] = -1;
+          offsets[k] = -0.5;
+        }
+      }
+
+      Eigen::Vector3d p = source;  // our moving variable as we walk over the grid
+      Eigen::Vector3i inds((int)std::floor(p[0]), (int)std::floor(p[1]), (int)std::floor(p[2]));
+      if (voxel_set.insert(inds).second)
+      {
+        subsample.push_back(i);
+        continue;
+      }
+      double depth = 0;
+      // for every ray, walk over its length and if there is a free space then add it
+      do
+      {
+        double ls[3] = { (std::round(p[0] + offsets[0]) - p[0]) / dir[0], (std::round(p[1] + offsets[1]) - p[1]) / dir[1],
+                         (std::round(p[2] + offsets[2]) - p[2]) / dir[2] };
+        int axis = (ls[0] < ls[1] && ls[0] < ls[2]) ? 0 : (ls[1] < ls[2] ? 1 : 2);
+        inds[axis] += adds[axis];
+        double minL = ls[axis] * length;
+        depth += minL + eps;
+        p = source + dir * (depth / length);
+        
+        if (voxel_set.insert(inds).second)
+        {
+          subsample.push_back(i);
+          break; // only adding to one cell
+        }
+      } while (depth <= maxDist);
+    }
+    chunk.resize(subsample.size());
+    for (int64_t i = 0; i < (int64_t)subsample.size(); i++)
+    {
+      int64_t id = subsample[i];
+      chunk.starts[i] = starts[id];
+      chunk.ends[i] = ends[id];
+      chunk.colours[i] = colours[id];
+      chunk.times[i] = times[id];
+    }
+    writer.writeChunk(chunk);
+  };
+
+  if (!ray::Cloud::read(file_stub + ".ply", decimate))
+    return false;
+  writer.end();
+  return true;
+}
+
 bool decimateAngular(const std::string &file_stub, double radius_per_length)
 {
   ray::CloudWriter writer;

raylib/raydecimation.h

@@ -27,6 +27,12 @@ bool RAYLIB_EXPORT decimateTemporal(const std::string &file_stub, int num_rays);
 /// This allows a more even distribution of points while maintaining details better than pure spatial decimation
 bool RAYLIB_EXPORT decimateSpatioTemporal(const std::string &file_stub, double vox_width, int num_rays);
 
+/// @brief Maintains a maximum number of rays intersecting each voxel. This has some ambiguity, but is a useful routine
+/// as it maintains the integrity of the full ray cloud including free space, so is better for combine operations
+/// By contrast, standard spatial decimation removes free space whenever the end points coincide 
+bool RAYLIB_EXPORT decimateRaysSpatial(const std::string &file_stub, double vox_width);
+
+
 /// @brief decimate to no more than 1 point per voxel of width @c radius_per_length x ray length. 
 /// This is used when error is proportional to ray length, prioritising closer measurements and leaving distant areas sparse
 bool RAYLIB_EXPORT decimateAngular(const std::string &file_stub, double radius_per_length);