From 98c220861bcc23e5b1b8cdd6ae74dda31462ff96 Mon Sep 17 00:00:00 2001 From: yuecideng Date: Fri, 29 Jul 2022 09:21:57 +0800 Subject: [PATCH 01/15] init tensor fpfh --- cpp/open3d/t/pipelines/CMakeLists.txt | 1 + cpp/open3d/t/pipelines/kernel/CMakeLists.txt | 3 + cpp/open3d/t/pipelines/kernel/Feature.cpp | 55 ++++++++ cpp/open3d/t/pipelines/kernel/Feature.h | 58 +++++++++ cpp/open3d/t/pipelines/kernel/FeatureCPU.cpp | 27 ++++ cpp/open3d/t/pipelines/kernel/FeatureCUDA.cu | 27 ++++ cpp/open3d/t/pipelines/kernel/FeatureImpl.h | 123 ++++++++++++++++++ .../t/pipelines/registration/Feature.cpp | 83 ++++++++++++ cpp/open3d/t/pipelines/registration/Feature.h | 57 ++++++++ 9 files changed, 434 insertions(+) create mode 100644 cpp/open3d/t/pipelines/kernel/Feature.cpp create mode 100644 cpp/open3d/t/pipelines/kernel/Feature.h create mode 100644 cpp/open3d/t/pipelines/kernel/FeatureCPU.cpp create mode 100644 cpp/open3d/t/pipelines/kernel/FeatureCUDA.cu create mode 100644 cpp/open3d/t/pipelines/kernel/FeatureImpl.h create mode 100644 cpp/open3d/t/pipelines/registration/Feature.cpp create mode 100644 cpp/open3d/t/pipelines/registration/Feature.h 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..af0eeaad458 --- /dev/null +++ b/cpp/open3d/t/pipelines/kernel/Feature.cpp @@ -0,0 +1,55 @@ +// ---------------------------------------------------------------------------- +// - 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 ComputeSPFHFeature(const core::Tensor &points, + const core::Tensor &normals, + const core::Tensor &indices, + const core::Tensor &distance2, + core::Tensor &spfhs) { + core::AssertTensorShape(spfhs, {points.GetLength(), 33}); + const core::Tensor points_d = points.Contiguous(); + const core::Tensor normals_d = normals.Contiguous(); + if (points_d.IsCPU()) { + ComputeSPFHFeatureCPU(points_d, normals_d, indices, distance2, spfhs); + } else { + CUDA_CALL(ComputeSPFHFeatureCUDA, normals_d, points_d, indices, + distance2, spfhs); + } +} + +} // 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..3f1f334eff1 --- /dev/null +++ b/cpp/open3d/t/pipelines/kernel/Feature.h @@ -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/core/CUDAUtils.h" +#include "open3d/core/Tensor.h" + +namespace open3d { +namespace t { +namespace pipelines { +namespace kernel { + +void ComputeSPFHFeature(const core::Tensor &points, + const core::Tensor &normals, + const core::Tensor &indices, + const core::Tensor &distance2, + core::Tensor &spfhs); + +void ComputeSPFHFeatureCPU(const core::Tensor &points, + const core::Tensor &normals, + const core::Tensor &indices, + const core::Tensor &distance2, + core::Tensor &spfhs); + +#ifdef BUILD_CUDA_MODULE +void ComputeSPFHFeatureCUDA(const core::Tensor &points, + const core::Tensor &normals, + const core::Tensor &indices, + const core::Tensor &distance2, + core::Tensor &spfhs); +#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..e529c362952 --- /dev/null +++ b/cpp/open3d/t/pipelines/kernel/FeatureImpl.h @@ -0,0 +1,123 @@ +// ---------------------------------------------------------------------------- +// - 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/ParallelFor.h" +#include "open3d/core/linalg/kernel/Matrix.h" +#include "open3d/t/pipelines/kernel/Feature.h" + +namespace open3d { +namespace t { +namespace pipelines { +namespace kernel { + +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]; + + const scalar_t angle1 = + core::linalg::kernel::dot_3x1(n1, dp2p1) / feature[3]; + const scalar_t angle2 = + core::linalg::kernel::dot_3x1(n2, dp2p1) / feature[4]; + 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(core::linalg::kernel::dot_3x1(v, v)); + 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]; + 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)); +} + +#if defined(__CUDACC__) +void ComputeSPFHFeatureCUDA +#else +void ComputeSPFHFeatureCPU +#endif + (const core::Tensor &points, + const core::Tensor &normals, + const core::Tensor &indices, + const core::Tensor &distance2, + core::Tensor &spfhs) { + const core::Dtype dtype = points.GetDtype(); + const int64_t n = points.GetLength(); + const int64_t num_nn = 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(); + scalar_t *spfhs_ptr = spfhs.GetDataPtr(); + + core::ParallelFor(points.GetDevice(), n, + [=] OPEN3D_DEVICE(int64_t workload_idx) { + int64_t idx = 3 * workload_idx; + + }); + }); +} + +} // 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..8bef0f540ea --- /dev/null +++ b/cpp/open3d/t/pipelines/registration/Feature.cpp @@ -0,0 +1,83 @@ +// ---------------------------------------------------------------------------- +// - 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" + +namespace open3d { +namespace t { + +namespace pipelines { +namespace registration { + +core::Tensor ComputeFPFHFeature( + const geometry::PointCloud &input, + const int max_nn = 100, + const utility::optional radius = utility::nullopt) { + core::AssertTensorDtypes(input.GetPointPositions(), + {core::Float64, core::Float32}); + if (max_nn <= 1) { + utility::LogError("max_nn must be greater than 1."); + } + if (radius.value() <= 0) { + utility::LogError("radius must be greater than 0."); + } + + // Compute nearest neighbors and squared distances. + core::Tensor indices, distance2; + core::nns::NearestNeighborSearch tree(input.GetPointPositions(), + core::Int32); + if (radius.has_value()) { + bool check = tree.HybridIndex(radius.value()); + if (!check) { + utility::LogError("Building HybridIndex failed."); + } + core::Tensor counts; + std::tie(indices, distance2, counts) = tree.HybridSearch( + input.GetPointPositions(), radius.value(), max_nn); + } else { + bool check = tree.KnnIndex(); + if (!check) { + utility::LogError("Building KnnIndex failed."); + } + std::tie(indices, distance2) = tree.KnnSearch( + input.GetPointPositions(), max_nn); + } + + const core::Device device = input.GetDevice(); + const core::Dtype dtype = input.GetPointPositions().GetDtype(); + const int64_t size = input.GetPointPositions().GetLength(); + + core::Tensor fpfh = core::Tensor::Zeros({size, 33}, dtype, device); + +} + +} // 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..28a9982790e --- /dev/null +++ b/cpp/open3d/t/pipelines/registration/Feature.h @@ -0,0 +1,57 @@ +// ---------------------------------------------------------------------------- +// - 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" + +namespace open3d { +namespace t { + +namespace geometry { +class PointCloud; +} + +namespace pipelines { +namespace registration { + +/// Function to compute FPFH feature for a point cloud. +/// +/// \param input The Input point cloud. +/// \param max_nn Neighbor search max neighbors parameter [Default = 100]. +/// \param radius [optional] Neighbor search radius parameter to use +/// HybridSearch. [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 int max_nn = 100, + const utility::optional radius = utility::nullopt); + +} // namespace registration +} // namespace pipelines +} // namespace t +} // namespace open3d From 583f6d46f98870cc2ffe107d345132df272f6695 Mon Sep 17 00:00:00 2001 From: yuecideng Date: Wed, 10 Aug 2022 23:30:39 +0800 Subject: [PATCH 02/15] update --- cpp/open3d/t/pipelines/kernel/Feature.h | 6 +-- cpp/open3d/t/pipelines/kernel/FeatureImpl.h | 47 +++++++++++++++++-- .../t/pipelines/registration/Feature.cpp | 39 ++++++++++----- cpp/open3d/t/pipelines/registration/Feature.h | 5 +- 4 files changed, 78 insertions(+), 19 deletions(-) diff --git a/cpp/open3d/t/pipelines/kernel/Feature.h b/cpp/open3d/t/pipelines/kernel/Feature.h index 3f1f334eff1..4bcc26fe96c 100644 --- a/cpp/open3d/t/pipelines/kernel/Feature.h +++ b/cpp/open3d/t/pipelines/kernel/Feature.h @@ -35,20 +35,20 @@ namespace kernel { void ComputeSPFHFeature(const core::Tensor &points, const core::Tensor &normals, const core::Tensor &indices, - const core::Tensor &distance2, + const core::Tensor &counts, core::Tensor &spfhs); void ComputeSPFHFeatureCPU(const core::Tensor &points, const core::Tensor &normals, const core::Tensor &indices, - const core::Tensor &distance2, + const core::Tensor &counts, core::Tensor &spfhs); #ifdef BUILD_CUDA_MODULE void ComputeSPFHFeatureCUDA(const core::Tensor &points, const core::Tensor &normals, const core::Tensor &indices, - const core::Tensor &distance2, + const core::Tensor &counts, core::Tensor &spfhs); #endif diff --git a/cpp/open3d/t/pipelines/kernel/FeatureImpl.h b/cpp/open3d/t/pipelines/kernel/FeatureImpl.h index e529c362952..2839d0c2d21 100644 --- a/cpp/open3d/t/pipelines/kernel/FeatureImpl.h +++ b/cpp/open3d/t/pipelines/kernel/FeatureImpl.h @@ -33,6 +33,11 @@ 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, @@ -43,6 +48,15 @@ OPEN3D_HOST_DEVICE void ComputePairFeature(const scalar_t *p1, 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; + } const scalar_t angle1 = core::linalg::kernel::dot_3x1(n1, dp2p1) / feature[3]; @@ -88,6 +102,34 @@ OPEN3D_HOST_DEVICE void ComputePairFeature(const scalar_t *p1, 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_index = + static_cast(floor(11 * (feature[0] + M_PI) / (2.0 * M_PI))); + h_index = max(0, h_index); + if (h_index >= 11) { + h_index = 10; + } + spfh[idx * 33 + h_index] += hist_incr; + + h_index = static_cast(floor(11 * (feature[1] + 1.0) * 0.5)); + h_index = max(0, h_index); + if (h_index >= 11) { + h_index = 10; + } + spfh[idx * 33 + h_index + 11] += hist_incr; + + h_index = static_cast(floor(11 * (feature[2] + 1.0) * 0.5)); + h_index = max(0, h_index); + if (h_index >= 11) { + h_index = 10; + } + spfh[idx * 33 + h_index + 22] += hist_incr; +} + #if defined(__CUDACC__) void ComputeSPFHFeatureCUDA #else @@ -96,7 +138,7 @@ void ComputeSPFHFeatureCPU (const core::Tensor &points, const core::Tensor &normals, const core::Tensor &indices, - const core::Tensor &distance2, + const core::Tensor &counts, core::Tensor &spfhs) { const core::Dtype dtype = points.GetDtype(); const int64_t n = points.GetLength(); @@ -106,13 +148,12 @@ void ComputeSPFHFeatureCPU 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 int64_t *counts_ptr = counts.GetDataPtr(); scalar_t *spfhs_ptr = spfhs.GetDataPtr(); core::ParallelFor(points.GetDevice(), n, [=] OPEN3D_DEVICE(int64_t workload_idx) { int64_t idx = 3 * workload_idx; - }); }); } diff --git a/cpp/open3d/t/pipelines/registration/Feature.cpp b/cpp/open3d/t/pipelines/registration/Feature.cpp index 8bef0f540ea..6702bdae21f 100644 --- a/cpp/open3d/t/pipelines/registration/Feature.cpp +++ b/cpp/open3d/t/pipelines/registration/Feature.cpp @@ -37,36 +37,54 @@ namespace registration { core::Tensor ComputeFPFHFeature( const geometry::PointCloud &input, - const int max_nn = 100, + const utility::optional max_nn = 100, const utility::optional radius = utility::nullopt) { core::AssertTensorDtypes(input.GetPointPositions(), {core::Float64, core::Float32}); - if (max_nn <= 1) { - utility::LogError("max_nn must be greater than 1."); + if (max_nn.value() <= 3) { + utility::LogError("max_nn must be greater than 3."); } if (radius.value() <= 0) { utility::LogError("radius must be greater than 0."); } + if (!input.HasPointNormals()) { + utility::LogError("Failed because 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; + core::Tensor indices, distance2, counts; core::nns::NearestNeighborSearch tree(input.GetPointPositions(), core::Int32); - if (radius.has_value()) { + if (radius.has_value() && max_nn.has_value()) { bool check = tree.HybridIndex(radius.value()); if (!check) { utility::LogError("Building HybridIndex failed."); } - core::Tensor counts; std::tie(indices, distance2, counts) = tree.HybridSearch( - input.GetPointPositions(), radius.value(), max_nn); - } else { + input.GetPointPositions(), radius.value(), max_nn.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); + std::tie(indices, distance2) = + tree.KnnSearch(input.GetPointPositions(), max_nn.value()); + + // Make counts full with max_nn. + counts = + core::Tensor::Full({num_points}, max_nn.value(), dtype, device); + } 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()); } const core::Device device = input.GetDevice(); @@ -74,7 +92,6 @@ core::Tensor ComputeFPFHFeature( const int64_t size = input.GetPointPositions().GetLength(); core::Tensor fpfh = core::Tensor::Zeros({size, 33}, dtype, device); - } } // namespace registration diff --git a/cpp/open3d/t/pipelines/registration/Feature.h b/cpp/open3d/t/pipelines/registration/Feature.h index 28a9982790e..a22ef0709e3 100644 --- a/cpp/open3d/t/pipelines/registration/Feature.h +++ b/cpp/open3d/t/pipelines/registration/Feature.h @@ -41,14 +41,15 @@ namespace registration { /// Function to compute FPFH feature for a point cloud. /// /// \param input The Input point cloud. -/// \param max_nn Neighbor search max neighbors parameter [Default = 100]. +/// \param max_nn [optional] Neighbor search max neighbors parameter [Default = +/// 100]. /// \param radius [optional] Neighbor search radius parameter to use /// HybridSearch. [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 int max_nn = 100, + const utility::optional max_nn = 100, const utility::optional radius = utility::nullopt); } // namespace registration From faffaeb25bd867d54b0612d9f5c7ef29c147d84d Mon Sep 17 00:00:00 2001 From: yuecideng Date: Fri, 12 Aug 2022 01:01:44 +0000 Subject: [PATCH 03/15] wip --- .clang-format | 2 +- cpp/open3d/t/pipelines/kernel/Feature.cpp | 14 +- cpp/open3d/t/pipelines/kernel/Feature.h | 15 +- cpp/open3d/t/pipelines/kernel/FeatureImpl.h | 173 ++++++++++++++---- .../t/pipelines/registration/Feature.cpp | 14 +- cpp/open3d/t/pipelines/registration/Feature.h | 14 +- cpp/pybind/t/pipelines/CMakeLists.txt | 1 + .../t/pipelines/registration/feature.cpp | 61 ++++++ .../t/pipelines/registration/registration.cpp | 1 + .../t/pipelines/registration/registration.h | 1 + 10 files changed, 239 insertions(+), 57 deletions(-) create mode 100644 cpp/pybind/t/pipelines/registration/feature.cpp diff --git a/.clang-format b/.clang-format index 63f78edb55a..cf4f504e0e9 100644 --- a/.clang-format +++ b/.clang-format @@ -2,7 +2,7 @@ BasedOnStyle: Google IndentWidth: 4 ColumnLimit: 80 UseTab: Never -Standard: c++14 +# Standard: c++14 ContinuationIndentWidth: 8 AccessModifierOffset: -4 BinPackParameters: false diff --git a/cpp/open3d/t/pipelines/kernel/Feature.cpp b/cpp/open3d/t/pipelines/kernel/Feature.cpp index af0eeaad458..f94fe2ec906 100644 --- a/cpp/open3d/t/pipelines/kernel/Feature.cpp +++ b/cpp/open3d/t/pipelines/kernel/Feature.cpp @@ -33,19 +33,21 @@ namespace t { namespace pipelines { namespace kernel { -void ComputeSPFHFeature(const core::Tensor &points, +void ComputeFPFHFeature(const core::Tensor &points, const core::Tensor &normals, const core::Tensor &indices, const core::Tensor &distance2, - core::Tensor &spfhs) { - core::AssertTensorShape(spfhs, {points.GetLength(), 33}); + 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(); if (points_d.IsCPU()) { - ComputeSPFHFeatureCPU(points_d, normals_d, indices, distance2, spfhs); + ComputeFPFHFeatureCPU(points_d, normals_d, indices, distance2, counts, + fpfhs); } else { - CUDA_CALL(ComputeSPFHFeatureCUDA, normals_d, points_d, indices, - distance2, spfhs); + CUDA_CALL(ComputeFPFHFeatureCUDA, points_d, normals_d, indices, + distance2, counts, fpfhs); } } diff --git a/cpp/open3d/t/pipelines/kernel/Feature.h b/cpp/open3d/t/pipelines/kernel/Feature.h index 4bcc26fe96c..7efaff5359c 100644 --- a/cpp/open3d/t/pipelines/kernel/Feature.h +++ b/cpp/open3d/t/pipelines/kernel/Feature.h @@ -32,24 +32,27 @@ namespace t { namespace pipelines { namespace kernel { -void ComputeSPFHFeature(const core::Tensor &points, +void ComputeFPFHFeature(const core::Tensor &points, const core::Tensor &normals, const core::Tensor &indices, + const core::Tensor &distance2, const core::Tensor &counts, - core::Tensor &spfhs); + core::Tensor &fpfhs); -void ComputeSPFHFeatureCPU(const core::Tensor &points, +void ComputeFPFHFeatureCPU(const core::Tensor &points, const core::Tensor &normals, const core::Tensor &indices, + const core::Tensor &distance2, const core::Tensor &counts, - core::Tensor &spfhs); + core::Tensor &fpfhs); #ifdef BUILD_CUDA_MODULE -void ComputeSPFHFeatureCUDA(const core::Tensor &points, +void ComputeFPFHFeatureCUDA(const core::Tensor &points, const core::Tensor &normals, const core::Tensor &indices, + const core::Tensor &distance2, const core::Tensor &counts, - core::Tensor &spfhs); + core::Tensor &fpfhs); #endif } // namespace kernel diff --git a/cpp/open3d/t/pipelines/kernel/FeatureImpl.h b/cpp/open3d/t/pipelines/kernel/FeatureImpl.h index 2839d0c2d21..523182add37 100644 --- a/cpp/open3d/t/pipelines/kernel/FeatureImpl.h +++ b/cpp/open3d/t/pipelines/kernel/FeatureImpl.h @@ -24,6 +24,8 @@ // 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" @@ -58,10 +60,8 @@ OPEN3D_HOST_DEVICE void ComputePairFeature(const scalar_t *p1, return; } - const scalar_t angle1 = - core::linalg::kernel::dot_3x1(n1, dp2p1) / feature[3]; - const scalar_t angle2 = - core::linalg::kernel::dot_3x1(n2, dp2p1) / feature[4]; + 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]; @@ -85,7 +85,7 @@ OPEN3D_HOST_DEVICE void ComputePairFeature(const scalar_t *p1, scalar_t v[3]; core::linalg::kernel::cross_3x1(dp2p1, n1_copy, v); - const scalar_t v_norm = sqrt(core::linalg::kernel::dot_3x1(v, 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; @@ -97,6 +97,7 @@ OPEN3D_HOST_DEVICE void ComputePairFeature(const scalar_t *p1, 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)); @@ -107,56 +108,164 @@ OPEN3D_HOST_DEVICE void UpdateSPFHFeature(const scalar_t *feature, int64_t idx, scalar_t hist_incr, scalar_t *spfh) { - int h_index = + int h_index1 = static_cast(floor(11 * (feature[0] + M_PI) / (2.0 * M_PI))); - h_index = max(0, h_index); - if (h_index >= 11) { - h_index = 10; - } - spfh[idx * 33 + h_index] += hist_incr; + h_index1 = h_index1 >= 11 ? 10 : max(0, h_index1); - h_index = static_cast(floor(11 * (feature[1] + 1.0) * 0.5)); - h_index = max(0, h_index); - if (h_index >= 11) { - h_index = 10; - } - spfh[idx * 33 + h_index + 11] += hist_incr; + 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); - h_index = static_cast(floor(11 * (feature[2] + 1.0) * 0.5)); - h_index = max(0, h_index); - if (h_index >= 11) { - h_index = 10; +#if defined(BUILD_CUDA_MODULE) && defined(__CUDACC__) + atomicAdd(&spfh[idx * 33 + h_index1], hist_incr); + atomicAdd(&spfh[idx * 33 + 11 + h_index2], hist_incr); + atomicAdd(&spfh[idx * 33 + 22 + h_index3], hist_incr); +#else +#pragma omp critical(UpdateSPFHFeature) + { + spfh[idx * 33 + h_index1] += hist_incr; + spfh[idx * 33 + h_index2 + 11] += hist_incr; + spfh[idx * 33 + h_index3 + 22] += hist_incr; } - spfh[idx * 33 + h_index + 22] += hist_incr; +#endif } #if defined(__CUDACC__) -void ComputeSPFHFeatureCUDA +void ComputeFPFHFeatureCUDA #else -void ComputeSPFHFeatureCPU +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 &spfhs) { + core::Tensor &fpfhs) { const core::Dtype dtype = points.GetDtype(); const int64_t n = points.GetLength(); - const int64_t num_nn = indices.GetShape()[1]; + + 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 int64_t *counts_ptr = counts.GetDataPtr(); + const scalar_t *distance2_ptr = distance2.GetDataPtr(); + const int32_t *counts_ptr = + counts.To(core::Int32).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; - core::ParallelFor(points.GetDevice(), n, - [=] OPEN3D_DEVICE(int64_t workload_idx) { - int64_t idx = 3 * 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) { + 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}; + 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; +#if defined(BUILD_CUDA_MODULE) && defined(__CUDACC__) + for (int j = 0; j < 33; j++) { + const scalar_t val = + spfhs_ptr[indices_ptr[idx] * 33 + j] / + dist; + sum[j / 11] += val; + atomicAdd(&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++) { + atomicAdd(&fpfhs_ptr[workload_idx * 33 + j], + sum[j / 11]); + atomicAdd(&fpfhs_ptr[workload_idx * 33 + j], + spfhs_ptr[workload_idx * 33 + j]); + } +#else +#pragma omp critical(ComputeFPFHFeatureCPU) + { + 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]; + } + } +#endif + } + } + }); }); -} +} // namespace kernel } // namespace kernel } // namespace pipelines diff --git a/cpp/open3d/t/pipelines/registration/Feature.cpp b/cpp/open3d/t/pipelines/registration/Feature.cpp index 6702bdae21f..d3a3d9e1792 100644 --- a/cpp/open3d/t/pipelines/registration/Feature.cpp +++ b/cpp/open3d/t/pipelines/registration/Feature.cpp @@ -28,6 +28,7 @@ #include "open3d/core/nns/NearestNeighborSearch.h" #include "open3d/t/geometry/PointCloud.h" +#include "open3d/t/pipelines/kernel/Feature.h" namespace open3d { namespace t { @@ -35,10 +36,9 @@ namespace t { namespace pipelines { namespace registration { -core::Tensor ComputeFPFHFeature( - const geometry::PointCloud &input, - const utility::optional max_nn = 100, - const utility::optional radius = utility::nullopt) { +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.value() <= 3) { @@ -87,11 +87,13 @@ core::Tensor ComputeFPFHFeature( input.GetPointPositions(), radius.value()); } - const core::Device device = input.GetDevice(); - const core::Dtype dtype = input.GetPointPositions().GetDtype(); 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 diff --git a/cpp/open3d/t/pipelines/registration/Feature.h b/cpp/open3d/t/pipelines/registration/Feature.h index a22ef0709e3..7bfbd6e117d 100644 --- a/cpp/open3d/t/pipelines/registration/Feature.h +++ b/cpp/open3d/t/pipelines/registration/Feature.h @@ -39,14 +39,16 @@ namespace pipelines { namespace registration { /// Function to compute FPFH feature for a point cloud. +/// It uses KNN search if only max_nn parameter is provided, Radius search if +/// only radius parameter is provided, and Hybrid search if both are provided. /// -/// \param input The Input point cloud. -/// \param max_nn [optional] Neighbor search max neighbors parameter [Default = +/// \param input The input point cloud with data type float32 ot float64. +/// \param max_nn [optional] Neighbor search max neighbors parameter. [Default = /// 100]. -/// \param radius [optional] Neighbor search radius parameter to use -/// HybridSearch. [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. +/// \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, 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..95e1470e431 --- /dev/null +++ b/cpp/pybind/t/pipelines/registration/feature.cpp @@ -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/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, + R"(Function to compute FPFH feature for a point cloud. +It uses KNN search if only max_nn parameter is provided, Radius search if only +radius parameter is provided, and Hybrid search if both are provided.)", + py::arg("input"), py::arg("max_nn") = 100, + py::arg("radius") = py::none()); + docstring::FunctionDocInject( + m, "compute_fpfh_feature", + {{"input", + "The input point cloud with data type float32 ot 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 \ No newline at end of file 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); From 238f141e408b6f0c7f926ca1ffc24dccf771adba Mon Sep 17 00:00:00 2001 From: yuecideng Date: Fri, 12 Aug 2022 07:45:40 +0000 Subject: [PATCH 04/15] fix error in fpfh kernel --- cpp/open3d/t/pipelines/kernel/FeatureImpl.h | 56 +++++-------------- .../t/pipelines/registration/Feature.cpp | 10 ++++ cpp/open3d/t/pipelines/registration/Feature.h | 10 ++-- cpp/open3d/visualization/gui/FileDialog.cpp | 2 +- .../t/pipelines/registration/feature.cpp | 12 ++-- 5 files changed, 36 insertions(+), 54 deletions(-) diff --git a/cpp/open3d/t/pipelines/kernel/FeatureImpl.h b/cpp/open3d/t/pipelines/kernel/FeatureImpl.h index 523182add37..02401b3db71 100644 --- a/cpp/open3d/t/pipelines/kernel/FeatureImpl.h +++ b/cpp/open3d/t/pipelines/kernel/FeatureImpl.h @@ -118,18 +118,9 @@ OPEN3D_HOST_DEVICE void UpdateSPFHFeature(const scalar_t *feature, int h_index3 = static_cast(floor(11 * (feature[2] + 1.0) * 0.5)); h_index3 = h_index3 >= 11 ? 10 : max(0, h_index3); -#if defined(BUILD_CUDA_MODULE) && defined(__CUDACC__) - atomicAdd(&spfh[idx * 33 + h_index1], hist_incr); - atomicAdd(&spfh[idx * 33 + 11 + h_index2], hist_incr); - atomicAdd(&spfh[idx * 33 + 22 + h_index3], hist_incr); -#else -#pragma omp critical(UpdateSPFHFeature) - { - spfh[idx * 33 + h_index1] += hist_incr; - spfh[idx * 33 + h_index2 + 11] += hist_incr; - spfh[idx * 33 + h_index3 + 22] += hist_incr; - } -#endif + 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__) @@ -216,7 +207,7 @@ void ComputeFPFHFeatureCPU : counts_ptr[workload_idx]; if (indice_size > 1) { - scalar_t sum[3] = {0}; + scalar_t sum[3] = {0.0, 0.0, 0.0}; for (int i = 1; i < indice_size; i++) { const int idx = is_radius_search @@ -224,43 +215,22 @@ void ComputeFPFHFeatureCPU : workload_idx * nn_size + i; const scalar_t dist = distance2_ptr[idx]; if (dist == 0.0) continue; -#if defined(BUILD_CUDA_MODULE) && defined(__CUDACC__) + for (int j = 0; j < 33; j++) { const scalar_t val = spfhs_ptr[indices_ptr[idx] * 33 + j] / dist; sum[j / 11] += val; - atomicAdd(&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++) { - atomicAdd(&fpfhs_ptr[workload_idx * 33 + j], - sum[j / 11]); - atomicAdd(&fpfhs_ptr[workload_idx * 33 + j], - spfhs_ptr[workload_idx * 33 + j]); - } -#else -#pragma omp critical(ComputeFPFHFeatureCPU) - { - 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]; - } - } -#endif + } + 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]; } } }); diff --git a/cpp/open3d/t/pipelines/registration/Feature.cpp b/cpp/open3d/t/pipelines/registration/Feature.cpp index d3a3d9e1792..277d91b249b 100644 --- a/cpp/open3d/t/pipelines/registration/Feature.cpp +++ b/cpp/open3d/t/pipelines/registration/Feature.cpp @@ -67,6 +67,10 @@ core::Tensor ComputeFPFHFeature(const geometry::PointCloud &input, } 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) { @@ -78,6 +82,9 @@ core::Tensor ComputeFPFHFeature(const geometry::PointCloud &input, // Make counts full with max_nn. counts = core::Tensor::Full({num_points}, max_nn.value(), dtype, 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) { @@ -85,6 +92,9 @@ core::Tensor ComputeFPFHFeature(const geometry::PointCloud &input, } std::tie(indices, distance2, counts) = tree.FixedRadiusSearch( input.GetPointPositions(), radius.value()); + utility::LogDebug( + "Use RadiusSearch [radius: {}] for computing FPFH feature.", + radius.value()); } const int64_t size = input.GetPointPositions().GetLength(); diff --git a/cpp/open3d/t/pipelines/registration/Feature.h b/cpp/open3d/t/pipelines/registration/Feature.h index 7bfbd6e117d..b63723eb1e2 100644 --- a/cpp/open3d/t/pipelines/registration/Feature.h +++ b/cpp/open3d/t/pipelines/registration/Feature.h @@ -27,6 +27,7 @@ #pragma once #include "open3d/core/Tensor.h" +#include "open3d/utility/Optional.h" namespace open3d { namespace t { @@ -39,14 +40,15 @@ namespace pipelines { namespace registration { /// Function to compute FPFH feature for a point cloud. -/// It uses KNN search if only max_nn parameter is provided, Radius search if -/// only radius parameter is provided, and Hybrid search if both are provided. +/// 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 ot float64. +/// \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]. +/// 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( diff --git a/cpp/open3d/visualization/gui/FileDialog.cpp b/cpp/open3d/visualization/gui/FileDialog.cpp index 353d19c3cc4..6be8293143c 100644 --- a/cpp/open3d/visualization/gui/FileDialog.cpp +++ b/cpp/open3d/visualization/gui/FileDialog.cpp @@ -128,7 +128,7 @@ struct FileDialog::Impl { std::shared_ptr dirtree_; std::shared_ptr filelist_; std::shared_ptr filter_; - std::unordered_map> + std::unordered_map > filter_idx_2_filter; std::shared_ptr filter_row_; std::shared_ptr