Added K-D tree search to speed up querying the closest points

CRAN-SUBMISSION

@@ -1,3 +1,3 @@
-Version: 0.1.9
-Date: 2024-11-08 17:19:07 UTC
-SHA: 8e9e71be96af24ec2444a59a4113abdf02983ec1
+Version: 0.2.0
+Date: 2024-12-16 19:26:22 UTC
+SHA: 9f0b33e5d12e58814edbf6a5b83489c9b1de0d3e

DESCRIPTION

@@ -1,7 +1,7 @@
 Package: ravetools
 Type: Package
 Title: Signal and Image Processing Toolbox for Analyzing Intracranial Electroencephalography Data
-Version: 0.2.0
+Version: 0.2.0.9000
 Language: en-US
 Authors@R: c(
     person("Zhengjia", "Wang", email = "[email protected]", role = c("aut", "cre")),

NAMESPACE

@@ -113,6 +113,7 @@ export(rgl_view)
 export(shift_array)
 export(surface_path)
 export(vcg_isosurface)
+export(vcg_kdtree_nearest)
 export(vcg_mesh_volume)
 export(vcg_raycaster)
 export(vcg_smooth_explicit)

R/RcppExports.R

@@ -617,6 +617,10 @@ vcgRaycaster <- function(vb_, it_, rayOrigin, rayDirection, maxDistance, bothSid
     .Call(`_ravetools_vcgRaycaster`, vb_, it_, rayOrigin, rayDirection, maxDistance, bothSides, threads)
 }
 
+vcgKDTreeSearch <- function(target_, query_, k, nPointsPerCell = 16L, maxDepth = 64L) {
+    .Call(`_ravetools_vcgKDTreeSearch`, target_, query_, k, nPointsPerCell, maxDepth)
+}
+
 # Register entry points for exported C++ functions
 methods::setLoadAction(function(ns) {
     .Call(`_ravetools_RcppExport_registerCCallable`)

R/vcg.R

@@ -571,3 +571,116 @@ vcg_raycaster <- function(
   )
 
 }
+
+#' @title Find nearest \code{k} points
+#' @description
+#' For each point in the query, find the nearest \code{k} points in target using
+#' \code{K-D} tree.
+#' @param target a matrix with \code{n} rows (number of points) and 2 or 3
+#' columns, or a \code{mesh3d} object. This is the target point cloud where
+#' nearest distances will be sought
+#' @param query a matrix with \code{n} rows (number of points) and 2 or 3
+#' columns, or a \code{mesh3d} object. This is the query point cloud where
+#' for each point, the nearest \code{k} points in \code{target} will be sought.
+#' @param k positive number of nearest neighbors to look for
+#' @param leaf_size the suggested leaf size for the \code{K-D} tree; default is
+#' \code{16}; larger leaf size will result in smaller depth
+#' @param max_depth maximum depth of the \code{K-D} tree; default is \code{64}
+#' @returns A list of two matrices: \code{index} is a matrix of indices of
+#' \code{target} points, whose distances are close to the corresponding
+#' \code{query} point. If no point in \code{target} is found, then \code{NA}
+#' will be presented. Each \code{distance} is the corresponding distance
+#' from the query point to the target point.
+#'
+#' @examples
+#'
+#' # Find nearest point in B with the smallest distance for each point in A
+#'
+#' library(ravetools)
+#'
+#' n <- 10
+#' A <- matrix(rnorm(n * 2), nrow = n)
+#' B <- matrix(rnorm(n * 4), nrow = n * 2)
+#' result <- vcg_kdtree_nearest(
+#'   target = B, query = A,
+#'    k = 1
+#' )
+#'
+#' plot(
+#'   rbind(A, B),
+#'   pch = 20,
+#'   col = c(rep("red", n), rep("black", n * 2)),
+#'   xlab = "x",
+#'   ylab = "y",
+#'   main = "Black: target; Red: query"
+#' )
+#'
+#' nearest_points <- B[result$index, ]
+#' arrows(A[, 1],
+#'        A[, 2],
+#'        nearest_points[, 1],
+#'        nearest_points[, 2],
+#'        col = "red",
+#'        length = 0.1)
+#'
+#' # ---- Sanity check ------------------------------------------------
+#' nearest_index <- apply(A, 1, function(pt) {
+#'   which.min(colSums((t(B) - pt) ^ 2))
+#' })
+#'
+#' result$index == nearest_index
+#'
+#'
+#'
+#' @export
+vcg_kdtree_nearest <- function(
+    target, query, k = 1, leaf_size = 16, max_depth = 64) {
+
+  get_point_cloud <- function(x) {
+    if(is.matrix(x) || is.array(x)) {
+      x <- x[drop = FALSE]
+      stopifnot2(
+        is.matrix(x) && ncol(x) %in% c(2, 3),
+        msg = "`vcg_kdtree_nearest`: input `x` must be a column-matrix with 2 or 3 columns and `n` rows as number of points."
+      )
+      if( ncol(x)  == 2 ) {
+        x <- cbind(x, 0)
+      }
+      x <- t(x)
+    } else {
+      x <- meshintegrity(mesh = x, facecheck = FALSE)
+      x <- x$vb[1:3, , drop = FALSE]
+    }
+    x
+  }
+
+  target <- get_point_cloud(target)
+  query <- get_point_cloud(query)
+
+  k <- as.integer(k)
+  if(!is.finite(k) || k <= 0) {
+    stop("`vcg_kdtree_nearest`: `k` must be finite positive.")
+  }
+
+  leaf_size <- as.integer(leaf_size)
+  if(!is.finite(leaf_size) || leaf_size <= 0) {
+    stop("`vcg_kdtree_nearest`: `leaf_size` must be finite positive.")
+  }
+
+  max_depth <- as.integer(max_depth)
+  if(!is.finite(max_depth) || max_depth <= 0) {
+    stop("`vcg_kdtree_nearest`: `max_depth` must be finite positive.")
+  }
+
+  result <- vcgKDTreeSearch(
+    target_ = target,
+    query_ = query,
+    k = k,
+    nPointsPerCell = leaf_size,
+    maxDepth = max_depth
+  )
+  result$index <- result$index + 1L
+  result$index[result$index == 0] <- NA_integer_
+  result
+
+}

ravetools.Rproj

@@ -17,7 +17,6 @@ StripTrailingWhitespace: Yes
 
 BuildType: Package
 PackageUseDevtools: Yes
-PackageCleanBeforeInstall: No
 PackageInstallArgs: --no-multiarch --with-keep.source
 PackageCheckArgs: --as-cran --run-donttest
 PackageRoxygenize: rd,collate,namespace,vignette

src/RcppExports.cpp

@@ -3608,7 +3608,7 @@ BEGIN_RCPP
 END_RCPP
 }
 // vcgRaycaster
-RcppExport SEXP vcgRaycaster(SEXP vb_, SEXP it_, const Rcpp::NumericVector& rayOrigin, const Rcpp::NumericVector& rayDirection, const float& maxDistance, const bool& bothSides, const int& threads);
+SEXP vcgRaycaster(SEXP vb_, SEXP it_, const Rcpp::NumericVector& rayOrigin, const Rcpp::NumericVector& rayDirection, const float& maxDistance, const bool& bothSides, const int& threads);
 RcppExport SEXP _ravetools_vcgRaycaster(SEXP vb_SEXP, SEXP it_SEXP, SEXP rayOriginSEXP, SEXP rayDirectionSEXP, SEXP maxDistanceSEXP, SEXP bothSidesSEXP, SEXP threadsSEXP) {
 BEGIN_RCPP
     Rcpp::RObject rcpp_result_gen;
@@ -3624,6 +3624,21 @@ BEGIN_RCPP
     return rcpp_result_gen;
 END_RCPP
 }
+// vcgKDTreeSearch
+SEXP vcgKDTreeSearch(SEXP target_, SEXP query_, unsigned int k, unsigned int nPointsPerCell, unsigned int maxDepth);
+RcppExport SEXP _ravetools_vcgKDTreeSearch(SEXP target_SEXP, SEXP query_SEXP, SEXP kSEXP, SEXP nPointsPerCellSEXP, SEXP maxDepthSEXP) {
+BEGIN_RCPP
+    Rcpp::RObject rcpp_result_gen;
+    Rcpp::RNGScope rcpp_rngScope_gen;
+    Rcpp::traits::input_parameter< SEXP >::type target_(target_SEXP);
+    Rcpp::traits::input_parameter< SEXP >::type query_(query_SEXP);
+    Rcpp::traits::input_parameter< unsigned int >::type k(kSEXP);
+    Rcpp::traits::input_parameter< unsigned int >::type nPointsPerCell(nPointsPerCellSEXP);
+    Rcpp::traits::input_parameter< unsigned int >::type maxDepth(maxDepthSEXP);
+    rcpp_result_gen = Rcpp::wrap(vcgKDTreeSearch(target_, query_, k, nPointsPerCell, maxDepth));
+    return rcpp_result_gen;
+END_RCPP
+}
 
 // validate (ensure exported C++ functions exist before calling them)
 static int _ravetools_RcppExport_validate(const char* sig) { 
@@ -3948,6 +3963,7 @@ static const R_CallMethodDef CallEntries[] = {
     {"_ravetools_vcgSphere", (DL_FUNC) &_ravetools_vcgSphere, 2},
     {"_ravetools_vcgDijkstra", (DL_FUNC) &_ravetools_vcgDijkstra, 4},
     {"_ravetools_vcgRaycaster", (DL_FUNC) &_ravetools_vcgRaycaster, 7},
+    {"_ravetools_vcgKDTreeSearch", (DL_FUNC) &_ravetools_vcgKDTreeSearch, 5},
     {"_ravetools_RcppExport_registerCCallable", (DL_FUNC) &_ravetools_RcppExport_registerCCallable, 0},
     {NULL, NULL, 0}
 };

src/vcgCommon.cpp

@@ -571,7 +571,7 @@ SEXP vcgDijkstra(SEXP vb_, SEXP it_, const Rcpp::IntegerVector & source, const d
 
 
 // [[Rcpp::export]]
-RcppExport SEXP vcgRaycaster(
+SEXP vcgRaycaster(
     SEXP vb_ , SEXP it_,
     const Rcpp::NumericVector & rayOrigin, // 3 x n matrix
     const Rcpp::NumericVector & rayDirection,
@@ -728,3 +728,64 @@ RcppExport SEXP vcgRaycaster(
   }
   return R_NilValue;
 }
+
+
+// [[Rcpp::export]]
+SEXP vcgKDTreeSearch(
+    SEXP target_, SEXP query_,
+    unsigned int k,
+    unsigned int nPointsPerCell = 16,
+    unsigned int maxDepth = 64
+)
+{
+  try {
+
+    ravetools::MyPointCloud target, query;
+    ravetools::IOMesh<ravetools::MyPointCloud>::vcgReadR(target, target_);
+    ravetools::IOMesh<ravetools::MyPointCloud>::vcgReadR(query, query_);
+
+    // List out = Rvcg::KDtree< PcMesh, PcMesh >::KDtreeIO(target, query, k,nofP, mDepth,threads);
+    // typedef std::pair<float,int> mypair;
+    Rcpp::IntegerMatrix index(query.vn, k);
+    Rcpp::NumericMatrix distance(query.vn, k);
+    std::fill(index.begin(), index.end(), -1);
+
+    vcg::VertexConstDataWrapper<ravetools::MyPointCloud> targetWrapper(target);
+    vcg::KdTree<float> tree(targetWrapper, nPointsPerCell, maxDepth);
+
+    //tree.setMaxNofNeighbors(k);
+    vcg::KdTree<float>::PriorityQueue queue;
+
+    std::vector< std::pair<float,int> > sortPairs;
+
+    for (int i = 0; i < query.vn; i++) {
+      tree.doQueryK(query.vert[i].cP(), k, queue);
+      int neighbors = queue.getNofElements();
+
+      sortPairs.clear();
+
+      for (int j = 0; j < neighbors; j++) {
+        int neightId = queue.getIndex(j);
+        float dist = Distance(query.vert[i].cP(), target.vert[neightId].cP());
+        sortPairs.push_back( std::pair<float,int>(dist, neightId) );
+      }
+
+      std::sort(sortPairs.begin(), sortPairs.end());
+      for (int j = 0; j < neighbors; j++){
+        index(i, j) = sortPairs[j].second;
+        distance(i, j) = sortPairs[j].first;
+      }
+    }
+
+    return Rcpp::List::create(
+      Rcpp::Named("index") = index,
+      Rcpp::Named("distance") = distance
+    );
+
+  } catch (std::exception& e) {
+    Rcpp::stop( e.what() );
+  } catch (...) {
+    Rcpp::stop("unknown exception");
+  }
+  return R_NilValue; // -Wall
+}

src/vcgCommon.h

@@ -20,6 +20,7 @@
 
 #include <vcg/space/index/grid_static_ptr.h>
 #include <vcg/space/index/spatial_hashing.h>
+#include <vcg/space/index/kdtree/kdtree.h>
 #include <vcg/space/point3.h>
 
 #include <vcg/complex/algorithms/update/bounding.h>
@@ -132,6 +133,27 @@ typedef  MyMesh::FaceContainer FaceContainer;
 /*typedef MyMesh::ConstVertexIterator ConstVertexIterator;
 typedef MyMesh::ConstFaceIterator   ConstFaceIterator;*/
 
+// for point clouds
+class MyPointCloudVertex;
+struct MyPointCloudUsedTypes : public vcg::UsedTypes<
+  vcg::Use<MyPointCloudVertex>::AsVertexType
+>{};
+
+class MyPointCloudEdge : public vcg::Edge<MyPointCloudUsedTypes> {};
+
+class MyPointCloudVertex : public vcg::Vertex<
+  MyPointCloudUsedTypes,
+  // vertex::InfoOcf,
+  vcg::vertex::Coord3f,
+  vcg::vertex::BitFlags,
+  vcg::vertex::Normal3f,
+  vcg::vertex::Mark,
+  vcg::vertex::Color4b,
+  vcg::vertex::Qualityf
+>{};
+
+class MyPointCloud : public vcg::tri::TriMesh< std::vector<MyPointCloudVertex> > {};
+
 
 
 template <class VOX_TYPE>