FarthestPointDownSample: arg to specify start index to start downsamp…

…ling from (#7076)

* FPS overload

* namespace tests ends later down below

* FPS overload tests

* FPS input args can be const

* update FPS docstring

* FPS style guide compliance

* default value instead of overload

* overflow check not needed

* updated FPS tests

cpp/open3d/geometry/PointCloud.cpp

@@ -507,7 +507,7 @@ std::shared_ptr<PointCloud> PointCloud::RandomDownSample(
 }
 
 std::shared_ptr<PointCloud> PointCloud::FarthestPointDownSample(
-        size_t num_samples) const {
+        const size_t num_samples, const size_t start_index) const {
     if (num_samples == 0) {
         return std::make_shared<PointCloud>();
     } else if (num_samples == points_.size()) {
@@ -516,6 +516,9 @@ std::shared_ptr<PointCloud> PointCloud::FarthestPointDownSample(
         utility::LogError(
                 "Illegal number of samples: {}, must <= point size: {}",
                 num_samples, points_.size());
+    } else if (start_index >= points_.size()) {
+        utility::LogError("Illegal start index: {}, must < point size: {}",
+                          start_index, points_.size());
     }
     // We can also keep track of the non-selected indices with unordered_set,
     // but since typically num_samples << num_points, it may not be worth it.
@@ -524,7 +527,7 @@ std::shared_ptr<PointCloud> PointCloud::FarthestPointDownSample(
     const size_t num_points = points_.size();
     std::vector<double> distances(num_points,
                                   std::numeric_limits<double>::infinity());
-    size_t farthest_index = 0;
+    size_t farthest_index = start_index;
     for (size_t i = 0; i < num_samples; i++) {
         selected_indices.push_back(farthest_index);
         const Eigen::Vector3d &selected = points_[farthest_index];

cpp/open3d/geometry/PointCloud.h

@@ -173,11 +173,12 @@ class PointCloud : public Geometry3D {
     /// with a set of points has farthest distance.
     ///
     /// The sample is performed by selecting the farthest point from previous
-    /// selected points iteratively.
+    /// selected points iteratively, starting from `start_index`.
     ///
     /// \param num_samples Number of points to be sampled.
+    /// \param start_index Index to start downsampling from.
     std::shared_ptr<PointCloud> FarthestPointDownSample(
-            size_t num_samples) const;
+            const size_t num_samples, const size_t start_index = 0) const;
 
     /// \brief Function to crop pointcloud into output pointcloud
     ///

cpp/open3d/t/geometry/PointCloud.cpp

@@ -384,7 +384,8 @@ PointCloud PointCloud::RandomDownSample(double sampling_ratio) const {
             false, false);
 }
 
-PointCloud PointCloud::FarthestPointDownSample(size_t num_samples) const {
+PointCloud PointCloud::FarthestPointDownSample(const size_t num_samples,
+                                               const size_t start_index) const {
     const core::Dtype dtype = GetPointPositions().GetDtype();
     const int64_t num_points = GetPointPositions().GetLength();
     if (num_samples == 0) {
@@ -395,14 +396,17 @@ PointCloud PointCloud::FarthestPointDownSample(size_t num_samples) const {
         utility::LogError(
                 "Illegal number of samples: {}, must <= point size: {}",
                 num_samples, num_points);
+    } else if (start_index >= size_t(num_points)) {
+        utility::LogError("Illegal start index: {}, must <= point size: {}",
+                          start_index, num_points);
     }
     core::Tensor selection_mask =
             core::Tensor::Zeros({num_points}, core::Bool, GetDevice());
     core::Tensor smallest_distances = core::Tensor::Full(
             {num_points}, std::numeric_limits<double>::infinity(), dtype,
             GetDevice());
 
-    int64_t farthest_index = 0;
+    int64_t farthest_index = static_cast<int64_t>(start_index);
 
     for (size_t i = 0; i < num_samples; i++) {
         selection_mask[farthest_index] = true;

cpp/open3d/t/geometry/PointCloud.h

@@ -352,10 +352,12 @@ class PointCloud : public Geometry, public DrawableGeometry {
     /// points has farthest distance.
     ///
     /// The sampling is performed by selecting the farthest point from previous
-    /// selected points iteratively.
+    /// selected points iteratively, starting from `start_index`.
     ///
     /// \param num_samples Number of points to be sampled.
-    PointCloud FarthestPointDownSample(size_t num_samples) const;
+    /// \param start_index Index to start downsampling from.
+    PointCloud FarthestPointDownSample(const size_t num_samples,
+                                       const size_t start_index = 0) const;
 
     /// \brief Remove points that have less than \p nb_points neighbors in a
     /// sphere of a given radius.

cpp/pybind/geometry/pointcloud.cpp

@@ -90,7 +90,11 @@ void pybind_pointcloud_definitions(py::module &m) {
                  "set of points has farthest distance. The sample is performed "
                  "by selecting the farthest point from previous selected "
                  "points iteratively.",
-                 "num_samples"_a)
+                 "num_samples"_a,
+                 "Index to start downsampling from. Valid index is a "
+                 "non-negative number less than number of points in the "
+                 "input pointcloud.",
+                 "start_index"_a = 0)
             .def("crop",
                  (std::shared_ptr<PointCloud>(PointCloud::*)(
                          const AxisAlignedBoundingBox &, bool) const) &

cpp/pybind/t/geometry/pointcloud.cpp

@@ -256,7 +256,11 @@ void pybind_pointcloud_definitions(py::module& m) {
                    "of points has farthest distance.The sampling is performed "
                    "by selecting the farthest point from previous selected "
                    "points iteratively",
-                   "num_samples"_a);
+                   "num_samples"_a,
+                   "Index to start downsampling from. Valid index is a "
+                   "non-negative number less than number of points in the "
+                   "input pointcloud.",
+                   "start_index"_a = 0);
     pointcloud.def("remove_radius_outliers", &PointCloud::RemoveRadiusOutliers,
                    "nb_points"_a, "search_radius"_a,
                    R"(Remove points that have less than nb_points neighbors in a
@@ -656,7 +660,8 @@ The implementation is inspired by the PCL implementation. Reference:
               "in the pointcloud."}});
     docstring::ClassMethodDocInject(
             m, "PointCloud", "farthest_point_down_sample",
-            {{"num_samples", "Number of points to be sampled."}});
+            {{"num_samples", "Number of points to be sampled."},
+             {"start_index", "Index of point to start downsampling from."}});
     docstring::ClassMethodDocInject(
             m, "PointCloud", "remove_radius_outliers",
             {{"nb_points",

cpp/tests/geometry/PointCloud.cpp

@@ -878,11 +878,17 @@ TEST(PointCloud, FarthestPointDownSample) {
                               {1.0, 1.0, 1.5}});
     std::shared_ptr<geometry::PointCloud> pcd_down =
             pcd.FarthestPointDownSample(4);
-    ExpectEQ(pcd_down->points_, std::vector<Eigen::Vector3d>({{0, 2.0, 0},
-                                                              {1.0, 1.0, 0},
-                                                              {1.0, 0, 1.0},
-                                                              {0, 1.0, 1.0}}));
-}  // namespace tests
+    std::vector<Eigen::Vector3d> expected = {
+            {0, 2.0, 0}, {1.0, 1.0, 0}, {1.0, 0, 1.0}, {0, 1.0, 1.0}};
+
+    std::shared_ptr<geometry::PointCloud> pcd_down_2 =
+            pcd.FarthestPointDownSample(4, 4);
+    std::vector<Eigen::Vector3d> expected_2 = {
+            {0, 2.0, 0}, {1.0, 1.0, 0}, {0, 0, 1.0}, {1.0, 1.0, 1.5}};
+
+    ExpectEQ(pcd_down->points_, expected);
+    ExpectEQ(pcd_down_2->points_, expected_2);
+}
 
 TEST(PointCloud, Crop_AxisAlignedBoundingBox) {
     geometry::AxisAlignedBoundingBox aabb({0, 0, 0}, {2, 2, 2});

cpp/tests/t/geometry/PointCloud.cpp

@@ -954,11 +954,17 @@ TEST_P(PointCloudPermuteDevices, FarthestPointDownSample) {
                                        {0, 1.0, 1.0},
                                        {1.0, 1.0, 1.5}},
                                       device));
+
     auto pcd_small_down = pcd_small.FarthestPointDownSample(4);
-    EXPECT_TRUE(pcd_small_down.GetPointPositions().AllClose(
-            core::Tensor::Init<float>(
-                    {{0, 2.0, 0}, {1.0, 1.0, 0}, {1.0, 0, 1.0}, {0, 1.0, 1.0}},
-                    device)));
+    auto expected = core::Tensor::Init<float>(
+            {{0, 2.0, 0}, {1.0, 1.0, 0}, {1.0, 0, 1.0}, {0, 1.0, 1.0}}, device);
+
+    auto pcd_small_down_2 = pcd_small.FarthestPointDownSample(4, 4);
+    auto expected_2 = core::Tensor::Init<float>(
+            {{0, 2.0, 0}, {1.0, 1.0, 0}, {0, 0, 1.0}, {1.0, 1.0, 1.5}}, device);
+
+    EXPECT_TRUE(pcd_small_down.GetPointPositions().AllClose(expected));
+    EXPECT_TRUE(pcd_small_down_2.GetPointPositions().AllClose(expected_2));
 }
 
 TEST_P(PointCloudPermuteDevices, RemoveRadiusOutliers) {