FarthestPointDownSample: arg to specify start index to start downsampling from

cpp/open3d/geometry/PointCloud.cpp

@@ -507,7 +507,13 @@ std::shared_ptr<PointCloud> PointCloud::RandomDownSample(
 }
 
 std::shared_ptr<PointCloud> PointCloud::FarthestPointDownSample(
-        size_t num_samples) const {
+        const size_t num_samples) const {
+    constexpr size_t start_index = 0;
+    return FarthestPointDownSample(num_samples, start_index);
+}
+
+std::shared_ptr<PointCloud> PointCloud::FarthestPointDownSample(
+        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 +522,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 +533,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

@@ -177,7 +177,18 @@ class PointCloud : public Geometry3D {
     ///
     /// \param num_samples Number of points to be sampled.
     std::shared_ptr<PointCloud> FarthestPointDownSample(
-            size_t num_samples) const;
+            const size_t num_samples) const;
+
+    /// \brief Function to downsample input pointcloud into output pointcloud
+    /// with a set of points has farthest distance.
+    ///
+    /// The sample is performed by selecting the farthest point from previous
+    /// 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(
+            const size_t num_samples, const size_t start_index) const;
 
     /// \brief Function to crop pointcloud into output pointcloud
     ///

cpp/open3d/t/geometry/PointCloud.cpp

@@ -384,7 +384,13 @@ 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 {
+    constexpr size_t start_index = 0;
+    return FarthestPointDownSample(num_samples, start_index);
+}
+
+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 +401,21 @@ 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);
+    } else if (start_index >
+               static_cast<size_t>(std::numeric_limits<int64_t>::max())) {
+        utility::LogError("Illegal start index: {}, must <= int64_t max: {}",
+                          start_index, std::numeric_limits<int64_t>::max());
     }
     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

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

cpp/pybind/geometry/pointcloud.cpp

@@ -85,12 +85,25 @@ void pybind_pointcloud_definitions(py::module &m) {
                  "sampling the indexes from the point cloud.",
                  "sampling_ratio"_a)
             .def("farthest_point_down_sample",
-                 &PointCloud::FarthestPointDownSample,
+                 py::overload_cast<size_t>(&PointCloud::FarthestPointDownSample,
+                                           py::const_),
                  "Downsamples input pointcloud into output pointcloud with a "
                  "set of points has farthest distance. The sample is performed "
                  "by selecting the farthest point from previous selected "
                  "points iteratively.",
                  "num_samples"_a)
+            .def("farthest_point_down_sample",
+                 py::overload_cast<size_t, size_t>(
+                         &PointCloud::FarthestPointDownSample, py::const_),
+                 "Downsamples input pointcloud into output pointcloud with a "
+                 "set of points has farthest distance. The sample is performed "
+                 "by selecting the farthest point from previous selected "
+                 "points iteratively.",
+                 "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)
             .def("crop",
                  (std::shared_ptr<PointCloud>(PointCloud::*)(
                          const AxisAlignedBoundingBox &, bool) const) &

cpp/pybind/t/geometry/pointcloud.cpp

@@ -251,12 +251,25 @@ void pybind_pointcloud_definitions(py::module& m) {
                    "and its attributes.",
                    "sampling_ratio"_a);
     pointcloud.def("farthest_point_down_sample",
-                   &PointCloud::FarthestPointDownSample,
+                   py::overload_cast<size_t>(
+                           &PointCloud::FarthestPointDownSample, py::const_),
                    "Downsample a pointcloud into output pointcloud with a set "
                    "of points has farthest distance.The sampling is performed "
                    "by selecting the farthest point from previous selected "
                    "points iteratively",
                    "num_samples"_a);
+    pointcloud.def("farthest_point_down_sample",
+                   py::overload_cast<size_t, size_t>(
+                           &PointCloud::FarthestPointDownSample, py::const_),
+                   "Downsample a pointcloud into output pointcloud with a set "
+                   "of points has farthest distance.The sampling is performed "
+                   "by selecting the farthest point from previous selected "
+                   "points iteratively",
+                   "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);
     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 +669,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

@@ -876,13 +876,15 @@ TEST(PointCloud, FarthestPointDownSample) {
                               {1.0, 0, 1.0},
                               {0, 1.0, 1.0},
                               {1.0, 1.0, 1.5}});
+    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 =
             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::shared_ptr<geometry::PointCloud> pcd_down_2 =
+            pcd.FarthestPointDownSample(4, 0);
+    ExpectEQ(pcd_down->points_, expected);
+    ExpectEQ(pcd_down_2->points_, expected);
+}
 
 TEST(PointCloud, Crop_AxisAlignedBoundingBox) {
     geometry::AxisAlignedBoundingBox aabb({0, 0, 0}, {2, 2, 2});

cpp/tests/t/geometry/PointCloud.cpp

@@ -954,11 +954,13 @@ TEST_P(PointCloudPermuteDevices, FarthestPointDownSample) {
                                        {0, 1.0, 1.0},
                                        {1.0, 1.0, 1.5}},
                                       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 = 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 pcd_small_down_2 = pcd_small.FarthestPointDownSample(4, 0);
+    EXPECT_TRUE(pcd_small_down.GetPointPositions().AllClose(expected));
+    EXPECT_TRUE(pcd_small_down_2.GetPointPositions().AllClose(expected));
 }
 
 TEST_P(PointCloudPermuteDevices, RemoveRadiusOutliers) {