iss keypoint

README.md

@@ -29,6 +29,8 @@ This repository is based on [Open3D](https://github.com/intel-isl/Open3D).
     * Point cloud features
         * FPFH
         * SHOT
+    * Point cloud keypoints
+        * ISS
     * Point cloud clustering
         * [G-DBSCAN: A GPU Accelerated Algorithm for Density-based Clustering](https://www.sciencedirect.com/science/article/pii/S1877050913003438)
     * Point cloud/Triangle mesh filtering, down sampling

docs/index.md

@@ -17,6 +17,8 @@ This repository is based on [Open3D](https://github.com/intel-isl/Open3D).
     * Point cloud features
         * FPFH
         * SHOT
+    * Point cloud keypoints
+        * ISS
     * Point cloud clustering
         * [G-DBSCAN: A GPU Accelerated Algorithm for Density-based Clustering](https://www.sciencedirect.com/science/article/pii/S1877050913003438)
     * Point cloud/Triangle mesh filtering, down sampling

examples/python/advanced/iss_keypoint_detector.py

@@ -0,0 +1,23 @@
+import os
+import sys
+import time
+import cupoch as cph
+dir_path = os.path.dirname(os.path.realpath(__file__))
+sys.path.append(os.path.join(dir_path, '../misc'))
+import meshes
+
+# Compute ISS Keypoints on Armadillo
+mesh = meshes.armadillo()
+pcd = cph.geometry.PointCloud()
+pcd.points = mesh.vertices
+
+tic = time.time()
+keypoints = cph.geometry.keypoint.compute_iss_keypoints(pcd)
+toc = 1000 * (time.time() - tic)
+print("ISS Computation took {:.0f} [ms]".format(toc))
+print(len(pcd.points), len(keypoints.points))
+
+mesh.compute_vertex_normals()
+mesh.paint_uniform_color([0.5, 0.5, 0.5])
+keypoints.paint_uniform_color([1.0, 0.75, 0.0])
+cph.visualization.draw_geometries([keypoints, mesh])

src/cupoch/geometry/down_sample.cu

@@ -124,6 +124,45 @@ std::shared_ptr<PointCloud> PointCloud::SelectByIndex(
     return output;
 }
 
+std::shared_ptr<PointCloud> PointCloud::SelectByMask(
+        const utility::device_vector<bool> &mask, bool invert) const {
+    auto output = std::make_shared<PointCloud>();
+    if (points_.size() != mask.size()) {
+        utility::LogError("[SelectByMask] The point size should be equal to the mask size.\n");
+        return output;
+    }
+    const bool has_normals = HasNormals();
+    const bool has_colors = HasColors();
+    if (has_normals) output->normals_.resize(mask.size());
+    if (has_colors) output->colors_.resize(mask.size());
+    output->points_.resize(mask.size());
+    auto fn = [invert] __device__ (bool flag) { return invert ? !flag : flag;};
+    if (has_normals && has_colors) {
+        auto begin = make_tuple_begin(output->points_, output->normals_, output->colors_);
+        auto end = thrust::copy_if(make_tuple_begin(points_, normals_, colors_),
+                make_tuple_end(points_, normals_, colors_),
+                mask.begin(), begin, fn);
+        resize_all(thrust::distance(begin, end), output->points_, output->normals_, output->colors_);
+    } else if (has_colors) {
+        auto begin = make_tuple_begin(output->points_, output->colors_);
+        auto end = thrust::copy_if(make_tuple_begin(points_, colors_),
+                make_tuple_end(points_, colors_),
+                mask.begin(), begin, fn);
+        resize_all(thrust::distance(begin, end), output->points_, output->colors_);
+    } else if (has_normals) {
+        auto begin = make_tuple_begin(output->points_, output->normals_);
+        auto end = thrust::copy_if(make_tuple_begin(points_, normals_),
+                make_tuple_end(points_, normals_),
+                mask.begin(), begin, fn);
+        resize_all(thrust::distance(begin, end), output->points_, output->normals_);
+    } else {
+        auto end = thrust::copy_if(points_.begin(), points_.end(),
+                mask.begin(), output->points_.begin(), fn);
+        output->points_.resize(thrust::distance(output->points_.begin(), end));
+    }
+    return output;
+}
+
 std::shared_ptr<PointCloud> PointCloud::VoxelDownSample(
         float voxel_size) const {
     auto output = std::make_shared<PointCloud>();

src/cupoch/geometry/iss_keypoints.cu

@@ -0,0 +1,175 @@
+/**
+ * Copyright (c) 2021 Neka-Nat
+ * Permission is hereby granted, free of charge, to any person obtaining a copy
+ * of this software and associated documentation files (the "Software"), to deal
+ * in the Software without restriction, including without limitation the rights
+ * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+ * copies of the Software, and to permit persons to whom the Software is
+ * furnished to do so, subject to the following conditions:
+ *
+ * The above copyright notice and this permission notice shall be included in
+ * all copies or substantial portions of the Software.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+ * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+ * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+ * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+ * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING
+ * FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS
+ * IN THE SOFTWARE.
+ **/
+#include <thrust/iterator/discard_iterator.h>
+#include <Eigen/Geometry>
+
+#include "cupoch/geometry/geometry_functor.h"
+#include "cupoch/geometry/kdtree_flann.h"
+#include "cupoch/geometry/keypoint.h"
+#include "cupoch/geometry/pointcloud.h"
+#include "cupoch/utility/console.h"
+#include "cupoch/utility/eigenvalue.h"
+#include "cupoch/utility/range.h"
+
+namespace cupoch {
+
+namespace {
+
+float ComputeModelResolution(const geometry::PointCloud& points,
+                             const geometry::KDTreeFlann& kdtree) {
+    utility::device_vector<int> indices;
+    utility::device_vector<float> distance2;
+    kdtree.SearchKNN(points.points_, 2, indices, distance2);
+    thrust::strided_range<
+            utility::device_vector<float>::const_iterator>
+            range_dists(distance2.begin() + 1, distance2.end(), 2);
+    float resolution = thrust::reduce(utility::exec_policy(utility::GetStream(0))
+            ->on(utility::GetStream(0)),
+            range_dists.begin(), range_dists.end(), 0.0f);
+    resolution /= points.points_.size();
+    return std::sqrt(resolution);
+}
+
+__device__ Eigen::Vector3f ComputeThirdEigenValue(const Eigen::Matrix<float, 9, 1> &cum,
+                                                  int count) {
+    Eigen::Matrix<float, 9, 1> cumulants = cum / (float)count;
+    Eigen::Matrix3f covariance;
+    covariance(0, 0) = cumulants(3) - cumulants(0) * cumulants(0);
+    covariance(1, 1) = cumulants(6) - cumulants(1) * cumulants(1);
+    covariance(2, 2) = cumulants(8) - cumulants(2) * cumulants(2);
+    covariance(0, 1) = cumulants(4) - cumulants(0) * cumulants(1);
+    covariance(1, 0) = covariance(0, 1);
+    covariance(0, 2) = cumulants(5) - cumulants(0) * cumulants(2);
+    covariance(2, 0) = covariance(0, 2);
+    covariance(1, 2) = cumulants(7) - cumulants(1) * cumulants(2);
+    covariance(2, 1) = covariance(1, 2);
+    if (covariance.isZero()) return Eigen::Vector3f::Constant(-1.0);
+
+    return utility::FastEigen3x3Val(covariance);
+}
+
+struct compute_third_eigen_values_functor {
+    compute_third_eigen_values_functor(int min_neighbors, float gamma_21, float gamma_32)
+    : min_neighbors_(min_neighbors), gamma_21_(gamma_21), gamma_32_(gamma_32) {};
+    const int min_neighbors_;
+    const float gamma_21_;
+    const float gamma_32_;
+    __device__ float operator()(
+            const thrust::tuple<Eigen::Matrix<float, 9, 1>, int> &x) const {
+        if (thrust::get<1>(x) < min_neighbors_) return -1.0;
+        Eigen::Vector3f eigs = ComputeThirdEigenValue(thrust::get<0>(x), thrust::get<1>(x));
+        return (eigs[2] > 0 && eigs[1] / eigs[2] < gamma_21_ && eigs[0] / eigs[1] < gamma_32_) ? eigs[0] : -1.0;
+    }
+};
+
+struct is_local_maxima_functor {
+    is_local_maxima_functor(const int *indices, const float *third_eigen_values, int knn)
+    : indices_(indices), third_eigen_values_(third_eigen_values), knn_(knn) {}
+    const int *indices_;
+    const float *third_eigen_values_;
+    const int knn_;
+    __device__ bool operator() (size_t idx) {
+        const float query = third_eigen_values_[idx];
+        if (query < 0) return false;
+        for (int k = 0; k < knn_; ++k) {
+            const int l = indices_[idx * knn_ + k];
+            if (l < 0) continue;
+            if (query < third_eigen_values_[l]) {
+                return false;
+            }
+        }
+        return true;
+    }
+};
+
+}  // namespace
+
+namespace geometry {
+namespace keypoint {
+
+std::shared_ptr<PointCloud> ComputeISSKeypoints(
+        const PointCloud& input,
+        float salient_radius /* = 0.0 */,
+        float non_max_radius /* = 0.0 */,
+        float gamma_21 /* = 0.975 */,
+        float gamma_32 /* = 0.975 */,
+        int min_neighbors /*= 5 */,
+        int max_neighbors /*= NUM_MAX_NN */) {
+    if (input.points_.empty()) {
+        utility::LogWarning("[ComputeISSKeypoints] Input PointCloud is empty!");
+        return std::make_shared<PointCloud>();
+    }
+
+    KDTreeFlann kdtree;
+    kdtree.SetGeometry(input);
+    if (salient_radius == 0.0 || non_max_radius == 0.0) {
+        const float resolution = ComputeModelResolution(input, kdtree);
+        salient_radius = 6 * resolution;
+        non_max_radius = 4 * resolution;
+        utility::LogDebug(
+                "[ComputeISSKeypoints] Computed salient_radius = {}, "
+                "non_max_radius = {} from input model",
+                salient_radius, non_max_radius);
+    }
+    utility::device_vector<int> indices;
+    utility::device_vector<float> distance2;
+    kdtree.SearchRadius(input.points_, salient_radius, max_neighbors, indices,
+                        distance2);
+
+    const size_t n_pt = input.points_.size();
+    utility::device_vector<float> third_eigen_values(n_pt, -1);
+    utility::device_vector<Eigen::Matrix<float, 9, 1>> cumulants(n_pt);
+    utility::device_vector<int> counts(n_pt);
+    thrust::repeated_range<thrust::counting_iterator<size_t>> range(
+            thrust::make_counting_iterator<size_t>(0),
+            thrust::make_counting_iterator(n_pt), max_neighbors);
+    thrust::reduce_by_key(
+            utility::exec_policy(0)->on(0), range.begin(), range.end(),
+            thrust::make_transform_iterator(
+                    indices.begin(),
+                    geometry::compute_cumulant_functor(
+                            thrust::raw_pointer_cast(input.points_.data()))),
+            thrust::make_discard_iterator(),
+            make_tuple_begin(cumulants, counts), thrust::equal_to<size_t>(),
+            add_tuple_functor<Eigen::Matrix<float, 9, 1>, int>());
+    thrust::transform(make_tuple_begin(cumulants, counts),
+                      make_tuple_end(cumulants, counts), third_eigen_values.begin(),
+                      compute_third_eigen_values_functor(min_neighbors, gamma_21, gamma_32));
+
+    utility::device_vector<bool> mask(n_pt);
+    kdtree.SearchRadius(input.points_, non_max_radius, max_neighbors, indices,
+                        distance2);
+    is_local_maxima_functor func(thrust::raw_pointer_cast(indices.data()),
+                                 thrust::raw_pointer_cast(third_eigen_values.data()),
+                                 max_neighbors);
+    thrust::transform(thrust::make_counting_iterator<size_t>(0),
+                      thrust::make_counting_iterator(n_pt),
+                      mask.begin(), func);
+
+    auto out = input.SelectByMask(mask);
+    utility::LogDebug("[ComputeISSKeypoints] Extracted {} keypoints",
+                      out->points_.size());
+    return out;
+}
+
+}  // namespace keypoint
+}  // namespace geometry
+}  // namespace cupoch

src/cupoch/geometry/keypoint.h

@@ -0,0 +1,63 @@
+/**
+ * Copyright (c) 2020 Neka-Nat
+ * Permission is hereby granted, free of charge, to any person obtaining a copy
+ * of this software and associated documentation files (the "Software"), to deal
+ * in the Software without restriction, including without limitation the rights
+ * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+ * copies of the Software, and to permit persons to whom the Software is
+ * furnished to do so, subject to the following conditions:
+ *
+ * The above copyright notice and this permission notice shall be included in
+ * all copies or substantial portions of the Software.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+ * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+ * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+ * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+ * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING
+ * FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS
+ * IN THE SOFTWARE.
+ **/
+#pragma once
+
+#include <memory>
+#include "cupoch/geometry/kdtree_search_param.h"
+
+namespace cupoch {
+namespace geometry {
+
+class PointCloud;
+
+namespace keypoint {
+
+/// \brief Function that computes the ISS Keypoints from an input point
+/// cloud. This implements the keypoint detection module proposed in Yu
+/// Zhong ,"Intrinsic Shape Signatures: A Shape Descriptor for 3D Object
+/// Recognition", 2009. The implementation is inspired by the PCL one.
+///
+/// \param input The input PointCloud where to compute the ISS Keypoints.
+/// \param salient_radius The radius of the spherical neighborhood used to
+/// detect the keypoints
+/// \param non_max_radius The non maxima supression radius. If non of
+/// the input parameters are specified or are 0.0, then they will be computed
+/// from the input data, taking into account the Model Resolution.
+/// \param gamma_21 The upper bound on the ratio between the second and the
+/// first eigenvalue
+/// \param gamma_32 The upper bound on the ratio between the third and the
+/// second eigenvalue
+/// \param min_neighbors Minimum number of neighbors that has to be found to
+/// consider a keypoint.
+/// \param max_neighbors Maximum number of neighbors that has to be found to
+/// consider a keypoint.
+/// \authors Ignacio Vizzo and Cyrill Stachniss, University of Bonn.
+std::shared_ptr<PointCloud> ComputeISSKeypoints(const PointCloud &input,
+                                                float salient_radius = 0.0,
+                                                float non_max_radius = 0.0,
+                                                float gamma_21 = 0.975,
+                                                float gamma_32 = 0.975,
+                                                int min_neighbors = 5,
+                                                int max_neighbors = NUM_MAX_NN);
+
+}  // namespace keypoint
+}  // namespace geometry
+}  // namespace cupoch

src/cupoch/geometry/pointcloud.h

@@ -115,6 +115,10 @@ class PointCloud : public GeometryBase3D {
             const utility::device_vector<size_t> &indices,
             bool invert = false) const;
 
+    std::shared_ptr<PointCloud> SelectByMask(
+            const utility::device_vector<bool> &mask,
+            bool invert = false) const;
+
     /// Function to downsample \param input pointcloud into output pointcloud
     /// with a voxel \param voxel_size defines the resolution of the voxel grid,
     /// smaller value leads to denser output point cloud. Normals and colors are

src/python/README.md

@@ -11,6 +11,8 @@
     * Point cloud features
         * FPFH
         * SHOT
+    * Point cloud keypoints
+        * ISS
     * Point cloud clustering
         * [G-DBSCAN: A GPU Accelerated Algorithm for Density-based Clustering](https://www.sciencedirect.com/science/article/pii/S1877050913003438)
     * Point cloud/Triangle mesh filtering, down sampling

src/python/cupoch_pybind/device_vector_wrapper.cu

@@ -125,6 +125,7 @@ template class device_vector_wrapper<Eigen::Vector3i>;
 template class device_vector_wrapper<Eigen::Vector2i>;
 template class device_vector_wrapper<Eigen::Matrix<float, 33, 1>>;
 template class device_vector_wrapper<Eigen::Matrix<float, 352, 1>>;
+template class device_vector_wrapper<bool>;
 template class device_vector_wrapper<float>;
 template class device_vector_wrapper<int>;
 template class device_vector_wrapper<size_t>;
@@ -155,6 +156,8 @@ template void FromWrapper<Eigen::Matrix<float, 33, 1>>(
 template void FromWrapper<Eigen::Matrix<float, 352, 1>>(
         utility::device_vector<Eigen::Matrix<float, 352, 1>>& dv,
         const device_vector_wrapper<Eigen::Matrix<float, 352, 1>>& vec);
+template void FromWrapper<bool>(utility::device_vector<bool>& dv,
+                                const device_vector_wrapper<bool>& vec);
 template void FromWrapper<float>(utility::device_vector<float>& dv,
                                  const device_vector_wrapper<float>& vec);
 template void FromWrapper<int>(utility::device_vector<int>& dv,

src/python/cupoch_pybind/device_vector_wrapper.h

@@ -82,6 +82,7 @@ using device_vector_vector352f =
 using device_vector_int = device_vector_wrapper<int>;
 using device_vector_size_t = device_vector_wrapper<size_t>;
 using device_vector_float = device_vector_wrapper<float>;
+using device_vector_bool = device_vector_wrapper<bool>;
 using device_vector_occupancyvoxel =
         device_vector_wrapper<geometry::OccupancyVoxel>;
 using device_vector_primitives =

src/python/cupoch_pybind/geometry/geometry.cpp

@@ -172,6 +172,7 @@ void pybind_geometry(py::module &m) {
     pybind_geometry_classes(m_submodule);
     pybind_kdtreeflann(m_submodule);
     pybind_pointcloud(m_submodule);
+    pybind_keypoint(m_submodule);
     pybind_voxelgrid(m_submodule);
     pybind_occupanygrid(m_submodule);
     pybind_distancetransform(m_submodule);

src/python/cupoch_pybind/geometry/geometry.h

@@ -24,6 +24,7 @@
 void pybind_geometry(py::module &m);
 
 void pybind_pointcloud(py::module &m);
+void pybind_keypoint(py::module &m);
 void pybind_voxelgrid(py::module &m);
 void pybind_occupanygrid(py::module &m);
 void pybind_distancetransform(py::module &m);