diff --git a/cpp/benchmarks/t/pipelines/CMakeLists.txt b/cpp/benchmarks/t/pipelines/CMakeLists.txt index 551a0cb4c09..7689ece2201 100644 --- a/cpp/benchmarks/t/pipelines/CMakeLists.txt +++ b/cpp/benchmarks/t/pipelines/CMakeLists.txt @@ -1,4 +1,5 @@ target_sources(benchmarks PRIVATE odometry/RGBDOdometry.cpp + registration/Feature.cpp registration/Registration.cpp ) diff --git a/cpp/benchmarks/t/pipelines/registration/Feature.cpp b/cpp/benchmarks/t/pipelines/registration/Feature.cpp new file mode 100644 index 00000000000..b1a674ba342 --- /dev/null +++ b/cpp/benchmarks/t/pipelines/registration/Feature.cpp @@ -0,0 +1,152 @@ +// ---------------------------------------------------------------------------- +// - Open3D: www.open3d.org - +// ---------------------------------------------------------------------------- +// The MIT License (MIT) +// +// Copyright (c) 2018-2021 www.open3d.org +// +// 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 "open3d/t/pipelines/registration/Feature.h" + +#include + +#include "open3d/core/CUDAUtils.h" +#include "open3d/data/Dataset.h" +#include "open3d/geometry/PointCloud.h" +#include "open3d/pipelines/registration/Feature.h" +#include "open3d/t/geometry/PointCloud.h" +#include "open3d/t/io/PointCloudIO.h" + +namespace open3d { +namespace t { +namespace pipelines { +namespace registration { + +data::BunnyMesh pointcloud_ply; +static const std::string path = pointcloud_ply.GetPath(); + +void LegacyComputeFPFHFeature(benchmark::State& state, + utility::optional max_nn, + utility::optional radius) { + auto pcd = open3d::io::CreatePointCloudFromFile(path)->UniformDownSample(3); + pcd->EstimateNormals(); + for (auto _ : state) { + if (max_nn.has_value() && radius.has_value()) { + auto fpfh = open3d::pipelines::registration::ComputeFPFHFeature( + *pcd, open3d::geometry::KDTreeSearchParamHybrid( + radius.value(), max_nn.value())); + } else if (max_nn.has_value() && !radius.has_value()) { + auto fpfh = open3d::pipelines::registration::ComputeFPFHFeature( + *pcd, + open3d::geometry::KDTreeSearchParamKNN(max_nn.value())); + } else if (!max_nn.has_value() && radius.has_value()) { + auto fpfh = open3d::pipelines::registration::ComputeFPFHFeature( + *pcd, + open3d::geometry::KDTreeSearchParamRadius(radius.value())); + } + } +} + +void ComputeFPFHFeature(benchmark::State& state, + const core::Device& device, + const core::Dtype& dtype, + utility::optional max_nn, + utility::optional radius) { + t::geometry::PointCloud pcd; + t::io::ReadPointCloud(path, pcd); + pcd = pcd.To(device).UniformDownSample(3); + pcd.SetPointPositions(pcd.GetPointPositions().To(dtype)); + pcd.EstimateNormals(); + + core::Tensor fpfh; + // Warm up. + fpfh = t::pipelines::registration::ComputeFPFHFeature(pcd, max_nn, radius); + + for (auto _ : state) { + fpfh = t::pipelines::registration::ComputeFPFHFeature(pcd, max_nn, + radius); + core::cuda::Synchronize(device); + } +} + +BENCHMARK_CAPTURE(LegacyComputeFPFHFeature, + Legacy Hybrid[0.01 | 100], + 100, + 0.01) + ->Unit(benchmark::kMillisecond); +BENCHMARK_CAPTURE(LegacyComputeFPFHFeature, Legacy Hybrid[0.02 | 50], 50, 0.02) + ->Unit(benchmark::kMillisecond); +BENCHMARK_CAPTURE(LegacyComputeFPFHFeature, + Legacy Hybrid[0.02 | 100], + 100, + 0.02) + ->Unit(benchmark::kMillisecond); +BENCHMARK_CAPTURE(LegacyComputeFPFHFeature, + Legacy KNN[50], + 50, + utility::nullopt) + ->Unit(benchmark::kMillisecond); +BENCHMARK_CAPTURE(LegacyComputeFPFHFeature, + Legacy KNN[100], + 100, + utility::nullopt) + ->Unit(benchmark::kMillisecond); +BENCHMARK_CAPTURE(LegacyComputeFPFHFeature, + Legacy Radius[0.01], + utility::nullopt, + 0.01) + ->Unit(benchmark::kMillisecond); +BENCHMARK_CAPTURE(LegacyComputeFPFHFeature, + Legacy Radius[0.02], + utility::nullopt, + 0.02) + ->Unit(benchmark::kMillisecond); + +#define ENUM_FPFH_METHOD_DEVICE(METHOD_NAME, MAX_NN, RADIUS, DEVICE) \ + BENCHMARK_CAPTURE(ComputeFPFHFeature, METHOD_NAME##_Float32, \ + core::Device(DEVICE), core::Float32, MAX_NN, RADIUS) \ + ->Unit(benchmark::kMillisecond); \ + BENCHMARK_CAPTURE(ComputeFPFHFeature, DEVICE METHOD_NAME##_Float64, \ + core::Device(DEVICE), core::Float32, MAX_NN, RADIUS) \ + ->Unit(benchmark::kMillisecond); + +ENUM_FPFH_METHOD_DEVICE(CPU[0.02 | 50] Hybrid, 100, 0.01, "CPU:0") +ENUM_FPFH_METHOD_DEVICE(CPU[0.02 | 50] Hybrid, 50, 0.02, "CPU:0") +ENUM_FPFH_METHOD_DEVICE(CPU[0.02 | 100] Hybrid, 100, 0.02, "CPU:0") +ENUM_FPFH_METHOD_DEVICE(CPU[50] KNN, 50, utility::nullopt, "CPU:0") +ENUM_FPFH_METHOD_DEVICE(CPU[100] KNN, 100, utility::nullopt, "CPU:0") +ENUM_FPFH_METHOD_DEVICE(CPU[0.01] Radius, utility::nullopt, 0.01, "CPU:0") +ENUM_FPFH_METHOD_DEVICE(CPU[0.02] Radius, utility::nullopt, 0.02, "CPU:0") + +#ifdef BUILD_CUDA_MODULE +ENUM_FPFH_METHOD_DEVICE(CUDA[0.02 | 50] Hybrid, 100, 0.01, "CUDA:0") +ENUM_FPFH_METHOD_DEVICE(CUDA[0.02 | 50] Hybrid, 50, 0.01, "CUDA:0") +ENUM_FPFH_METHOD_DEVICE(CUDA[0.02 | 100] Hybrid, 100, 0.02, "CUDA:0") +ENUM_FPFH_METHOD_DEVICE(CUDA[50] KNN, 50, utility::nullopt, "CUDA:0") +ENUM_FPFH_METHOD_DEVICE(CUDA[100] KNN, 100, utility::nullopt, "CUDA:0") +ENUM_FPFH_METHOD_DEVICE(CUDA[0.01] Radius, utility::nullopt, 0.01, "CUDA:0") +ENUM_FPFH_METHOD_DEVICE(CUDA[0.02] Radius, utility::nullopt, 0.02, "CUDA:0") +#endif + +} // namespace registration +} // namespace pipelines +} // namespace t +} // namespace open3d diff --git a/cpp/open3d/t/pipelines/CMakeLists.txt b/cpp/open3d/t/pipelines/CMakeLists.txt index afc13830019..352a033acd9 100644 --- a/cpp/open3d/t/pipelines/CMakeLists.txt +++ b/cpp/open3d/t/pipelines/CMakeLists.txt @@ -9,6 +9,7 @@ target_sources(tpipelines PRIVATE target_sources(tpipelines PRIVATE registration/Registration.cpp registration/TransformationEstimation.cpp + registration/Feature.cpp ) target_sources(tpipelines PRIVATE diff --git a/cpp/open3d/t/pipelines/kernel/CMakeLists.txt b/cpp/open3d/t/pipelines/kernel/CMakeLists.txt index b0c5b45e46d..a766715e6e7 100644 --- a/cpp/open3d/t/pipelines/kernel/CMakeLists.txt +++ b/cpp/open3d/t/pipelines/kernel/CMakeLists.txt @@ -8,6 +8,8 @@ target_sources(tpipelines_kernel PRIVATE RGBDOdometry.cpp RGBDOdometryCPU.cpp TransformationConverter.cpp + Feature.cpp + FeatureCPU.cpp ) if (BUILD_CUDA_MODULE) @@ -16,6 +18,7 @@ if (BUILD_CUDA_MODULE) FillInLinearSystemCUDA.cu RGBDOdometryCUDA.cu TransformationConverter.cu + FeatureCUDA.cu ) endif() diff --git a/cpp/open3d/t/pipelines/kernel/Feature.cpp b/cpp/open3d/t/pipelines/kernel/Feature.cpp new file mode 100644 index 00000000000..7b9ada37bee --- /dev/null +++ b/cpp/open3d/t/pipelines/kernel/Feature.cpp @@ -0,0 +1,58 @@ +// ---------------------------------------------------------------------------- +// - Open3D: www.open3d.org - +// ---------------------------------------------------------------------------- +// The MIT License (MIT) +// +// Copyright (c) 2018-2021 www.open3d.org +// +// 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 "open3d/t/pipelines/kernel/Feature.h" + +#include "open3d/core/TensorCheck.h" + +namespace open3d { +namespace t { +namespace pipelines { +namespace kernel { + +void ComputeFPFHFeature(const core::Tensor &points, + const core::Tensor &normals, + const core::Tensor &indices, + const core::Tensor &distance2, + const core::Tensor &counts, + core::Tensor &fpfhs) { + core::AssertTensorShape(fpfhs, {points.GetLength(), 33}); + const core::Tensor points_d = points.Contiguous(); + const core::Tensor normals_d = normals.Contiguous(); + const core::Tensor counts_d = counts.To(core::Int32); + if (points_d.IsCPU()) { + ComputeFPFHFeatureCPU(points_d, normals_d, indices, distance2, counts_d, + fpfhs); + } else { + CUDA_CALL(ComputeFPFHFeatureCUDA, points_d, normals_d, indices, + distance2, counts_d, fpfhs); + } +} + +} // namespace kernel +} // namespace pipelines +} // namespace t +} // namespace open3d diff --git a/cpp/open3d/t/pipelines/kernel/Feature.h b/cpp/open3d/t/pipelines/kernel/Feature.h new file mode 100644 index 00000000000..7efaff5359c --- /dev/null +++ b/cpp/open3d/t/pipelines/kernel/Feature.h @@ -0,0 +1,61 @@ +// ---------------------------------------------------------------------------- +// - Open3D: www.open3d.org - +// ---------------------------------------------------------------------------- +// The MIT License (MIT) +// +// Copyright (c) 2018-2021 www.open3d.org +// +// 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 "open3d/core/CUDAUtils.h" +#include "open3d/core/Tensor.h" + +namespace open3d { +namespace t { +namespace pipelines { +namespace kernel { + +void ComputeFPFHFeature(const core::Tensor &points, + const core::Tensor &normals, + const core::Tensor &indices, + const core::Tensor &distance2, + const core::Tensor &counts, + core::Tensor &fpfhs); + +void ComputeFPFHFeatureCPU(const core::Tensor &points, + const core::Tensor &normals, + const core::Tensor &indices, + const core::Tensor &distance2, + const core::Tensor &counts, + core::Tensor &fpfhs); + +#ifdef BUILD_CUDA_MODULE +void ComputeFPFHFeatureCUDA(const core::Tensor &points, + const core::Tensor &normals, + const core::Tensor &indices, + const core::Tensor &distance2, + const core::Tensor &counts, + core::Tensor &fpfhs); +#endif + +} // namespace kernel +} // namespace pipelines +} // namespace t +} // namespace open3d diff --git a/cpp/open3d/t/pipelines/kernel/FeatureCPU.cpp b/cpp/open3d/t/pipelines/kernel/FeatureCPU.cpp new file mode 100644 index 00000000000..0557d6b1f23 --- /dev/null +++ b/cpp/open3d/t/pipelines/kernel/FeatureCPU.cpp @@ -0,0 +1,27 @@ +// ---------------------------------------------------------------------------- +// - Open3D: www.open3d.org - +// ---------------------------------------------------------------------------- +// The MIT License (MIT) +// +// Copyright (c) 2018-2021 www.open3d.org +// +// 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 "open3d/t/pipelines/kernel/FeatureImpl.h" diff --git a/cpp/open3d/t/pipelines/kernel/FeatureCUDA.cu b/cpp/open3d/t/pipelines/kernel/FeatureCUDA.cu new file mode 100644 index 00000000000..0557d6b1f23 --- /dev/null +++ b/cpp/open3d/t/pipelines/kernel/FeatureCUDA.cu @@ -0,0 +1,27 @@ +// ---------------------------------------------------------------------------- +// - Open3D: www.open3d.org - +// ---------------------------------------------------------------------------- +// The MIT License (MIT) +// +// Copyright (c) 2018-2021 www.open3d.org +// +// 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 "open3d/t/pipelines/kernel/FeatureImpl.h" diff --git a/cpp/open3d/t/pipelines/kernel/FeatureImpl.h b/cpp/open3d/t/pipelines/kernel/FeatureImpl.h new file mode 100644 index 00000000000..876d57c2a7d --- /dev/null +++ b/cpp/open3d/t/pipelines/kernel/FeatureImpl.h @@ -0,0 +1,242 @@ +// ---------------------------------------------------------------------------- +// - Open3D: www.open3d.org - +// ---------------------------------------------------------------------------- +// The MIT License (MIT) +// +// Copyright (c) 2018-2021 www.open3d.org +// +// 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 "open3d/core/CUDAUtils.h" +#include "open3d/core/Dispatch.h" +#include "open3d/core/ParallelFor.h" +#include "open3d/core/linalg/kernel/Matrix.h" +#include "open3d/t/pipelines/kernel/Feature.h" + +namespace open3d { +namespace t { +namespace pipelines { +namespace kernel { + +#ifndef __CUDACC__ +using std::max; +using std::min; +#endif + +template +OPEN3D_HOST_DEVICE void ComputePairFeature(const scalar_t *p1, + const scalar_t *n1, + const scalar_t *p2, + const scalar_t *n2, + scalar_t *feature) { + scalar_t dp2p1[3], n1_copy[3], n2_copy[3]; + dp2p1[0] = p2[0] - p1[0]; + dp2p1[1] = p2[1] - p1[1]; + dp2p1[2] = p2[2] - p1[2]; + feature[3] = sqrt(dp2p1[0] * dp2p1[0] + dp2p1[1] * dp2p1[1] + + dp2p1[2] * dp2p1[2]); + if (feature[3] == 0) { + feature[0] = 0; + feature[1] = 0; + feature[2] = 0; + feature[3] = 0; + return; + } + + scalar_t angle1 = core::linalg::kernel::dot_3x1(n1, dp2p1) / feature[3]; + scalar_t angle2 = core::linalg::kernel::dot_3x1(n2, dp2p1) / feature[3]; + if (acos(fabs(angle1)) > acos(fabs(angle2))) { + n1_copy[0] = n2[0]; + n1_copy[1] = n2[1]; + n1_copy[2] = n2[2]; + n2_copy[0] = n1[0]; + n2_copy[1] = n1[1]; + n2_copy[2] = n1[2]; + dp2p1[0] *= -1; + dp2p1[1] *= -1; + dp2p1[2] *= -1; + feature[2] = -angle2; + } else { + n1_copy[0] = n1[0]; + n1_copy[1] = n1[1]; + n1_copy[2] = n1[2]; + n2_copy[0] = n2[0]; + n2_copy[1] = n2[1]; + n2_copy[2] = n2[2]; + feature[2] = angle1; + } + + scalar_t v[3]; + core::linalg::kernel::cross_3x1(dp2p1, n1_copy, v); + const scalar_t v_norm = sqrt(v[0] * v[0] + v[1] * v[1] + v[2] * v[2]); + if (v_norm == 0.0) { + feature[0] = 0.0; + feature[1] = 0.0; + feature[2] = 0.0; + feature[3] = 0.0; + return; + } + v[0] /= v_norm; + v[1] /= v_norm; + v[2] /= v_norm; + scalar_t w[3]; + core::linalg::kernel::cross_3x1(n1_copy, v, w); + feature[1] = core::linalg::kernel::dot_3x1(v, n2_copy); + feature[0] = atan2(core::linalg::kernel::dot_3x1(w, n2_copy), + core::linalg::kernel::dot_3x1(n1_copy, n2_copy)); +} + +template +OPEN3D_HOST_DEVICE void UpdateSPFHFeature(const scalar_t *feature, + int64_t idx, + scalar_t hist_incr, + scalar_t *spfh) { + int h_index1 = + static_cast(floor(11 * (feature[0] + M_PI) / (2.0 * M_PI))); + h_index1 = h_index1 >= 11 ? 10 : max(0, h_index1); + + int h_index2 = static_cast(floor(11 * (feature[1] + 1.0) * 0.5)); + h_index2 = h_index2 >= 11 ? 10 : max(0, h_index2); + + int h_index3 = static_cast(floor(11 * (feature[2] + 1.0) * 0.5)); + h_index3 = h_index3 >= 11 ? 10 : max(0, h_index3); + + spfh[idx * 33 + h_index1] += hist_incr; + spfh[idx * 33 + h_index2 + 11] += hist_incr; + spfh[idx * 33 + h_index3 + 22] += hist_incr; +} + +#if defined(__CUDACC__) +void ComputeFPFHFeatureCUDA +#else +void ComputeFPFHFeatureCPU +#endif + (const core::Tensor &points, + const core::Tensor &normals, + const core::Tensor &indices, + const core::Tensor &distance2, + const core::Tensor &counts, + core::Tensor &fpfhs) { + const core::Dtype dtype = points.GetDtype(); + const int64_t n = points.GetLength(); + + core::Tensor spfhs = fpfhs.Clone(); + + // Check the nns type (knn = hybrid = false, radius = true). + // The nns radius search mode will resulting a prefix sum 1D tensor. + bool is_radius_search; + int nn_size = 0; + if (indices.GetShape().size() == 1) { + is_radius_search = true; + } else { + is_radius_search = false; + nn_size = indices.GetShape()[1]; + } + + DISPATCH_FLOAT_DTYPE_TO_TEMPLATE(dtype, [&]() { + const scalar_t *points_ptr = points.GetDataPtr(); + const scalar_t *normals_ptr = normals.GetDataPtr(); + const int32_t *indices_ptr = indices.GetDataPtr(); + const scalar_t *distance2_ptr = distance2.GetDataPtr(); + const int32_t *counts_ptr = counts.GetDataPtr(); + scalar_t *spfhs_ptr = spfhs.GetDataPtr(); + scalar_t *fpfhs_ptr = fpfhs.GetDataPtr(); + + // Compute SPFH features for each point. + core::ParallelFor( + points.GetDevice(), n, [=] OPEN3D_DEVICE(int64_t workload_idx) { + int64_t idx = 3 * workload_idx; + const scalar_t *point = points_ptr + idx; + const scalar_t *normal = normals_ptr + idx; + + const int indice_size = + is_radius_search ? (counts_ptr[workload_idx + 1] - + counts_ptr[workload_idx]) + : counts_ptr[workload_idx]; + + if (indice_size > 1) { + const scalar_t hist_incr = + 100.0 / static_cast(indice_size - 1); + for (int i = 1; i < indice_size; i++) { + const int point_idx = + is_radius_search + ? indices_ptr + [i + + counts_ptr[workload_idx]] + : indices_ptr[workload_idx * + nn_size + + i]; + + const scalar_t *point_ref = + points_ptr + 3 * point_idx; + const scalar_t *normal_ref = + normals_ptr + 3 * point_idx; + scalar_t fea[4] = {0}; + ComputePairFeature( + point, normal, point_ref, normal_ref, fea); + UpdateSPFHFeature(fea, workload_idx, + hist_incr, spfhs_ptr); + } + } + }); + + // Compute FPFH features for each point. + core::ParallelFor( + points.GetDevice(), n, [=] OPEN3D_DEVICE(int64_t workload_idx) { + const int indice_size = + is_radius_search ? (counts_ptr[workload_idx + 1] - + counts_ptr[workload_idx]) + : counts_ptr[workload_idx]; + + if (indice_size > 1) { + scalar_t sum[3] = {0.0, 0.0, 0.0}; + for (int i = 1; i < indice_size; i++) { + const int idx = + is_radius_search + ? i + counts_ptr[workload_idx] + : workload_idx * nn_size + i; + const scalar_t dist = distance2_ptr[idx]; + if (dist == 0.0) continue; + + for (int j = 0; j < 33; j++) { + const scalar_t val = + spfhs_ptr[indices_ptr[idx] * 33 + j] / + dist; + sum[j / 11] += val; + fpfhs_ptr[workload_idx * 33 + j] += val; + } + } + for (int j = 0; j < 3; j++) { + sum[j] = sum[j] != 0.0 ? 100.0 / sum[j] : 0.0; + } + for (int j = 0; j < 33; j++) { + fpfhs_ptr[workload_idx * 33 + j] *= sum[j / 11]; + fpfhs_ptr[workload_idx * 33 + j] += + spfhs_ptr[workload_idx * 33 + j]; + } + } + }); + }); +} // namespace kernel + +} // namespace kernel +} // namespace pipelines +} // namespace t +} // namespace open3d diff --git a/cpp/open3d/t/pipelines/registration/Feature.cpp b/cpp/open3d/t/pipelines/registration/Feature.cpp new file mode 100644 index 00000000000..328bcddf2f2 --- /dev/null +++ b/cpp/open3d/t/pipelines/registration/Feature.cpp @@ -0,0 +1,115 @@ +// ---------------------------------------------------------------------------- +// - Open3D: www.open3d.org - +// ---------------------------------------------------------------------------- +// The MIT License (MIT) +// +// Copyright (c) 2018-2021 www.open3d.org +// +// 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 "open3d/t/pipelines/registration/Feature.h" + +#include "open3d/core/nns/NearestNeighborSearch.h" +#include "open3d/t/geometry/PointCloud.h" +#include "open3d/t/pipelines/kernel/Feature.h" + +namespace open3d { +namespace t { + +namespace pipelines { +namespace registration { + +core::Tensor ComputeFPFHFeature(const geometry::PointCloud &input, + const utility::optional max_nn, + const utility::optional radius) { + core::AssertTensorDtypes(input.GetPointPositions(), + {core::Float64, core::Float32}); + if (max_nn.has_value() && max_nn.value() <= 3) { + utility::LogError("max_nn must be greater than 3."); + } + if (radius.has_value() && radius.value() <= 0) { + utility::LogError("radius must be greater than 0."); + } + if (!input.HasPointNormals()) { + utility::LogError("The input point cloud has no normal."); + } + + const int64_t num_points = input.GetPointPositions().GetLength(); + const core::Dtype dtype = input.GetPointPositions().GetDtype(); + const core::Device device = input.GetPointPositions().GetDevice(); + + // Compute nearest neighbors and squared distances. + core::Tensor indices, distance2, counts; + core::nns::NearestNeighborSearch tree(input.GetPointPositions(), + core::Int32); + if (radius.has_value() && max_nn.has_value()) { + bool check = tree.HybridIndex(radius.value()); + if (!check) { + utility::LogError("Building HybridIndex failed."); + } + std::tie(indices, distance2, counts) = tree.HybridSearch( + input.GetPointPositions(), radius.value(), max_nn.value()); + utility::LogDebug( + "Use HybridSearch [max_nn: {} | radius {}] for computing FPFH " + "feature.", + max_nn.value(), radius.value()); + } else if (!radius.has_value() && max_nn.has_value()) { + bool check = tree.KnnIndex(); + if (!check) { + utility::LogError("Building KnnIndex failed."); + } + std::tie(indices, distance2) = + tree.KnnSearch(input.GetPointPositions(), max_nn.value()); + + // Make counts full with min(max_nn, num_points). + const int fill_value = + max_nn.value() > num_points ? num_points : max_nn.value(); + counts = core::Tensor::Full({num_points}, fill_value, core::Int32, + device); + utility::LogDebug( + "Use KNNSearch [max_nn: {}] for computing FPFH feature.", + max_nn.value()); + } else if (radius.has_value() && !max_nn.has_value()) { + bool check = tree.FixedRadiusIndex(radius.value()); + if (!check) { + utility::LogError("Building RadiusIndex failed."); + } + std::tie(indices, distance2, counts) = tree.FixedRadiusSearch( + input.GetPointPositions(), radius.value()); + utility::LogDebug( + "Use RadiusSearch [radius: {}] for computing FPFH feature.", + radius.value()); + } else { + utility::LogError("Both max_nn and radius are none."); + } + + const int64_t size = input.GetPointPositions().GetLength(); + + core::Tensor fpfh = core::Tensor::Zeros({size, 33}, dtype, device); + pipelines::kernel::ComputeFPFHFeature(input.GetPointPositions(), + input.GetPointNormals(), indices, + distance2, counts, fpfh); + return fpfh; +} + +} // namespace registration +} // namespace pipelines +} // namespace t +} // namespace open3d diff --git a/cpp/open3d/t/pipelines/registration/Feature.h b/cpp/open3d/t/pipelines/registration/Feature.h new file mode 100644 index 00000000000..b63723eb1e2 --- /dev/null +++ b/cpp/open3d/t/pipelines/registration/Feature.h @@ -0,0 +1,62 @@ +// ---------------------------------------------------------------------------- +// - Open3D: www.open3d.org - +// ---------------------------------------------------------------------------- +// The MIT License (MIT) +// +// Copyright (c) 2018-2021 www.open3d.org +// +// 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 "open3d/core/Tensor.h" +#include "open3d/utility/Optional.h" + +namespace open3d { +namespace t { + +namespace geometry { +class PointCloud; +} + +namespace pipelines { +namespace registration { + +/// Function to compute FPFH feature for a point cloud. +/// It uses KNN search (Not recommended to use on GPU) if only max_nn parameter +/// is provided, Radius search (Not recommended to use on GPU) if only radius +/// parameter is provided, and Hybrid search (Recommended) if both are provided. +/// +/// \param input The input point cloud with data type float32 or float64. +/// \param max_nn [optional] Neighbor search max neighbors parameter. [Default = +/// 100]. +/// \param radius [optional] Neighbor search radius parameter. [Recommended ~5x +/// voxel size]. +/// \return A Tensor of FPFH feature of the input point cloud with +/// shape {N, 33}, data type and device same as input. +core::Tensor ComputeFPFHFeature( + const geometry::PointCloud &input, + const utility::optional max_nn = 100, + const utility::optional radius = utility::nullopt); + +} // namespace registration +} // namespace pipelines +} // namespace t +} // namespace open3d diff --git a/cpp/pybind/t/pipelines/CMakeLists.txt b/cpp/pybind/t/pipelines/CMakeLists.txt index 83a23be40f1..79c4cf1005f 100644 --- a/cpp/pybind/t/pipelines/CMakeLists.txt +++ b/cpp/pybind/t/pipelines/CMakeLists.txt @@ -7,6 +7,7 @@ target_sources(pybind PRIVATE ) target_sources(pybind PRIVATE + registration/feature.cpp registration/registration.cpp registration/robust_kernel.cpp ) diff --git a/cpp/pybind/t/pipelines/registration/feature.cpp b/cpp/pybind/t/pipelines/registration/feature.cpp new file mode 100644 index 00000000000..f4193fe1c5b --- /dev/null +++ b/cpp/pybind/t/pipelines/registration/feature.cpp @@ -0,0 +1,62 @@ +// ---------------------------------------------------------------------------- +// - Open3D: www.open3d.org - +// ---------------------------------------------------------------------------- +// The MIT License (MIT) +// +// Copyright (c) 2018-2021 www.open3d.org +// +// 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 "open3d/t/pipelines/registration/Feature.h" + +#include "open3d/t/geometry/PointCloud.h" +#include "open3d/utility/Logging.h" +#include "pybind/docstring.h" +#include "pybind/t/pipelines/registration/registration.h" + +namespace open3d { +namespace t { +namespace pipelines { +namespace registration { + +void pybind_feature(py::module &m) { + m.def("compute_fpfh_feature", &ComputeFPFHFeature, + py::call_guard(), + R"(Function to compute FPFH feature for a point cloud. +It uses KNN search (Not recommended to use on GPU) if only max_nn parameter +is provided, Radius search (Not recommended to use on GPU) if only radius +parameter is provided, and Hybrid search (Recommended) if both are provided.)", + "input"_a, "max_nn"_a = 100, "radius"_a = py::none()); + docstring::FunctionDocInject( + m, "compute_fpfh_feature", + {{"input", + "The input point cloud with data type float32 or float64."}, + {"max_nn", + "[optional] Neighbor search max neighbors parameter.[Default = " + "100]"}, + {"radius", + "[optional] Neighbor search radius parameter. [Recommended ~5x " + "voxel size]"}}); +} + +} // namespace registration +} // namespace pipelines +} // namespace t +} // namespace open3d diff --git a/cpp/pybind/t/pipelines/registration/registration.cpp b/cpp/pybind/t/pipelines/registration/registration.cpp index c229e0952eb..666c11ebe18 100644 --- a/cpp/pybind/t/pipelines/registration/registration.cpp +++ b/cpp/pybind/t/pipelines/registration/registration.cpp @@ -344,6 +344,7 @@ void pybind_registration(py::module &m) { "registration", "Tensor-based registration pipeline."); pybind_registration_classes(m_submodule); pybind_registration_methods(m_submodule); + pybind_feature(m_submodule); pybind_robust_kernels(m_submodule); } diff --git a/cpp/pybind/t/pipelines/registration/registration.h b/cpp/pybind/t/pipelines/registration/registration.h index f514a441e87..136244e85a9 100644 --- a/cpp/pybind/t/pipelines/registration/registration.h +++ b/cpp/pybind/t/pipelines/registration/registration.h @@ -33,6 +33,7 @@ namespace t { namespace pipelines { namespace registration { +void pybind_feature(py::module &m); void pybind_registration(py::module &m); void pybind_robust_kernels(py::module &m); diff --git a/cpp/tests/t/pipelines/CMakeLists.txt b/cpp/tests/t/pipelines/CMakeLists.txt index a875780c705..8c0bc83b0cd 100644 --- a/cpp/tests/t/pipelines/CMakeLists.txt +++ b/cpp/tests/t/pipelines/CMakeLists.txt @@ -7,6 +7,7 @@ target_sources(tests PRIVATE ) target_sources(tests PRIVATE + registration/Feature.cpp registration/Registration.cpp registration/TransformationEstimation.cpp ) diff --git a/cpp/tests/t/pipelines/registration/Feature.cpp b/cpp/tests/t/pipelines/registration/Feature.cpp new file mode 100644 index 00000000000..f37e61ea7be --- /dev/null +++ b/cpp/tests/t/pipelines/registration/Feature.cpp @@ -0,0 +1,72 @@ +// ---------------------------------------------------------------------------- +// - Open3D: www.open3d.org - +// ---------------------------------------------------------------------------- +// The MIT License (MIT) +// +// Copyright (c) 2018-2021 www.open3d.org +// +// 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 "open3d/t/pipelines/registration/Feature.h" + +#include "core/CoreTest.h" +#include "open3d/core/EigenConverter.h" +#include "open3d/core/Tensor.h" +#include "open3d/data/Dataset.h" +#include "open3d/geometry/PointCloud.h" +#include "open3d/pipelines/registration/Feature.h" +#include "open3d/t/geometry/PointCloud.h" +#include "open3d/t/io/PointCloudIO.h" +#include "tests/Tests.h" + +namespace open3d { +namespace tests { + +class FeaturePermuteDevices : public PermuteDevices {}; +INSTANTIATE_TEST_SUITE_P(Feature, + FeaturePermuteDevices, + testing::ValuesIn(PermuteDevices::TestCases())); + +TEST_P(FeaturePermuteDevices, ComputeFPFHFeature) { + core::Device device = GetParam(); + + open3d::geometry::PointCloud pcd_legacy; + data::BunnyMesh byunny; + open3d::io::ReadPointCloud(byunny.GetPath(), pcd_legacy); + + pcd_legacy.EstimateNormals(); + // Convert to float64 to avoid precision loss. + const auto pcd = t::geometry::PointCloud::FromLegacy(pcd_legacy, + core::Float64, device); + + const auto fpfh = pipelines::registration::ComputeFPFHFeature( + pcd_legacy, geometry::KDTreeSearchParamHybrid(0.01, 100)); + const auto fpfh_t = + t::pipelines::registration::ComputeFPFHFeature(pcd, 100, 0.01); + + EXPECT_TRUE(fpfh_t.AllClose( + core::eigen_converter::EigenMatrixToTensor(fpfh->data_) + .T() + .To(fpfh_t.GetDevice(), fpfh_t.GetDtype()), + 1e-4, 1e-4)); +} + +} // namespace tests +} // namespace open3d