From 9121440ec9a7e3a938803a3a505b9d209463da08 Mon Sep 17 00:00:00 2001
From: "k.koide" <k.koide@aist.go.jp>
Date: Tue, 29 Oct 2024 15:25:20 +0900
Subject: [PATCH] add function to compute point distances

---
 .../gtsam_points/types/point_cloud_cpu.hpp    | 24 ++++++++++++++
 src/gtsam_points/types/point_cloud_cpu.cpp    | 33 +++++++++++++++++++
 2 files changed, 57 insertions(+)

diff --git a/include/gtsam_points/types/point_cloud_cpu.hpp b/include/gtsam_points/types/point_cloud_cpu.hpp
index 5eadcf10..e5dcd3cb 100644
--- a/include/gtsam_points/types/point_cloud_cpu.hpp
+++ b/include/gtsam_points/types/point_cloud_cpu.hpp
@@ -263,6 +263,30 @@ PointCloudCPU::Ptr remove_outliers(const PointCloud::ConstPtr& points, const std
  */
 PointCloudCPU::Ptr remove_outliers(const PointCloud::ConstPtr& points, const int k = 10, const double std_thresh = 1.0, const int num_threads = 1);
 
+/**
+ * @brief Compute point distances
+ * @param points          Input points
+ * @param max_scan_count  Maximum number of points to scan
+ * @return                Distances
+ */
+std::vector<double> distances(const PointCloud::ConstPtr& points, size_t max_scan_count = std::numeric_limits<size_t>::max());
+
+/**
+ * @brief Compute min and max point distances
+ * @param points          Input points
+ * @param max_scan_count  Maximum number of points to scan
+ * @return                Min and max distances
+ */
+std::pair<double, double> minmax_distance(const PointCloud::ConstPtr& points, size_t max_scan_count = std::numeric_limits<size_t>::max());
+
+/**
+ * @brief Compute median point distance
+ * @param points          Input points
+ * @param max_scan_count  Maximum number of points to scan
+ * @return                Median distance
+ */
+double median_distance(const PointCloud::ConstPtr& points, size_t max_scan_count = std::numeric_limits<size_t>::max());
+
 /**
  * @brief Merge a set of frames into one frame
  * @note  This function only merges points and covs and discard other point attributes.
diff --git a/src/gtsam_points/types/point_cloud_cpu.cpp b/src/gtsam_points/types/point_cloud_cpu.cpp
index c902e8b3..5cb0a0dc 100644
--- a/src/gtsam_points/types/point_cloud_cpu.cpp
+++ b/src/gtsam_points/types/point_cloud_cpu.cpp
@@ -928,4 +928,37 @@ PointCloudCPU::Ptr remove_outliers(const PointCloud::ConstPtr& frame, const int
   return remove_outliers(frame, neighbors, k, std_thresh);
 }
 
+std::vector<double> distances(const PointCloud::ConstPtr& points, size_t max_scan_count) {
+  std::vector<double> dists;
+  dists.reserve(points->size() < max_scan_count ? points->size() : max_scan_count * 2);
+
+  const size_t step = points->size() < max_scan_count ? 1 : points->size() / max_scan_count;
+  for (size_t i = 0; i < points->size(); i += step) {
+    const auto& p = points->points[i];
+    dists.emplace_back(p.head<3>().norm());
+  }
+
+  return dists;
+}
+
+std::pair<double, double> minmax_distance(const PointCloud::ConstPtr& points, size_t max_scan_count) {
+  const auto dists = distances(points, max_scan_count);
+  if (dists.empty()) {
+    return {0.0, 0.0};
+  }
+
+  const auto minmax = std::minmax_element(dists.begin(), dists.end());
+  return {*minmax.first, *minmax.second};
+}
+
+double median_distance(const PointCloud::ConstPtr& points, size_t max_scan_count) {
+  auto dists = distances(points, max_scan_count);
+  if (dists.empty()) {
+    return 0.0;
+  }
+
+  std::nth_element(dists.begin(), dists.begin() + dists.size() / 2, dists.end());
+  return dists[dists.size() / 2];
+}
+
 }  // namespace gtsam_points