From f9691371b2aaa3c9258da551c77abd34856292c7 Mon Sep 17 00:00:00 2001 From: Heethesh Vhavle Date: Tue, 21 Jun 2022 17:11:59 -0700 Subject: [PATCH 01/14] Added Doppler ICP into tensor pipeline --- cpp/open3d/core/linalg/kernel/Matrix.h | 2 +- cpp/open3d/t/io/CMakeLists.txt | 1 + cpp/open3d/t/io/PointCloudIO.cpp | 8 +- cpp/open3d/t/io/PointCloudIO.h | 8 + cpp/open3d/t/io/file_format/FileXYZD.cpp | 160 +++++++++++++ .../t/pipelines/kernel/Registration.cpp | 106 +++++++++ cpp/open3d/t/pipelines/kernel/Registration.h | 56 +++++ .../t/pipelines/kernel/RegistrationCPU.cpp | 168 +++++++++++++ .../t/pipelines/kernel/RegistrationCUDA.cu | 153 ++++++++++++ .../t/pipelines/kernel/RegistrationImpl.h | 221 ++++++++++++++++++ .../kernel/TransformationConverter.cpp | 47 ++++ .../kernel/TransformationConverter.cu | 26 +++ .../kernel/TransformationConverter.h | 8 + .../kernel/TransformationConverterImpl.h | 26 +++ .../t/pipelines/registration/Registration.cpp | 166 +++++++++++++ .../t/pipelines/registration/Registration.h | 41 +++- .../pipelines/registration/RobustKernelImpl.h | 64 +++++ .../registration/TransformationEstimation.cpp | 99 ++++++++ .../registration/TransformationEstimation.h | 134 +++++++++++ .../t/pipelines/registration/registration.cpp | 133 ++++++++++- 20 files changed, 1618 insertions(+), 9 deletions(-) create mode 100644 cpp/open3d/t/io/file_format/FileXYZD.cpp diff --git a/cpp/open3d/core/linalg/kernel/Matrix.h b/cpp/open3d/core/linalg/kernel/Matrix.h index ee017f91d92..1db6df60af0 100644 --- a/cpp/open3d/core/linalg/kernel/Matrix.h +++ b/cpp/open3d/core/linalg/kernel/Matrix.h @@ -55,7 +55,7 @@ static OPEN3D_DEVICE OPEN3D_FORCE_INLINE void matmul3x3_3x1(const scalar_t& m00, } template -OPEN3D_DEVICE OPEN3D_FORCE_INLINE void matmul3x3_3x1(const scalar_t* A_3x3, +OPEN3D_HOST_DEVICE OPEN3D_FORCE_INLINE void matmul3x3_3x1(const scalar_t* A_3x3, const scalar_t* B_3x1, scalar_t* C_3x1) { C_3x1[0] = A_3x3[0] * B_3x1[0] + A_3x3[1] * B_3x1[1] + A_3x3[2] * B_3x1[2]; diff --git a/cpp/open3d/t/io/CMakeLists.txt b/cpp/open3d/t/io/CMakeLists.txt index 9375cbf595b..434f20ae839 100644 --- a/cpp/open3d/t/io/CMakeLists.txt +++ b/cpp/open3d/t/io/CMakeLists.txt @@ -14,6 +14,7 @@ target_sources(tio PRIVATE file_format/FilePLY.cpp file_format/FilePNG.cpp file_format/FilePTS.cpp + file_format/FileXYZD.cpp file_format/FileXYZI.cpp ) diff --git a/cpp/open3d/t/io/PointCloudIO.cpp b/cpp/open3d/t/io/PointCloudIO.cpp index 1c3acba7616..d3187a5c4bc 100644 --- a/cpp/open3d/t/io/PointCloudIO.cpp +++ b/cpp/open3d/t/io/PointCloudIO.cpp @@ -48,6 +48,7 @@ static const std::unordered_map< file_extension_to_pointcloud_read_function{ {"npz", ReadPointCloudFromNPZ}, {"xyzi", ReadPointCloudFromXYZI}, + {"xyzd", ReadPointCloudFromXYZD}, {"pcd", ReadPointCloudFromPCD}, {"ply", ReadPointCloudFromPLY}, {"pts", ReadPointCloudFromPTS}, @@ -59,8 +60,11 @@ static const std::unordered_map< const geometry::PointCloud &, const open3d::io::WritePointCloudOption &)>> file_extension_to_pointcloud_write_function{ - {"npz", WritePointCloudToNPZ}, {"xyzi", WritePointCloudToXYZI}, - {"pcd", WritePointCloudToPCD}, {"ply", WritePointCloudToPLY}, + {"npz", WritePointCloudToNPZ}, + {"xyzi", WritePointCloudToXYZI}, + {"xyzd", WritePointCloudToXYZD}, + {"pcd", WritePointCloudToPCD}, + {"ply", WritePointCloudToPLY}, {"pts", WritePointCloudToPTS}, }; diff --git a/cpp/open3d/t/io/PointCloudIO.h b/cpp/open3d/t/io/PointCloudIO.h index 1d8d50df404..b5f8a97fc8d 100644 --- a/cpp/open3d/t/io/PointCloudIO.h +++ b/cpp/open3d/t/io/PointCloudIO.h @@ -77,6 +77,14 @@ bool WritePointCloudToXYZI(const std::string &filename, const geometry::PointCloud &pointcloud, const WritePointCloudOption ¶ms); +bool ReadPointCloudFromXYZD(const std::string &filename, + geometry::PointCloud &pointcloud, + const ReadPointCloudOption ¶ms); + +bool WritePointCloudToXYZD(const std::string &filename, + const geometry::PointCloud &pointcloud, + const WritePointCloudOption ¶ms); + bool ReadPointCloudFromPCD(const std::string &filename, geometry::PointCloud &pointcloud, const ReadPointCloudOption ¶ms); diff --git a/cpp/open3d/t/io/file_format/FileXYZD.cpp b/cpp/open3d/t/io/file_format/FileXYZD.cpp new file mode 100644 index 00000000000..c0de00c5377 --- /dev/null +++ b/cpp/open3d/t/io/file_format/FileXYZD.cpp @@ -0,0 +1,160 @@ +// ---------------------------------------------------------------------------- +// - 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 +#include + +#include "open3d/core/Dtype.h" +#include "open3d/core/Tensor.h" +#include "open3d/io/FileFormatIO.h" +#include "open3d/t/io/PointCloudIO.h" +#include "open3d/utility/FileSystem.h" +#include "open3d/utility/Logging.h" +#include "open3d/utility/ProgressReporters.h" + +namespace open3d { +namespace t { +namespace io { + +open3d::io::FileGeometry ReadFileGeometryTypeXYZD(const std::string &path) { + return open3d::io::CONTAINS_POINTS; +} + +bool ReadPointCloudFromXYZD(const std::string &filename, + geometry::PointCloud &pointcloud, + const open3d::io::ReadPointCloudOption ¶ms) { + try { + utility::filesystem::CFile file; + if (!file.Open(filename, "r")) { + utility::LogWarning("Read XYZD failed: unable to open file: {}", + filename); + return false; + } + utility::CountingProgressReporter reporter(params.update_progress); + reporter.SetTotal(file.GetFileSize()); + int64_t num_points = file.GetNumLines(); + + pointcloud.Clear(); + core::Tensor points({num_points, 3}, core::Float64); + core::Tensor dopplers({num_points}, core::Float64); + core::Tensor directions({num_points, 3}, core::Float64); + double *points_ptr = points.GetDataPtr(); + double *dopplers_ptr = dopplers.GetDataPtr(); + double *directions_ptr = directions.GetDataPtr(); + + int i = 0; + double x, y, z, doppler; + const char *line_buffer; + while ((line_buffer = file.ReadLine())) { + if (sscanf(line_buffer, "%lf %lf %lf %lf", &x, &y, &z, &doppler) == 4) { + points_ptr[3 * i + 0] = x; + points_ptr[3 * i + 1] = y; + points_ptr[3 * i + 2] = z; + dopplers_ptr[i] = doppler; + + const double norm = + sqrt(pow(x, 2.0) + pow(y, 2.0) + pow(z, 2.0)); + directions_ptr[3 * i + 0] = x / norm; + directions_ptr[3 * i + 1] = y / norm; + directions_ptr[3 * i + 2] = z / norm; + } + if (++i % 1000 == 0) { + reporter.Update(file.CurPos()); + } + } + pointcloud.SetPointPositions(points); + pointcloud.SetPointAttr("dopplers", dopplers); + pointcloud.SetPointAttr("directions", directions); + reporter.Finish(); + + return true; + } catch (const std::exception &e) { + utility::LogWarning("Read XYZ failed with exception: {}", e.what()); + return false; + } +} + +bool WritePointCloudToXYZD(const std::string &filename, + const geometry::PointCloud &pointcloud, + const open3d::io::WritePointCloudOption ¶ms) { + if (!pointcloud.HasPointAttr("dopplers")) { + utility::LogWarning("Missing dopplers attribute in the point cloud"); + return false; + } + + try { + utility::filesystem::CFile file; + if (!file.Open(filename, "w")) { + utility::LogWarning("Write XYZD failed: unable to open file: {}", + filename); + return false; + } + utility::CountingProgressReporter reporter(params.update_progress); + const core::Tensor &points = pointcloud.GetPointPositions(); + if (!points.GetShape().IsCompatible({utility::nullopt, 3})) { + utility::LogWarning( + "Write XYZD failed: Shape of points is {}, but it should " + "be Nx3.", + points.GetShape()); + return false; + } + const core::Tensor &dopplers = + pointcloud.GetPointAttr("dopplers").Flatten(); + if (points.GetShape(0) != dopplers.GetShape(0)) { + utility::LogWarning( + "Write XYZD failed: Points ({}) and Dopplers ({}) have " + "different lengths.", + points.GetShape(0), dopplers.GetShape(0)); + return false; + } + reporter.SetTotal(points.GetShape(0)); + + for (int i = 0; i < points.GetShape(0); i++) { + if (fprintf(file.GetFILE(), "%.10f %.10f %.10f %.10f\n", + points[i][0].Item(), + points[i][1].Item(), + points[i][2].Item(), + dopplers[i].Item()) < 0) { + utility::LogWarning( + "Write XYZD failed: unable to write file: {}", + filename); + return false; // error happened during writing. + } + if (i % 1000 == 0) { + reporter.Update(i); + } + } + reporter.Finish(); + return true; + } catch (const std::exception &e) { + utility::LogWarning("Write XYZD failed with exception: {}", e.what()); + return false; + } +} + +} // namespace io +} // namespace t +} // namespace open3d diff --git a/cpp/open3d/t/pipelines/kernel/Registration.cpp b/cpp/open3d/t/pipelines/kernel/Registration.cpp index 204b2e6be04..3e2cd05a31c 100644 --- a/cpp/open3d/t/pipelines/kernel/Registration.cpp +++ b/cpp/open3d/t/pipelines/kernel/Registration.cpp @@ -26,6 +26,7 @@ #include "open3d/t/pipelines/kernel/Registration.h" +#include "open3d/core/Dispatch.h" #include "open3d/core/TensorCheck.h" #include "open3d/t/pipelines/kernel/RegistrationImpl.h" @@ -111,6 +112,111 @@ core::Tensor ComputePoseColoredICP(const core::Tensor &source_points, return pose; } +core::Tensor ComputePoseDopplerICP( + const core::Tensor &source_points, + const core::Tensor &source_dopplers, + const core::Tensor &source_directions, + const core::Tensor &target_points, + const core::Tensor &target_normals, + const core::Tensor &correspondence_indices, + const core::Tensor ¤t_transform, + const core::Tensor &T_V_to_S, + const double period, + const size_t iteration, + const bool reject_dynamic_outliers, + const double doppler_outlier_threshold, + const size_t outlier_rejection_min_iteration, + const size_t geometric_robust_loss_min_iteration, + const size_t doppler_robust_loss_min_iteration, + const registration::RobustKernel &geometric_kernel, + const registration::RobustKernel &doppler_kernel, + const double &lambda_doppler) { + const core::Device device = source_points.GetDevice(); + const core::Dtype dtype = source_points.GetDtype(); + + // Pose {6,} tensor [ouput]. + core::Tensor output_pose = + core::Tensor::Empty({6}, core::Dtype::Float64, device); + + float residual = 0; + int inlier_count = 0; + + // Use robust kernels only after a specified minimum number of iterations. + const auto kernel_default = registration::RobustKernel( + registration::RobustKernelMethod::L2Loss, 1.0, 1.0); + const auto kernel_geometric = + (iteration >= geometric_robust_loss_min_iteration) + ? geometric_kernel + : kernel_default; + const auto kernel_doppler = (iteration >= doppler_robust_loss_min_iteration) + ? doppler_kernel + : kernel_default; + + // Enable outlier rejection based on the current iteration count. + const bool reject_outliers = reject_dynamic_outliers && + (iteration >= outlier_rejection_min_iteration); + + // Extract the rotation and translation parts from the matrix. + const core::Tensor R_S_to_V = + T_V_to_S.GetItem({core::TensorKey::Slice(0, 3, 1), + core::TensorKey::Slice(0, 3, 1)}) + .Inverse() + .Flatten() + .To(device, dtype); + const core::Tensor r_v_to_s_in_V = + T_V_to_S.GetItem({core::TensorKey::Slice(0, 3, 1), + core::TensorKey::Slice(3, 4, 1)}) + .Flatten() + .To(device, dtype); + + // Compute the pose (rotation + translation) vector. + const core::Tensor state_vector = + pipelines::kernel::TransformationToPose(current_transform) + .To(device, dtype); + + // Compute the linear and angular velocity from the pose vector. + const core::Tensor w_v_in_V = + (state_vector.GetItem(core::TensorKey::Slice(0, 3, 1)).Neg() / + period) + .To(device, dtype); + const core::Tensor v_v_in_V = + (state_vector.GetItem(core::TensorKey::Slice(3, 6, 1)).Neg() / + period) + .To(device, dtype); + + core::Device::DeviceType device_type = device.GetType(); + if (device_type == core::Device::DeviceType::CPU) { + ComputePoseDopplerICPCPU( + source_points.Contiguous(), source_dopplers.Contiguous(), + source_directions.Contiguous(), target_points.Contiguous(), + target_normals.Contiguous(), + correspondence_indices.Contiguous(), output_pose, residual, + inlier_count, dtype, device, R_S_to_V.Contiguous(), + r_v_to_s_in_V.Contiguous(), w_v_in_V.Contiguous(), + v_v_in_V.Contiguous(), period, reject_outliers, + doppler_outlier_threshold, kernel_geometric, kernel_doppler, + lambda_doppler); + } else if (device_type == core::Device::DeviceType::CUDA) { + CUDA_CALL(ComputePoseDopplerICPCUDA, source_points.Contiguous(), + source_dopplers.Contiguous(), source_directions.Contiguous(), + target_points.Contiguous(), target_normals.Contiguous(), + correspondence_indices.Contiguous(), output_pose, residual, + inlier_count, dtype, device, R_S_to_V.Contiguous(), + r_v_to_s_in_V.Contiguous(), w_v_in_V.Contiguous(), + v_v_in_V.Contiguous(), period, reject_outliers, + doppler_outlier_threshold, kernel_geometric, kernel_doppler, + lambda_doppler); + } else { + utility::LogError("Unimplemented device."); + } + + utility::LogDebug( + "DopplerPointToPlane Transform: residual {}, inlier_count {}", + residual, inlier_count); + + return output_pose; +} + std::tuple ComputeRtPointToPoint( const core::Tensor &source_points, const core::Tensor &target_points, diff --git a/cpp/open3d/t/pipelines/kernel/Registration.h b/cpp/open3d/t/pipelines/kernel/Registration.h index 700fe4aff33..9db035cf83d 100644 --- a/cpp/open3d/t/pipelines/kernel/Registration.h +++ b/cpp/open3d/t/pipelines/kernel/Registration.h @@ -87,6 +87,62 @@ core::Tensor ComputePoseColoredICP(const core::Tensor &source_positions, const registration::RobustKernel &kernel, const double &lambda_geometric); +/// \brief Computes pose for DopplerICP registration method. +/// +/// \param source_points source point positions of Float32 or Float64 dtype. +/// \param source_dopplers source point Dopplers of same dtype as source point +/// positions. +/// \param source_directions source point direction of same dtype as source +/// point positions. +/// \param target_points target point positions of same dtype as source point +/// positions. +/// \param target_normals target point normals of same dtype as source point +/// positions. +/// \param correspondence_indices Tensor of type Int64 containing indices of +/// corresponding target positions, where the value is the target index and the +/// index of the value itself is the source index. It contains -1 as value at +/// index with no correspondence. +/// \param current_transform The current pose estimate of ICP. +/// \param T_V_to_S The 4x4 transformation matrix to transform +/// sensor to vehicle frame. +/// \param period Time period (in seconds) between the source and the target +/// point clouds. +/// \param iteration current iteration number of the ICP algorithm. +/// \param reject_dynamic_outliers Whether or not to prune dynamic point +/// outlier correspondences. +/// \param doppler_outlier_threshold Correspondences with Doppler error +/// greater than this threshold are rejected from optimization. +/// \param outlier_rejection_min_iteration Number of iterations of ICP after +/// which outlier rejection is enabled. +/// \param geometric_robust_loss_min_iteration Number of iterations of ICP +/// after which robust loss for geometric term kicks in. +/// \param doppler_robust_loss_min_iteration Number of iterations of ICP +/// after which robust loss for Doppler term kicks in. +/// \param geometric_kernel statistical robust kernel for outlier rejection. +/// \param doppler_kernel statistical robust kernel for outlier rejection. +/// \param lambda_doppler weight for the Doppler objective. +/// \return Pose [alpha beta gamma, tx, ty, tz], a shape {6} tensor of dtype +/// Float64, where alpha, beta, gamma are the Euler angles in the ZYX order. +core::Tensor ComputePoseDopplerICP( + const core::Tensor &source_points, + const core::Tensor &source_dopplers, + const core::Tensor &source_directions, + const core::Tensor &target_points, + const core::Tensor &target_normals, + const core::Tensor &correspondence_indices, + const core::Tensor ¤t_transform, + const core::Tensor &T_V_to_S, + const double period, + const size_t iteration, + const bool reject_dynamic_outliers, + const double doppler_outlier_threshold, + const size_t outlier_rejection_min_iteration, + const size_t geometric_robust_loss_min_iteration, + const size_t doppler_robust_loss_min_iteration, + const registration::RobustKernel &geometric_kernel, + const registration::RobustKernel &doppler_kernel, + const double &lambda_doppler); + /// \brief Computes (R) Rotation {3,3} and (t) translation {3,} /// for point to point registration method. /// diff --git a/cpp/open3d/t/pipelines/kernel/RegistrationCPU.cpp b/cpp/open3d/t/pipelines/kernel/RegistrationCPU.cpp index ae347b8f238..22d3fa417e7 100644 --- a/cpp/open3d/t/pipelines/kernel/RegistrationCPU.cpp +++ b/cpp/open3d/t/pipelines/kernel/RegistrationCPU.cpp @@ -268,6 +268,174 @@ void ComputePoseColoredICPCPU(const core::Tensor &source_points, DecodeAndSolve6x6(global_sum, pose, residual, inlier_count); } +template +static void ComputePoseDopplerICPKernelCPU( + const scalar_t *source_points_ptr, + const scalar_t *source_dopplers_ptr, + const scalar_t *source_directions_ptr, + const scalar_t *target_points_ptr, + const scalar_t *target_normals_ptr, + const int64_t *correspondence_indices, + const scalar_t *R_S_to_V, + const scalar_t *r_v_to_s_in_V, + const scalar_t *v_s_in_S, + const bool prune_correspondences, + const scalar_t doppler_outlier_threshold, + const scalar_t sqrt_lambda_geometric, + const scalar_t sqrt_lambda_doppler, + const scalar_t sqrt_lambda_doppler_by_dt, + const int n, + scalar_t *global_sum, + funct1_t GetWeightFromRobustKernelFirst, // Geometric kernel + funct2_t GetWeightFromRobustKernelSecond // Doppler kernel +) { + // As, AtA is a symmetric matrix, we only need 21 elements instead of 36. + // Atb is of shape {6,1}. Combining both, A_1x29 is a temp. storage + // with [0:21] elements as AtA, [21:27] elements as Atb, 27th as residual + // and 28th as inlier_count. + std::vector A_1x29(29, 0.0); + +#ifdef _WIN32 + std::vector zeros_29(29, 0.0); + A_1x29 = tbb::parallel_reduce( + tbb::blocked_range(0, n), zeros_29, + [&](tbb::blocked_range r, std::vector A_reduction) { + for (int workload_idx = r.begin(); workload_idx < r.end(); + ++workload_idx) { +#else + scalar_t *A_reduction = A_1x29.data(); +#pragma omp parallel for reduction(+ : A_reduction[:29]) schedule(static) num_threads(utility::EstimateMaxThreads()) + for (int workload_idx = 0; workload_idx < n; ++workload_idx) { +#endif + scalar_t J_G[6] = {0}, J_D[6] = {0}; + scalar_t r_G = 0, r_D = 0; + + bool valid = GetJacobianDopplerICP( + workload_idx, source_points_ptr, + source_dopplers_ptr, source_directions_ptr, + target_points_ptr, target_normals_ptr, + correspondence_indices, R_S_to_V, r_v_to_s_in_V, + v_s_in_S, prune_correspondences, + doppler_outlier_threshold, sqrt_lambda_geometric, + sqrt_lambda_doppler, sqrt_lambda_doppler_by_dt, J_G, + J_D, r_G, r_D); + + scalar_t w_G = GetWeightFromRobustKernelFirst(r_G); + scalar_t w_D = GetWeightFromRobustKernelSecond(r_D); + + if (valid) { + // Dump J, r into JtJ and Jtr + int i = 0; + for (int j = 0; j < 6; ++j) { + for (int k = 0; k <= j; ++k) { + A_reduction[i] += J_G[j] * w_G * J_G[k] + + J_D[j] * w_D * J_D[k]; + ++i; + } + A_reduction[21 + j] += + J_G[j] * w_G * r_G + J_D[j] * w_D * r_D; + } + A_reduction[27] += r_G * r_G + r_D * r_D; + A_reduction[28] += 1; + } + } +#ifdef _WIN32 + return A_reduction; + }, + // TBB: Defining reduction operation. + [&](std::vector a, std::vector b) { + std::vector result(29); + for (int j = 0; j < 29; ++j) { + result[j] = a[j] + b[j]; + } + return result; + }); +#endif + + for (int i = 0; i < 29; ++i) { + global_sum[i] = A_1x29[i]; + } +} + +template +void PreComputeForDopplerICPKernelCPU(const scalar_t *R_S_to_V, + const scalar_t *r_v_to_s_in_V, + const scalar_t *w_v_in_V, + const scalar_t *v_v_in_V, + scalar_t *v_s_in_S) { + PreComputeForDopplerICP(R_S_to_V, r_v_to_s_in_V, w_v_in_V, v_v_in_V, + v_s_in_S); +} + +void ComputePoseDopplerICPCPU( + const core::Tensor &source_points, + const core::Tensor &source_dopplers, + const core::Tensor &source_directions, + const core::Tensor &target_points, + const core::Tensor &target_normals, + const core::Tensor &correspondence_indices, + core::Tensor &output_pose, + float &residual, + int &inlier_count, + const core::Dtype &dtype, + const core::Device &device, + const core::Tensor &R_S_to_V, + const core::Tensor &r_v_to_s_in_V, + const core::Tensor &w_v_in_V, + const core::Tensor &v_v_in_V, + const double period, + const bool prune_correspondences, + const double doppler_outlier_threshold, + const registration::RobustKernel &kernel_geometric, + const registration::RobustKernel &kernel_doppler, + const double &lambda_doppler) { + const int n = source_points.GetLength(); + + core::Tensor global_sum = core::Tensor::Zeros({29}, dtype, device); + core::Tensor v_s_in_S = core::Tensor::Zeros({3}, dtype, device); + + DISPATCH_FLOAT_DTYPE_TO_TEMPLATE(dtype, [&]() { + const scalar_t sqrt_lambda_geometric = + sqrt(1.0 - static_cast(lambda_doppler)); + const scalar_t sqrt_lambda_doppler = sqrt(lambda_doppler); + const scalar_t sqrt_lambda_doppler_by_dt = + sqrt_lambda_doppler / static_cast(period); + + PreComputeForDopplerICPKernelCPU( + R_S_to_V.GetDataPtr(), + r_v_to_s_in_V.GetDataPtr(), + w_v_in_V.GetDataPtr(), + v_v_in_V.GetDataPtr(), + v_s_in_S.GetDataPtr()); + + DISPATCH_DUAL_ROBUST_KERNEL_FUNCTION( + scalar_t, kernel_geometric.type_, + kernel_geometric.scaling_parameter_, kernel_doppler.type_, + kernel_doppler.scaling_parameter_, [&]() { + kernel::ComputePoseDopplerICPKernelCPU( + source_points.GetDataPtr(), + source_dopplers.GetDataPtr(), + source_directions.GetDataPtr(), + target_points.GetDataPtr(), + target_normals.GetDataPtr(), + correspondence_indices.GetDataPtr(), + R_S_to_V.GetDataPtr(), + r_v_to_s_in_V.GetDataPtr(), + v_s_in_S.GetDataPtr(), + prune_correspondences, + static_cast(doppler_outlier_threshold), + sqrt_lambda_geometric, sqrt_lambda_doppler, + sqrt_lambda_doppler_by_dt, n, + global_sum.GetDataPtr(), + GetWeightFromRobustKernelFirst, // Geometric kernel + GetWeightFromRobustKernelSecond // Doppler kernel + ); + }); + }); + + DecodeAndSolve6x6(global_sum, output_pose, residual, inlier_count); +} + template static void Get3x3SxyLinearSystem(const scalar_t *source_points_ptr, const scalar_t *target_points_ptr, diff --git a/cpp/open3d/t/pipelines/kernel/RegistrationCUDA.cu b/cpp/open3d/t/pipelines/kernel/RegistrationCUDA.cu index 4980b651fc3..2f27960d4b1 100644 --- a/cpp/open3d/t/pipelines/kernel/RegistrationCUDA.cu +++ b/cpp/open3d/t/pipelines/kernel/RegistrationCUDA.cu @@ -246,6 +246,159 @@ void ComputePoseColoredICPCUDA(const core::Tensor &source_points, DecodeAndSolve6x6(global_sum, pose, residual, inlier_count); } +template +__global__ void ComputePoseDopplerICPKernelCUDA( + const scalar_t *source_points_ptr, + const scalar_t *source_dopplers_ptr, + const scalar_t *source_directions_ptr, + const scalar_t *target_points_ptr, + const scalar_t *target_normals_ptr, + const int64_t *correspondence_indices, + const scalar_t *R_S_to_V, + const scalar_t *r_v_to_s_in_V, + const scalar_t *v_s_in_S, + const bool prune_correspondences, + const scalar_t doppler_outlier_threshold, + const scalar_t sqrt_lambda_geometric, + const scalar_t sqrt_lambda_doppler, + const scalar_t sqrt_lambda_doppler_by_dt, + const int n, + scalar_t *global_sum, + funct1_t GetWeightFromRobustKernelFirst, // Geometric kernel + funct2_t GetWeightFromRobustKernelSecond // Doppler kernel +) { + __shared__ scalar_t local_sum0[kThread1DUnit]; + __shared__ scalar_t local_sum1[kThread1DUnit]; + __shared__ scalar_t local_sum2[kThread1DUnit]; + + const int tid = threadIdx.x; + + local_sum0[tid] = 0; + local_sum1[tid] = 0; + local_sum2[tid] = 0; + + const int workload_idx = threadIdx.x + blockIdx.x * blockDim.x; + + if (workload_idx >= n) return; + + scalar_t J_G[6] = {0}, J_D[6] = {0}, reduction[29] = {0}; + scalar_t r_G = 0, r_D = 0; + + bool valid = GetJacobianDopplerICP( + workload_idx, source_points_ptr, source_dopplers_ptr, + source_directions_ptr, target_points_ptr, target_normals_ptr, + correspondence_indices, R_S_to_V, r_v_to_s_in_V, v_s_in_S, + prune_correspondences, doppler_outlier_threshold, + sqrt_lambda_geometric, sqrt_lambda_doppler, + sqrt_lambda_doppler_by_dt, J_G, J_D, r_G, r_D); + + scalar_t w_G = GetWeightFromRobustKernelFirst(r_G); + scalar_t w_D = GetWeightFromRobustKernelSecond(r_D); + + if (valid) { + // Dump J, r into JtJ and Jtr + int i = 0; + for (int j = 0; j < 6; ++j) { + for (int k = 0; k <= j; ++k) { + reduction[i] += J_G[j] * w_G * J_G[k] + J_D[j] * w_D * J_D[k]; + ++i; + } + reduction[21 + j] += J_G[j] * w_G * r_G + J_D[j] * w_D * r_D; + } + reduction[27] += r_G * r_G + r_D * r_D; + reduction[28] += 1; + } + + ReduceSum6x6LinearSystem(tid, valid, reduction, + local_sum0, local_sum1, + local_sum2, global_sum); +} + +template +__global__ void PreComputeForDopplerICPKernelCUDA(const scalar_t *R_S_to_V, + const scalar_t *r_v_to_s_in_V, + const scalar_t *w_v_in_V, + const scalar_t *v_v_in_V, + scalar_t *v_s_in_S) { + PreComputeForDopplerICP(R_S_to_V, r_v_to_s_in_V, w_v_in_V, v_v_in_V, + v_s_in_S); +} + +void ComputePoseDopplerICPCUDA( + const core::Tensor &source_points, + const core::Tensor &source_dopplers, + const core::Tensor &source_directions, + const core::Tensor &target_points, + const core::Tensor &target_normals, + const core::Tensor &correspondence_indices, + core::Tensor &output_pose, + float &residual, + int &inlier_count, + const core::Dtype &dtype, + const core::Device &device, + const core::Tensor &R_S_to_V, + const core::Tensor &r_v_to_s_in_V, + const core::Tensor &w_v_in_V, + const core::Tensor &v_v_in_V, + const double period, + const bool prune_correspondences, + const double doppler_outlier_threshold, + const registration::RobustKernel &kernel_geometric, + const registration::RobustKernel &kernel_doppler, + const double &lambda_doppler) { + int n = source_points.GetLength(); + const dim3 blocks((n + kThread1DUnit - 1) / kThread1DUnit); + const dim3 threads(kThread1DUnit); + + core::Tensor global_sum = core::Tensor::Zeros({29}, dtype, device); + core::Tensor v_s_in_S = core::Tensor::Zeros({3}, dtype, device); + + DISPATCH_FLOAT_DTYPE_TO_TEMPLATE(dtype, [&]() { + const scalar_t sqrt_lambda_geometric = + sqrt(1.0 - static_cast(lambda_doppler)); + const scalar_t sqrt_lambda_doppler = sqrt(lambda_doppler); + const scalar_t sqrt_lambda_doppler_by_dt = + sqrt_lambda_doppler / static_cast(period); + + PreComputeForDopplerICPKernelCUDA + <<<1, 1, 0, core::cuda::GetStream()>>>( + R_S_to_V.GetDataPtr(), + r_v_to_s_in_V.GetDataPtr(), + w_v_in_V.GetDataPtr(), + v_v_in_V.GetDataPtr(), + v_s_in_S.GetDataPtr()); + + DISPATCH_DUAL_ROBUST_KERNEL_FUNCTION( + scalar_t, kernel_geometric.type_, + kernel_geometric.scaling_parameter_, kernel_doppler.type_, + kernel_doppler.scaling_parameter_, [&]() { + ComputePoseDopplerICPKernelCUDA<<< + blocks, threads, 0, core::cuda::GetStream()>>>( + source_points.GetDataPtr(), + source_dopplers.GetDataPtr(), + source_directions.GetDataPtr(), + target_points.GetDataPtr(), + target_normals.GetDataPtr(), + correspondence_indices.GetDataPtr(), + R_S_to_V.GetDataPtr(), + r_v_to_s_in_V.GetDataPtr(), + v_s_in_S.GetDataPtr(), + prune_correspondences, + static_cast(doppler_outlier_threshold), + sqrt_lambda_geometric, sqrt_lambda_doppler, + sqrt_lambda_doppler_by_dt, n, + global_sum.GetDataPtr(), + GetWeightFromRobustKernelFirst, // Geometric kernel + GetWeightFromRobustKernelSecond // Doppler kernel + ); + }); + }); + + core::cuda::Synchronize(); + + DecodeAndSolve6x6(global_sum, output_pose, residual, inlier_count); +} + template __global__ void ComputeInformationMatrixKernelCUDA( const scalar_t *target_points_ptr, diff --git a/cpp/open3d/t/pipelines/kernel/RegistrationImpl.h b/cpp/open3d/t/pipelines/kernel/RegistrationImpl.h index 6624c175f6d..32f3c0b73d7 100644 --- a/cpp/open3d/t/pipelines/kernel/RegistrationImpl.h +++ b/cpp/open3d/t/pipelines/kernel/RegistrationImpl.h @@ -28,10 +28,18 @@ #pragma once +#include + #include "open3d/core/CUDAUtils.h" #include "open3d/core/Tensor.h" +#include "open3d/core/linalg/kernel/Matrix.h" +#include "open3d/t/pipelines/kernel/TransformationConverter.h" #include "open3d/t/pipelines/registration/RobustKernel.h" +#ifndef __CUDACC__ +using std::abs; +#endif + namespace open3d { namespace t { namespace pipelines { @@ -63,6 +71,29 @@ void ComputePoseColoredICPCPU(const core::Tensor &source_points, const registration::RobustKernel &kernel, const double &lambda_geometric); +void ComputePoseDopplerICPCPU( + const core::Tensor &source_points, + const core::Tensor &source_dopplers, + const core::Tensor &source_directions, + const core::Tensor &target_points, + const core::Tensor &target_normals, + const core::Tensor &correspondence_indices, + core::Tensor &output_pose, + float &residual, + int &inlier_count, + const core::Dtype &dtype, + const core::Device &device, + const core::Tensor &R_S_to_V, + const core::Tensor &r_v_to_s_in_V, + const core::Tensor &w_v_in_V, + const core::Tensor &v_v_in_V, + const double period, + const bool prune_correspondences, + const double doppler_outlier_threshold, + const registration::RobustKernel &kernel_geometric, + const registration::RobustKernel &kernel_doppler, + const double &lambda_doppler); + #ifdef BUILD_CUDA_MODULE void ComputePosePointToPlaneCUDA(const core::Tensor &source_points, const core::Tensor &target_points, @@ -89,6 +120,29 @@ void ComputePoseColoredICPCUDA(const core::Tensor &source_points, const core::Device &device, const registration::RobustKernel &kernel, const double &lambda_geometric); + +void ComputePoseDopplerICPCUDA( + const core::Tensor &source_points, + const core::Tensor &source_dopplers, + const core::Tensor &source_directions, + const core::Tensor &target_points, + const core::Tensor &target_normals, + const core::Tensor &correspondence_indices, + core::Tensor &output_pose, + float &residual, + int &inlier_count, + const core::Dtype &dtype, + const core::Device &device, + const core::Tensor &R_S_to_V, + const core::Tensor &r_v_to_s_in_V, + const core::Tensor &w_v_in_V, + const core::Tensor &v_v_in_V, + const double period, + const bool prune_correspondences, + const double doppler_outlier_threshold, + const registration::RobustKernel &kernel_geometric, + const registration::RobustKernel &kernel_doppler, + const double &lambda_doppler); #endif void ComputeRtPointToPointCPU(const core::Tensor &source_points, @@ -280,6 +334,173 @@ template bool GetJacobianColoredICP(const int64_t workload_idx, double &r_G, double &r_I); +template +static void DisplayVector(std::string name, const scalar_T *vector) { + utility::LogDebug("{} = [{}, {}, {}]", name, vector[0], vector[1], + vector[2]); +} + +template +OPEN3D_HOST_DEVICE inline void PreComputeForDopplerICP( + const scalar_t *R_S_to_V, + const scalar_t *r_v_to_s_in_V, + const scalar_t *w_v_in_V, + const scalar_t *v_v_in_V, + scalar_t *v_s_in_S) { + // Compute v_s_in_V = v_v_in_V + w_v_in_V.cross(r_v_to_s_in_V). + scalar_t v_s_in_V[3] = {0}; + core::linalg::kernel::cross_3x1(w_v_in_V, r_v_to_s_in_V, v_s_in_V); + v_s_in_V[0] += v_v_in_V[0]; + v_s_in_V[1] += v_v_in_V[1]; + v_s_in_V[2] += v_v_in_V[2]; + + // Compute v_s_in_S = R_S_to_V * v_s_in_V. + core::linalg::kernel::matmul3x3_3x1(R_S_to_V, v_s_in_V, v_s_in_S); +} + +template void PreComputeForDopplerICP(const float *R_S_to_V, + const float *r_v_to_s_in_V, + const float *w_v_in_V, + const float *v_v_in_V, + float *v_s_in_S); + +template void PreComputeForDopplerICP(const double *R_S_to_V, + const double *r_v_to_s_in_V, + const double *w_v_in_V, + const double *v_v_in_V, + double *v_s_in_S); + +template +OPEN3D_HOST_DEVICE inline bool GetJacobianDopplerICP( + const int64_t workload_idx, + const scalar_t *source_points_ptr, + const scalar_t *source_dopplers_ptr, + const scalar_t *source_directions_ptr, + const scalar_t *target_points_ptr, + const scalar_t *target_normals_ptr, + const int64_t *correspondence_indices, + const scalar_t *R_S_to_V, + const scalar_t *r_v_to_s_in_V, + const scalar_t *v_s_in_S, + const bool prune_correspondences, + const scalar_t doppler_outlier_threshold, + const scalar_t &sqrt_lambda_geometric, + const scalar_t &sqrt_lambda_doppler, + const scalar_t &sqrt_lambda_doppler_by_dt, + scalar_t *J_G, + scalar_t *J_D, + scalar_t &r_G, + scalar_t &r_D) { + // Reference paper: https://arxiv.org/abs/2201.11944. + if (correspondence_indices[workload_idx] == -1) { + return false; + } + + const int64_t target_idx = 3 * correspondence_indices[workload_idx]; + const int64_t source_idx = 3 * workload_idx; + + const scalar_t &doppler_in_S = source_dopplers_ptr[workload_idx]; + + const scalar_t ds_in_V[3] = {source_directions_ptr[source_idx], + source_directions_ptr[source_idx + 1], + source_directions_ptr[source_idx + 2]}; + + // Compute predicted Doppler velocity (in sensor frame). + scalar_t ds_in_S[3] = {0}; + core::linalg::kernel::matmul3x3_3x1(R_S_to_V, ds_in_V, ds_in_S); + const scalar_t doppler_pred_in_S = + -core::linalg::kernel::dot_3x1(ds_in_S, v_s_in_S); + + // Compute Doppler error. + const double doppler_error = doppler_in_S - doppler_pred_in_S; + + // Dynamic point outlier rejection. + if (prune_correspondences && + abs(doppler_error) > doppler_outlier_threshold) { + // Jacobian and residual are set to 0 by default. + return true; + } + + // Compute Doppler residual and Jacobian. + scalar_t J_D_w[3] = {0}; + core::linalg::kernel::cross_3x1(ds_in_V, r_v_to_s_in_V, J_D_w); + J_D[0] = sqrt_lambda_doppler_by_dt * J_D_w[0]; + J_D[1] = sqrt_lambda_doppler_by_dt * J_D_w[1]; + J_D[2] = sqrt_lambda_doppler_by_dt * J_D_w[2]; + J_D[3] = sqrt_lambda_doppler_by_dt * -ds_in_V[0]; + J_D[4] = sqrt_lambda_doppler_by_dt * -ds_in_V[1]; + J_D[5] = sqrt_lambda_doppler_by_dt * -ds_in_V[2]; + r_D = sqrt_lambda_doppler * doppler_error; + + const scalar_t ps[3] = {source_points_ptr[source_idx], + source_points_ptr[source_idx + 1], + source_points_ptr[source_idx + 2]}; + + const scalar_t pt[3] = {target_points_ptr[target_idx], + target_points_ptr[target_idx + 1], + target_points_ptr[target_idx + 2]}; + + const scalar_t nt[3] = {target_normals_ptr[target_idx], + target_normals_ptr[target_idx + 1], + target_normals_ptr[target_idx + 2]}; + + // Compute geometric point-to-plane error. + const scalar_t p2p_error = (ps[0] - pt[0]) * nt[0] + + (ps[1] - pt[1]) * nt[1] + + (ps[2] - pt[2]) * nt[2]; + + // Compute geometric point-to-plane residual and Jacobian. + J_G[0] = sqrt_lambda_geometric * (-ps[2] * nt[1] + ps[1] * nt[2]); + J_G[1] = sqrt_lambda_geometric * (ps[2] * nt[0] - ps[0] * nt[2]); + J_G[2] = sqrt_lambda_geometric * (-ps[1] * nt[0] + ps[0] * nt[1]); + J_G[3] = sqrt_lambda_geometric * nt[0]; + J_G[4] = sqrt_lambda_geometric * nt[1]; + J_G[5] = sqrt_lambda_geometric * nt[2]; + r_G = sqrt_lambda_geometric * p2p_error; + + return true; +} + +template bool GetJacobianDopplerICP(const int64_t workload_idx, + const float *source_points_ptr, + const float *source_dopplers_ptr, + const float *source_directions_ptr, + const float *target_points_ptr, + const float *target_normals_ptr, + const int64_t *correspondence_indices, + const float *R_S_to_V, + const float *r_v_to_s_in_V, + const float *v_s_in_S, + const bool prune_correspondences, + const float doppler_outlier_threshold, + const float &sqrt_lambda_geometric, + const float &sqrt_lambda_doppler, + const float &sqrt_lambda_doppler_by_dt, + float *J_G, + float *J_D, + float &r_G, + float &r_D); + +template bool GetJacobianDopplerICP(const int64_t workload_idx, + const double *source_points_ptr, + const double *source_dopplers_ptr, + const double *source_directions_ptr, + const double *target_points_ptr, + const double *target_normals_ptr, + const int64_t *correspondence_indices, + const double *R_S_to_V, + const double *r_v_to_s_in_V, + const double *v_s_in_S, + const bool prune_correspondences, + const double doppler_outlier_threshold, + const double &sqrt_lambda_geometric, + const double &sqrt_lambda_doppler, + const double &sqrt_lambda_doppler_by_dt, + double *J_G, + double *J_D, + double &r_G, + double &r_D); + template OPEN3D_HOST_DEVICE inline bool GetInformationJacobians( int64_t workload_idx, diff --git a/cpp/open3d/t/pipelines/kernel/TransformationConverter.cpp b/cpp/open3d/t/pipelines/kernel/TransformationConverter.cpp index 6e7690fee1b..3d90e9f565c 100644 --- a/cpp/open3d/t/pipelines/kernel/TransformationConverter.cpp +++ b/cpp/open3d/t/pipelines/kernel/TransformationConverter.cpp @@ -111,6 +111,53 @@ core::Tensor PoseToTransformation(const core::Tensor &pose) { return transformation; } +template +static void TransformationToPoseDevice( + core::Tensor &pose, + const core::Tensor &transformation, + const core::Device::DeviceType &device_type) { + scalar_t *pose_ptr = pose.GetDataPtr(); + const scalar_t *transformation_ptr = transformation.GetDataPtr(); + + if (device_type == core::Device::DeviceType::CPU) { + TransformationToPoseImpl(pose_ptr, transformation_ptr); + } else if (device_type == core::Device::DeviceType::CUDA) { +#ifdef BUILD_CUDA_MODULE + TransformationToPoseCUDA(pose_ptr, transformation_ptr); +#else + utility::LogError("Not compiled with CUDA, but CUDA device is used."); +#endif + } else { + utility::LogError("Unimplemented device."); + } +} + +core::Tensor TransformationToPose(const core::Tensor &transformation) { + core::AssertTensorShape(transformation, {4, 4}); + core::AssertTensorDtypes(transformation, {core::Float32, core::Float64}); + + const core::Device device = transformation.GetDevice(); + const core::Dtype dtype = transformation.GetDtype(); + core::Tensor pose = core::Tensor::Zeros({6}, dtype, device); + pose = pose.Contiguous(); + core::Tensor transformation_ = transformation.Contiguous(); + + DISPATCH_FLOAT_DTYPE_TO_TEMPLATE(dtype, [&]() { + core::Device::DeviceType device_type = device.GetType(); + TransformationToPoseDevice(pose, transformation_, + device_type); + }); + + // Set translation parameters in pose vector. + pose.SetItem(core::TensorKey::Slice(3, 6, 1), + transformation_ + .GetItem({core::TensorKey::Slice(0, 3, 1), + core::TensorKey::Slice(3, 4, 1)}) + .Flatten()); + + return pose; +} + void DecodeAndSolve6x6(const core::Tensor &A_reduction, core::Tensor &delta, float &inlier_residual, diff --git a/cpp/open3d/t/pipelines/kernel/TransformationConverter.cu b/cpp/open3d/t/pipelines/kernel/TransformationConverter.cu index 1915a296f4b..b87205277e8 100644 --- a/cpp/open3d/t/pipelines/kernel/TransformationConverter.cu +++ b/cpp/open3d/t/pipelines/kernel/TransformationConverter.cu @@ -60,6 +60,32 @@ void PoseToTransformationCUDA(double *transformation_ptr, <<<1, 1, 0, core::cuda::GetStream()>>>(transformation_ptr, X_ptr); } +template +__global__ void TransformationToPoseKernel(scalar_t *X_ptr, + const scalar_t *transformation_ptr) { + TransformationToPoseImpl(X_ptr, transformation_ptr); +} + +template +void TransformationToPoseCUDA(scalar_t *X_ptr, + const scalar_t *transformation_ptr) { + utility::LogError("Unsupported data type."); +} + +template <> +void TransformationToPoseCUDA(float *X_ptr, + const float *transformation_ptr) { + TransformationToPoseKernel + <<<1, 1, 0, core::cuda::GetStream()>>>(X_ptr, transformation_ptr); +} + +template <> +void TransformationToPoseCUDA(double *X_ptr, + const double *transformation_ptr) { + TransformationToPoseKernel + <<<1, 1, 0, core::cuda::GetStream()>>>(X_ptr, transformation_ptr); +} + } // namespace kernel } // namespace pipelines } // namespace t diff --git a/cpp/open3d/t/pipelines/kernel/TransformationConverter.h b/cpp/open3d/t/pipelines/kernel/TransformationConverter.h index 6a885ca48ff..4f041ecc492 100644 --- a/cpp/open3d/t/pipelines/kernel/TransformationConverter.h +++ b/cpp/open3d/t/pipelines/kernel/TransformationConverter.h @@ -49,6 +49,14 @@ core::Tensor RtToTransformation(const core::Tensor &R, const core::Tensor &t); /// as pose. core::Tensor PoseToTransformation(const core::Tensor &pose); +/// \brief Convert transformation matrix to pose. +/// +/// \param transformation, a tensor of shape {4, 4}, of dtype Float32 +/// \return pose [alpha beta gamma, tx, ty, tz], a shape {6} tensor of dtype and +/// device same as transformation, where alpha, beta, gamma are the Euler angles +/// in the ZYX order. +core::Tensor TransformationToPose(const core::Tensor &transformation); + /// \brief Decodes a 6x6 linear system from a compressed 29x1 tensor. /// \param A_reduction 1x29 tensor storing a linear system, /// (21 for \f$J^T J\f$ matrix, 6 for \f$J^T r\f$, 1 for residual, diff --git a/cpp/open3d/t/pipelines/kernel/TransformationConverterImpl.h b/cpp/open3d/t/pipelines/kernel/TransformationConverterImpl.h index 1babb762747..7d213f7fda2 100644 --- a/cpp/open3d/t/pipelines/kernel/TransformationConverterImpl.h +++ b/cpp/open3d/t/pipelines/kernel/TransformationConverterImpl.h @@ -60,6 +60,24 @@ OPEN3D_HOST_DEVICE inline void PoseToTransformationImpl( transformation_ptr[10] = cos(pose_ptr[1]) * cos(pose_ptr[0]); } +/// Shared implementation for TransformationToPose function. +/// Reference method: utility::TransformMatrix4dToVector6d. +template +OPEN3D_HOST_DEVICE inline void TransformationToPoseImpl( + scalar_t *pose_ptr, const scalar_t *transformation_ptr) { + const scalar_t sy = sqrt(transformation_ptr[0] * transformation_ptr[0] + + transformation_ptr[4] * transformation_ptr[4]); + if (!(sy < 1e-6)) { + pose_ptr[0] = atan2(transformation_ptr[9], transformation_ptr[10]); + pose_ptr[1] = atan2(-transformation_ptr[8], sy); + pose_ptr[2] = atan2(transformation_ptr[4], transformation_ptr[0]); + } else { + pose_ptr[0] = atan2(-transformation_ptr[6], transformation_ptr[5]); + pose_ptr[1] = atan2(-transformation_ptr[8], sy); + pose_ptr[2] = 0; + } +} + #ifdef BUILD_CUDA_MODULE /// \brief Helper function for PoseToTransformationCUDA. /// Do not call this independently, as it only sets the transformation part @@ -68,6 +86,14 @@ OPEN3D_HOST_DEVICE inline void PoseToTransformationImpl( template void PoseToTransformationCUDA(scalar_t *transformation_ptr, const scalar_t *pose_ptr); + +/// \brief Helper function for TransformationToPoseCUDA. +/// Do not call this independently, as it only sets the rotation part in the +/// pose, using the Transformation, the rest is set in the parent function +/// TransformationToPose. +template +void TransformationToPoseCUDA(scalar_t *pose_ptr, + const scalar_t *transformation_ptr); #endif } // namespace kernel diff --git a/cpp/open3d/t/pipelines/registration/Registration.cpp b/cpp/open3d/t/pipelines/registration/Registration.cpp index 06f302f3c9f..61050b0d614 100644 --- a/cpp/open3d/t/pipelines/registration/Registration.cpp +++ b/cpp/open3d/t/pipelines/registration/Registration.cpp @@ -40,6 +40,13 @@ namespace t { namespace pipelines { namespace registration { +namespace { + +// Minimum time period (sec) between two sequential scans for Doppler ICP. +constexpr double kMinTimePeriod = 1e-3; + +} // namespace + static RegistrationResult ComputeRegistrationResult( const geometry::PointCloud &source, const core::nns::NearestNeighborSearch &target_nns, @@ -408,6 +415,165 @@ RegistrationResult MultiScaleICP( } // ---- Iterating over different resolution scale END -------------------- + result.num_iterations_ = iteration_count; + + return result; +} + +RegistrationResult DopplerICP( + const geometry::PointCloud &source, + const geometry::PointCloud &target, + const double max_correspondence_distance, + const core::Tensor &init_source_to_target, + const TransformationEstimationForDopplerICP &estimation, + const ICPConvergenceCriteria &criteria, + const double period, + const core::Tensor &T_V_to_S, + const std::function< + void(const std::unordered_map &)> + &callback_after_iteration) { + core::AssertTensorShape(init_source_to_target, {4, 4}); + core::AssertTensorShape(T_V_to_S, {4, 4}); + + geometry::PointCloud source_copy = source; + core::AssertTensorDtypes(source_copy.GetPointPositions(), + {core::Float64, core::Float32}); + + const core::Device device = source_copy.GetDevice(); + const core::Dtype dtype = source_copy.GetPointPositions().GetDtype(); + + if (!target.HasPointPositions() || !source_copy.HasPointPositions()) { + utility::LogError("Source and/or Target pointcloud is empty."); + } + core::AssertTensorDtype(target.GetPointPositions(), dtype); + core::AssertTensorDevice(target.GetPointPositions(), device); + + if (dtype == core::Float64 && + device.GetType() == core::Device::DeviceType::CUDA) { + utility::LogDebug( + "Use Float32 pointcloud for best performance on CUDA device."); + } + + if (estimation.GetTransformationEstimationType() != + TransformationEstimationType::DopplerICP) { + utility::LogError( + "DopplerICP requires TransformationEstimationDopplerICP."); + } + + if (!target.HasPointNormals()) { + utility::LogError( + "TransformationEstimationDopplerICP require pre-computed " + "normal vectors for target PointCloud."); + } + if (!source_copy.HasPointAttr("dopplers")) { + utility::LogError("Source pointcloud missing dopplers attribute."); + } + if (!source_copy.HasPointAttr("directions")) { + utility::LogError("Source pointcloud missing directions attribute."); + } + + if (max_correspondence_distance <= 0.0) { + utility::LogError( + " Max correspondence distance must be greater than 0, but" + " got {} in scale: {}.", + max_correspondence_distance, 0); + } + + if (std::abs(period) < kMinTimePeriod) { + utility::LogError("Time period too small."); + } + + // Transformation tensor is always of shape {4,4}, type Float64 on CPU:0. + core::Tensor transformation = + init_source_to_target.To(core::Device("CPU:0"), core::Float64); + RegistrationResult result(transformation); + + // Apply the initial transform on source pointcloud. + source_copy.Transform(transformation); + + // Initialize Neighbor Search. + core::nns::NearestNeighborSearch target_nns(target.GetPointPositions()); + bool check = target_nns.HybridIndex(max_correspondence_distance); + if (!check) { + utility::LogError("Index is not set."); + } + + int iteration_count = 0; + for (iteration_count = 0; iteration_count < criteria.max_iteration_; + ++iteration_count) { + double prev_fitness = result.fitness_; + double prev_inlier_rmse = result.inlier_rmse_; + + // Update the results and find correspondences. + result = ComputeRegistrationResult( + source_copy.GetPointPositions(), target_nns, + max_correspondence_distance, result.transformation_); + + // No correspondences. + if (result.fitness_ <= std::numeric_limits::min()) { + result.converged_ = false; + result.num_iterations_ = iteration_count; + return result; + } + + // Computing Transform between source and target, given + // correspondences. ComputeTransformation returns {4,4} shaped + // Float64 transformation tensor on CPU device. + core::Tensor update = + estimation + .ComputeTransformation(source_copy, target, + result.correspondences_, + result.transformation_, T_V_to_S, + period, iteration_count) + .To(core::Float64); + + // Multiply the transform to the cumulative transformation (update). + result.transformation_ = update.Matmul(result.transformation_); + + // Apply the transform on source pointcloud. + source_copy.Transform(update); + + utility::LogDebug("ICP Iteration #{:d}: Fitness {:.4f}, RMSE {:.4f}", + iteration_count, result.fitness_, + result.inlier_rmse_); + + // Additional per-iteration results can recorded in this callback. + if (callback_after_iteration) { + const core::Device host("CPU:0"); + + std::unordered_map loss_attribute_map{ + {"iteration_index", + core::Tensor::Init(iteration_count)}, + {"inlier_rmse", + core::Tensor::Init(result.inlier_rmse_)}, + {"fitness", core::Tensor::Init(result.fitness_)}, + {"transformation", result.transformation_.To(host)}}; + callback_after_iteration(loss_attribute_map); + } + + // ICPConvergenceCriteria, to terminate iteration. + if (iteration_count != 0 && + std::abs(prev_fitness - result.fitness_) < + criteria.relative_fitness_ && + std::abs(prev_inlier_rmse - result.inlier_rmse_) < + criteria.relative_rmse_) { + // Calculate final `fitness` and `inlier_rmse` for the current + // `transformation` stored in `result`. + result = ComputeRegistrationResult(source_copy, target_nns, + max_correspondence_distance, + result.transformation_); + result.converged_ = true; + break; + } + } + + result.num_iterations_ = iteration_count; + + // No correspondences. + if (result.fitness_ <= std::numeric_limits::epsilon()) { + result.converged_ = false; + } + return result; } diff --git a/cpp/open3d/t/pipelines/registration/Registration.h b/cpp/open3d/t/pipelines/registration/Registration.h index 0bcadda85ac..0a1b3b55973 100644 --- a/cpp/open3d/t/pipelines/registration/Registration.h +++ b/cpp/open3d/t/pipelines/registration/Registration.h @@ -110,6 +110,10 @@ class RegistrationResult { /// For ICP: the overlapping area (# of inlier correspondences / # of points /// in target). Higher is better. double fitness_; + /// Specifies whether the algorithm converged or not. + bool converged_{false}; + /// Number of iterations the algorithm took to converge. + size_t num_iterations_{0}; }; /// \brief Function for evaluating registration between point clouds. @@ -197,7 +201,42 @@ RegistrationResult MultiScaleICP( void(const std::unordered_map &)> &callback_after_iteration = nullptr); -/// \brief Computes `Information Matrix`, from the transformation between source +/// \brief Functions for DopplerICP registration. +/// +/// \param source The source point cloud. (Float32 or Float64 type). +/// \param target The target point cloud. (Float32 or Float64 type). +/// \param max_correspondence_distance Maximum correspondence points-pair +/// distance. +/// \param init_source_to_target Initial transformation estimation of type +/// Float64 on CPU. +/// \param estimation Estimation method. +/// \param criteria Convergence criteria. +/// \param period Time period (in seconds) between the source and the target +/// point clouds. Default value: 0.1. +/// \param T_V_to_S The 4x4 transformation matrix to transform sensor to vehicle +/// frame. Defaults to identity transform. +/// \param callback_after_iteration Optional lambda function, saves string to +/// tensor map of attributes such as "iteration_index", "scale_index", +/// "scale_iteration_index", "inlier_rmse", "fitness", "transformation", on CPU +/// device, updated after each iteration. +RegistrationResult DopplerICP( + const geometry::PointCloud &source, + const geometry::PointCloud &target, + const double max_correspondence_distance, + const core::Tensor &init_source_to_target = + core::Tensor::Eye(4, core::Float64, core::Device("CPU:0")), + const TransformationEstimationForDopplerICP &estimation = + TransformationEstimationForDopplerICP(), + const ICPConvergenceCriteria &criteria = ICPConvergenceCriteria(), + const double period = 0.1F, + const core::Tensor &T_V_to_S = core::Tensor::Eye(4, + core::Float64, + core::Device("CPU:0")), + const std::function< + void(const std::unordered_map &)> + &callback_after_iteration = nullptr); + +/// \brief Computes `Information Matrix`, from the transfromation between source /// and target pointcloud. It returns the `Information Matrix` of shape {6, 6}, /// of dtype `Float64` on device `CPU:0`. /// diff --git a/cpp/open3d/t/pipelines/registration/RobustKernelImpl.h b/cpp/open3d/t/pipelines/registration/RobustKernelImpl.h index 50469776d07..4f99513c83f 100644 --- a/cpp/open3d/t/pipelines/registration/RobustKernelImpl.h +++ b/cpp/open3d/t/pipelines/registration/RobustKernelImpl.h @@ -132,3 +132,67 @@ using open3d::t::pipelines::registration::RobustKernelMethod; utility::LogError("Unsupported method."); \ } \ }() + +/// \param scalar_t type: float / double. +/// \param METHOD_1 registration::RobustKernelMethod Loss type. +/// \param scaling_parameter_1 Scaling parameter for loss fine-tuning. +/// \param METHOD_2 registration::RobustKernelMethod Loss type. +/// \param scaling_parameter_2 Scaling parameter for loss fine-tuning. +#define DISPATCH_DUAL_ROBUST_KERNEL_FUNCTION(scalar_t, METHOD_1, \ + scaling_parameter_1, METHOD_2, \ + scaling_parameter_2, ...) \ + [&] { \ + scalar_t scale_1 = static_cast(scaling_parameter_1); \ + scalar_t scale_2 = static_cast(scaling_parameter_2); \ + if (METHOD_1 == RobustKernelMethod::L2Loss && \ + METHOD_2 == RobustKernelMethod::L2Loss) { \ + auto GetWeightFromRobustKernelFirst = \ + [=] OPEN3D_HOST_DEVICE(scalar_t residual) -> scalar_t { \ + return 1.0; \ + }; \ + auto GetWeightFromRobustKernelSecond = \ + [=] OPEN3D_HOST_DEVICE(scalar_t residual) -> scalar_t { \ + return 1.0; \ + }; \ + return __VA_ARGS__(); \ + } else if (METHOD_1 == RobustKernelMethod::L2Loss && \ + METHOD_2 == RobustKernelMethod::TukeyLoss) { \ + auto GetWeightFromRobustKernelFirst = \ + [=] OPEN3D_HOST_DEVICE(scalar_t residual) -> scalar_t { \ + return 1.0; \ + }; \ + auto GetWeightFromRobustKernelSecond = \ + [=] OPEN3D_HOST_DEVICE(scalar_t residual) -> scalar_t { \ + return Square(1.0 - Square(min((scalar_t)1.0, \ + abs(residual) / scale_2))); \ + }; \ + return __VA_ARGS__(); \ + } else if (METHOD_1 == RobustKernelMethod::TukeyLoss && \ + METHOD_2 == RobustKernelMethod::L2Loss) { \ + auto GetWeightFromRobustKernelFirst = \ + [=] OPEN3D_HOST_DEVICE(scalar_t residual) -> scalar_t { \ + return Square(1.0 - Square(min((scalar_t)1.0, \ + abs(residual) / scale_1))); \ + }; \ + auto GetWeightFromRobustKernelSecond = \ + [=] OPEN3D_HOST_DEVICE(scalar_t residual) -> scalar_t { \ + return 1.0; \ + }; \ + return __VA_ARGS__(); \ + } else if (METHOD_1 == RobustKernelMethod::TukeyLoss && \ + METHOD_2 == RobustKernelMethod::TukeyLoss) { \ + auto GetWeightFromRobustKernelFirst = \ + [=] OPEN3D_HOST_DEVICE(scalar_t residual) -> scalar_t { \ + return Square(1.0 - Square(min((scalar_t)1.0, \ + abs(residual) / scale_1))); \ + }; \ + auto GetWeightFromRobustKernelSecond = \ + [=] OPEN3D_HOST_DEVICE(scalar_t residual) -> scalar_t { \ + return Square(1.0 - Square(min((scalar_t)1.0, \ + abs(residual) / scale_2))); \ + }; \ + return __VA_ARGS__(); \ + } else { \ + utility::LogError("Unsupported method."); \ + } \ + }() diff --git a/cpp/open3d/t/pipelines/registration/TransformationEstimation.cpp b/cpp/open3d/t/pipelines/registration/TransformationEstimation.cpp index 4be3abd2b5f..b2efee922ca 100644 --- a/cpp/open3d/t/pipelines/registration/TransformationEstimation.cpp +++ b/cpp/open3d/t/pipelines/registration/TransformationEstimation.cpp @@ -317,6 +317,105 @@ core::Tensor TransformationEstimationForColoredICP::ComputeTransformation( return transform; } +double TransformationEstimationForDopplerICP::ComputeRMSE( + const geometry::PointCloud &source, + const geometry::PointCloud &target, + const core::Tensor &correspondences) const { + if (!target.HasPointPositions() || !source.HasPointPositions()) { + utility::LogError("Source and/or Target pointcloud is empty."); + } + if (!target.HasPointNormals()) { + utility::LogError("Target pointcloud missing normals attribute."); + } + + core::AssertTensorDtype(target.GetPointPositions(), + source.GetPointPositions().GetDtype()); + core::AssertTensorDevice(target.GetPointPositions(), source.GetDevice()); + + AssertValidCorrespondences(correspondences, source.GetPointPositions()); + + core::Tensor valid = correspondences.Ne(-1).Reshape({-1}); + core::Tensor neighbour_indices = + correspondences.IndexGet({valid}).Reshape({-1}); + core::Tensor source_points_indexed = + source.GetPointPositions().IndexGet({valid}); + core::Tensor target_points_indexed = + target.GetPointPositions().IndexGet({neighbour_indices}); + core::Tensor target_normals_indexed = + target.GetPointNormals().IndexGet({neighbour_indices}); + + core::Tensor error_t = (source_points_indexed - target_points_indexed) + .Mul_(target_normals_indexed); + error_t.Mul_(error_t); + double error = error_t.Sum({0, 1}).To(core::Float64).Item(); + return std::sqrt(error / + static_cast(neighbour_indices.GetLength())); +} + +core::Tensor TransformationEstimationForDopplerICP::ComputeTransformation( + const geometry::PointCloud &source, + const geometry::PointCloud &target, + const core::Tensor &correspondences) const { + utility::LogError( + "This method should not be called for DopplerICP. DopplerICP " + "requires the period, current transformation, and T_V_to_S " + "calibration."); + return core::Tensor::Eye(4, core::Float64, core::Device("CPU:0")); +} + +core::Tensor TransformationEstimationForDopplerICP::ComputeTransformation( + const geometry::PointCloud &source, + const geometry::PointCloud &target, + const core::Tensor &correspondences, + const core::Tensor ¤t_transform, + const core::Tensor &T_V_to_S, + const double period, + const size_t iteration) const { + if (!source.HasPointPositions() || !target.HasPointPositions()) { + utility::LogError("Source and/or Target pointcloud is empty."); + } + if (!target.HasPointNormals()) { + utility::LogError("Target pointcloud missing normals attribute."); + } + if (!source.HasPointAttr("dopplers")) { + utility::LogError("Source pointcloud missing dopplers attribute."); + } + if (!source.HasPointAttr("directions")) { + utility::LogError("Source pointcloud missing directions attribute."); + } + + core::AssertTensorDtypes(source.GetPointPositions(), + {core::Float64, core::Float32}); + const core::Dtype dtype = source.GetPointPositions().GetDtype(); + + core::AssertTensorDtype(target.GetPointPositions(), dtype); + core::AssertTensorDtype(target.GetPointNormals(), dtype); + core::AssertTensorDtype(source.GetPointAttr("dopplers"), dtype); + core::AssertTensorDtype(source.GetPointAttr("directions"), dtype); + + core::AssertTensorDevice(target.GetPointPositions(), source.GetDevice()); + + AssertValidCorrespondences(correspondences, source.GetPointPositions()); + + // Get pose {6} of type Float64 from correspondences indexed source + // and target point cloud. + core::Tensor pose = pipelines::kernel::ComputePoseDopplerICP( + source.GetPointPositions(), source.GetPointAttr("dopplers"), + source.GetPointAttr("directions"), target.GetPointPositions(), + target.GetPointNormals(), correspondences, current_transform, + T_V_to_S, period, iteration, this->reject_dynamic_outliers_, + this->doppler_outlier_threshold_, + this->outlier_rejection_min_iteration_, + this->geometric_robust_loss_min_iteration_, + this->doppler_robust_loss_min_iteration_, this->geometric_kernel_, + this->doppler_kernel_, this->lambda_doppler_); + + // Get transformation {4,4} of type Float64 from pose {6}. + core::Tensor transform = pipelines::kernel::PoseToTransformation(pose); + + return transform; +} + } // namespace registration } // namespace pipelines } // namespace t diff --git a/cpp/open3d/t/pipelines/registration/TransformationEstimation.h b/cpp/open3d/t/pipelines/registration/TransformationEstimation.h index eb1160823a0..e3c08f01d66 100644 --- a/cpp/open3d/t/pipelines/registration/TransformationEstimation.h +++ b/cpp/open3d/t/pipelines/registration/TransformationEstimation.h @@ -51,6 +51,7 @@ enum class TransformationEstimationType { PointToPoint = 1, PointToPlane = 2, ColoredICP = 3, + DopplerICP = 4, }; /// \class TransformationEstimation @@ -286,6 +287,139 @@ class TransformationEstimationForColoredICP : public TransformationEstimation { TransformationEstimationType::ColoredICP; }; +/// \class TransformationEstimationForDopplerICP +/// +/// This is implementation of Doppler ICP. +/// Reference: https://arxiv.org/abs/2201.11944 +/// +/// Class to estimate a transformation matrix tensor of shape {4, 4}, dtype +/// Float64, on CPU device for DopplerICP method. +class TransformationEstimationForDopplerICP : public TransformationEstimation { +public: + ~TransformationEstimationForDopplerICP() override{}; + + /// \brief Constructor. + /// + /// \param lambda_doppler `λ ∈ [0, 1]` in the overall energy `(1−λ)EG + + /// λED`. Refer the documentation of DopplerICP for more information. + /// \param reject_dynamic_outliers Whether or not to reject dynamic point + /// outlier correspondences. + /// \param doppler_outlier_threshold Correspondences with Doppler error + /// greater than this threshold are rejected from optimization. + /// \param outlier_rejection_min_iteration Number of iterations of ICP after + /// which outlier rejection is enabled. + /// \param geometric_robust_loss_min_iteration Number of iterations of ICP + /// after which robust loss for geometric term kicks in. + /// \param doppler_robust_loss_min_iteration Number of iterations of ICP + /// after which robust loss for Doppler term kicks in. + /// \param geometric_kernel (optional) Any of the implemented statistical + /// robust kernel for outlier rejection for the geometric term. + /// \param doppler_kernel (optional) Any of the implemented statistical + /// robust kernel for outlier rejection for the Doppler term. + explicit TransformationEstimationForDopplerICP( + double lambda_doppler = 0.01, + bool reject_dynamic_outliers = false, + double doppler_outlier_threshold = 2.0, + size_t outlier_rejection_min_iteration = 2, + size_t geometric_robust_loss_min_iteration = 0, + size_t doppler_robust_loss_min_iteration = 2, + const RobustKernel &geometric_kernel = + RobustKernel(RobustKernelMethod::L2Loss, 1.0, 1.0), + const RobustKernel &doppler_kernel = + RobustKernel(RobustKernelMethod::L2Loss, 1.0, 1.0)) + : lambda_doppler_(lambda_doppler), + reject_dynamic_outliers_(reject_dynamic_outliers), + doppler_outlier_threshold_(doppler_outlier_threshold), + outlier_rejection_min_iteration_(outlier_rejection_min_iteration), + geometric_robust_loss_min_iteration_( + geometric_robust_loss_min_iteration), + doppler_robust_loss_min_iteration_(doppler_robust_loss_min_iteration), + geometric_kernel_(geometric_kernel), + doppler_kernel_(doppler_kernel) { + if (lambda_doppler_ < 0 || lambda_doppler_ > 1.0) { + lambda_doppler_ = 0.01; + } + } + + TransformationEstimationType GetTransformationEstimationType() + const override { + return type_; + }; + +public: + /// \brief Computes RMSE (double) for DopplerICP method, between two + /// pointclouds, given correspondences. + /// + /// \param source Source pointcloud. (Float32 or Float64 type). + /// \param target Target pointcloud. (Float32 or Float64 type). It must + /// contain normals, directions, and Doppler of the same shape and dtype + /// as the positions. + /// \param correspondences Tensor of type Int64 containing indices of + /// corresponding target points, where the value is the target index and the + /// index of the value itself is the source index. It contains -1 as value + /// at index with no correspondence. + double ComputeRMSE(const geometry::PointCloud &source, + const geometry::PointCloud &target, + const core::Tensor &correspondences) const override; + + /// \brief This method is only present to override the method in the base + /// class and must not be used. Use the other method below. + core::Tensor ComputeTransformation( + const geometry::PointCloud &source, + const geometry::PointCloud &target, + const core::Tensor &correspondences) const override; + + /// \brief Estimates the transformation matrix for DopplerICP method, + /// a tensor of shape {4, 4}, and dtype Float64 on CPU device. + /// + /// \param source Source pointcloud. (Float32 or Float64 type). + /// \param target Target pointcloud. (Float32 or Float64 type). It must + /// contain normals, directions, and Doppler of the same shape and dtype + /// as the positions. + /// \param correspondences Tensor of type Int64 containing indices of + /// corresponding target points, where the value is the target index and the + /// index of the value itself is the source index. It contains -1 as value + /// at index with no correspondence. + /// \param current_transform The current pose estimate of ICP. + /// \param T_V_to_S The 4x4 transformation matrix to transform + /// sensor to vehicle frame. + /// \param period Time period (in seconds) between the source and the target + /// point clouds. + /// \param iteration current iteration number of the ICP algorithm. + /// \return transformation between source to target, a tensor of shape {4, + /// 4}, type Float64 on CPU device. + core::Tensor ComputeTransformation(const geometry::PointCloud &source, + const geometry::PointCloud &target, + const core::Tensor &correspondences, + const core::Tensor ¤t_transform, + const core::Tensor &T_V_to_S, + const double period, + const size_t iteration) const; + +public: + /// Factor that weighs the Doppler residual term in DICP objective. + double lambda_doppler_{0.01}; + /// Whether or not to prune dynamic point outlier correspondences. + bool reject_dynamic_outliers_{false}; + /// Correspondences with Doppler error greater than this threshold are + /// rejected from optimization. + double doppler_outlier_threshold_{2.0}; + /// Number of iterations of ICP after which outlier rejection is enabled. + size_t outlier_rejection_min_iteration_{2}; + /// Number of iterations of ICP after which robust loss kicks in. + size_t geometric_robust_loss_min_iteration_{0}; + size_t doppler_robust_loss_min_iteration_{2}; + /// RobustKernel for outlier rejection. + RobustKernel geometric_kernel_ = + RobustKernel(RobustKernelMethod::L2Loss, 1.0, 1.0); + RobustKernel doppler_kernel_ = + RobustKernel(RobustKernelMethod::L2Loss, 1.0, 1.0); + +private: + const TransformationEstimationType type_ = + TransformationEstimationType::DopplerICP; +}; + } // namespace registration } // namespace pipelines } // namespace t diff --git a/cpp/pybind/t/pipelines/registration/registration.cpp b/cpp/pybind/t/pipelines/registration/registration.cpp index c229e0952eb..c6a607cf659 100644 --- a/cpp/pybind/t/pipelines/registration/registration.cpp +++ b/cpp/pybind/t/pipelines/registration/registration.cpp @@ -107,7 +107,7 @@ void pybind_registration_classes(py::module &m) { &RegistrationResult::transformation_, "``4 x 4`` float64 tensor on CPU: The estimated " "transformation matrix.") - .def_readwrite("correspondences_", + .def_readwrite("correspondence_set", &RegistrationResult::correspondences_, "Tensor of type Int64 containing indices of " "corresponding target points, where the value is " @@ -121,13 +121,23 @@ void pybind_registration_classes(py::module &m) { "float: The overlapping area (# of inlier " "correspondences " "/ # of points in source). Higher is better.") + .def_readwrite( + "converged", &RegistrationResult::converged_, + "bool: Specifies whether the algorithm converged or not.") + .def_readwrite( + "num_iterations", &RegistrationResult::num_iterations_, + "int: Number of iterations the algorithm took to converge.") .def("__repr__", [](const RegistrationResult &rr) { return fmt::format( - "RegistrationResult[fitness_={:e}, " - "inlier_rmse={:e}, correspondences={:d}]." + "RegistrationResult[" + "converged={}" + ", num_iteration={:d}" + ", fitness_={:e}" + ", inlier_rmse={:e}" + ", correspondences={:d}]." "\nAccess transformation to get result.", - rr.fitness_, rr.inlier_rmse_, - rr.fitness_ * rr.correspondences_.GetLength()); + rr.converged_, rr.num_iterations_, rr.fitness_, + rr.inlier_rmse_, rr.correspondences_.GetLength()); }); // open3d.t.pipelines.registration.TransformationEstimation @@ -251,6 +261,100 @@ void pybind_registration_classes(py::module &m) { .def_readwrite("kernel", &TransformationEstimationForColoredICP::kernel_, "Robust Kernel used in the Optimization"); + + // open3d.t.pipelines.registration.TransformationEstimationForDopplerICP + // TransformationEstimation + py::class_< + TransformationEstimationForDopplerICP, + PyTransformationEstimation, + TransformationEstimation> + te_dop(m, "TransformationEstimationForDopplerICP", + "Class to estimate a transformation between two point " + "clouds using color information"); + py::detail::bind_default_constructor( + te_dop); + py::detail::bind_copy_functions( + te_dop); + te_dop.def(py::init([](double lambda_doppler, bool reject_dynamic_outliers, + double doppler_outlier_threshold, + size_t outlier_rejection_min_iteration, + size_t geometric_robust_loss_min_iteration, + size_t doppler_robust_loss_min_iteration, + RobustKernel &geometric_kernel, + RobustKernel &doppler_kernel) { + return new TransformationEstimationForDopplerICP( + lambda_doppler, reject_dynamic_outliers, + doppler_outlier_threshold, + outlier_rejection_min_iteration, + geometric_robust_loss_min_iteration, + doppler_robust_loss_min_iteration, geometric_kernel, + doppler_kernel); + }), + "lambda_doppler"_a, "reject_dynamic_outliers"_a, + "doppler_outlier_threshold"_a, + "outlier_rejection_min_iteration"_a, + "geometric_robust_loss_min_iteration"_a, + "doppler_robust_loss_min_iteration"_a, "goemetric_kernel"_a, + "doppler_kernel"_a) + .def(py::init([](const double lambda_doppler) { + return new TransformationEstimationForDopplerICP( + lambda_doppler); + }), + "lambda_doppler"_a) + .def("compute_transformation", + py::overload_cast( + &TransformationEstimationForDopplerICP:: + ComputeTransformation, + py::const_), + "Compute transformation from source to target point cloud " + "given correspondences") + .def("__repr__", + [](const TransformationEstimationForDopplerICP &te) { + return std::string( + "TransformationEstimationForDopplerICP " + "with lambda_doppler: ") + + std::to_string(te.lambda_doppler_); + }) + .def_readwrite( + "lambda_doppler", + &TransformationEstimationForDopplerICP::lambda_doppler_, + "lambda_doppler") + .def_readwrite("reject_dynamic_outliers", + &TransformationEstimationForDopplerICP:: + reject_dynamic_outliers_, + "Performs dynamic point outlier rejection of " + "correspondences") + .def_readwrite("doppler_outlier_threshold", + &TransformationEstimationForDopplerICP:: + doppler_outlier_threshold_, + "doppler_outlier_threshold") + .def_readwrite("outlier_rejection_min_iteration", + &TransformationEstimationForDopplerICP:: + outlier_rejection_min_iteration_, + "Number of iterations of ICP after which outlier " + "rejection is enabled.") + .def_readwrite("geometric_robust_loss_min_iteration", + &TransformationEstimationForDopplerICP:: + geometric_robust_loss_min_iteration_, + "Minimum iterations after which Robust Kernel is " + "used for the Geometric error") + .def_readwrite("doppler_robust_loss_min_iteration", + &TransformationEstimationForDopplerICP:: + doppler_robust_loss_min_iteration_, + "Minimum iterations after which Robust Kernel is " + "used for the Doppler error") + .def_readwrite( + "geometric_kernel", + &TransformationEstimationForDopplerICP::geometric_kernel_, + "Robust Kernel used in the Geometric Error Optimization") + .def_readwrite( + "doppler_kernel", + &TransformationEstimationForDopplerICP::doppler_kernel_, + "Robust Kernel used in the Doppler Error Optimization"); } // Registration functions have similar arguments, sharing arg docstrings. @@ -278,11 +382,17 @@ static const std::unordered_map "o3d.utility.DoubleVector of maximum correspondence " "points-pair distances for multi-scale icp."}, {"option", "Registration option"}, + {"period", + "Time period (in seconds) between the source and the target " + "point clouds."}, {"source", "The source point cloud."}, {"target", "The target point cloud."}, {"transformation", "The 4x4 transformation matrix of type Float64 " "to transform ``source`` to ``target``"}, + {"T_V_to_S", + "The 4x4 transformation matrix to transform ``sensor`` to " + "``vehicle`` frame."}, {"voxel_size", "The input pointclouds will be down-sampled to this " "`voxel_size` scale. If `voxel_size` < 0, original scale will " @@ -328,6 +438,19 @@ void pybind_registration_methods(py::module &m) { docstring::FunctionDocInject(m, "multi_scale_icp", map_shared_argument_docstrings); + m.def("doppler_icp", &DopplerICP, py::call_guard(), + "Function for Doppler ICP registration", "source"_a, "target"_a, + "max_correspondence_distance"_a, + "init_source_to_target"_a = + core::Tensor::Eye(4, core::Float64, core::Device("CPU:0")), + "estimation_method"_a = TransformationEstimationForDopplerICP(), + "criteria"_a = ICPConvergenceCriteria(), "period"_a = 0.1F, + "T_V_to_S"_a = + core::Tensor::Eye(4, core::Float64, core::Device("CPU:0")), + "callback_after_iteration"_a = py::none()); + docstring::FunctionDocInject(m, "doppler_icp", + map_shared_argument_docstrings); + m.def("get_information_matrix", &GetInformationMatrix, py::call_guard(), "Function for computing information matrix from transformation " From fe04ca9a0a6818c578beac2f8352a47a7e1db6fa Mon Sep 17 00:00:00 2001 From: Heethesh Vhavle Date: Tue, 21 Jun 2022 17:25:32 -0700 Subject: [PATCH 02/14] style check and cite paper --- cpp/open3d/core/linalg/kernel/Matrix.h | 4 ++-- cpp/open3d/t/io/file_format/FileXYZD.cpp | 3 ++- cpp/open3d/t/pipelines/kernel/RegistrationImpl.h | 1 - .../t/pipelines/registration/TransformationEstimation.h | 5 +++-- 4 files changed, 7 insertions(+), 6 deletions(-) diff --git a/cpp/open3d/core/linalg/kernel/Matrix.h b/cpp/open3d/core/linalg/kernel/Matrix.h index 1db6df60af0..fd027afe462 100644 --- a/cpp/open3d/core/linalg/kernel/Matrix.h +++ b/cpp/open3d/core/linalg/kernel/Matrix.h @@ -56,8 +56,8 @@ static OPEN3D_DEVICE OPEN3D_FORCE_INLINE void matmul3x3_3x1(const scalar_t& m00, template OPEN3D_HOST_DEVICE OPEN3D_FORCE_INLINE void matmul3x3_3x1(const scalar_t* A_3x3, - const scalar_t* B_3x1, - scalar_t* C_3x1) { + const scalar_t* B_3x1, + scalar_t* C_3x1) { C_3x1[0] = A_3x3[0] * B_3x1[0] + A_3x3[1] * B_3x1[1] + A_3x3[2] * B_3x1[2]; C_3x1[1] = A_3x3[3] * B_3x1[0] + A_3x3[4] * B_3x1[1] + A_3x3[5] * B_3x1[2]; C_3x1[2] = A_3x3[6] * B_3x1[0] + A_3x3[7] * B_3x1[1] + A_3x3[8] * B_3x1[2]; diff --git a/cpp/open3d/t/io/file_format/FileXYZD.cpp b/cpp/open3d/t/io/file_format/FileXYZD.cpp index c0de00c5377..c9ddc197e84 100644 --- a/cpp/open3d/t/io/file_format/FileXYZD.cpp +++ b/cpp/open3d/t/io/file_format/FileXYZD.cpp @@ -69,7 +69,8 @@ bool ReadPointCloudFromXYZD(const std::string &filename, double x, y, z, doppler; const char *line_buffer; while ((line_buffer = file.ReadLine())) { - if (sscanf(line_buffer, "%lf %lf %lf %lf", &x, &y, &z, &doppler) == 4) { + if (sscanf(line_buffer, "%lf %lf %lf %lf", &x, &y, &z, &doppler) == + 4) { points_ptr[3 * i + 0] = x; points_ptr[3 * i + 1] = y; points_ptr[3 * i + 2] = z; diff --git a/cpp/open3d/t/pipelines/kernel/RegistrationImpl.h b/cpp/open3d/t/pipelines/kernel/RegistrationImpl.h index 32f3c0b73d7..18c460d4c0c 100644 --- a/cpp/open3d/t/pipelines/kernel/RegistrationImpl.h +++ b/cpp/open3d/t/pipelines/kernel/RegistrationImpl.h @@ -391,7 +391,6 @@ OPEN3D_HOST_DEVICE inline bool GetJacobianDopplerICP( scalar_t *J_D, scalar_t &r_G, scalar_t &r_D) { - // Reference paper: https://arxiv.org/abs/2201.11944. if (correspondence_indices[workload_idx] == -1) { return false; } diff --git a/cpp/open3d/t/pipelines/registration/TransformationEstimation.h b/cpp/open3d/t/pipelines/registration/TransformationEstimation.h index e3c08f01d66..951330f2b36 100644 --- a/cpp/open3d/t/pipelines/registration/TransformationEstimation.h +++ b/cpp/open3d/t/pipelines/registration/TransformationEstimation.h @@ -289,8 +289,9 @@ class TransformationEstimationForColoredICP : public TransformationEstimation { /// \class TransformationEstimationForDopplerICP /// -/// This is implementation of Doppler ICP. -/// Reference: https://arxiv.org/abs/2201.11944 +/// This is the implementation of the following paper: +/// B. Hexsel, H. Vhavle, Y. Chen, +/// DICP: Doppler Iterative Closest Point Algorithm, RSS 2022. /// /// Class to estimate a transformation matrix tensor of shape {4, 4}, dtype /// Float64, on CPU device for DopplerICP method. From 41c40ffdbbb44602c7ba8a5a4e7ba4256961a3a1 Mon Sep 17 00:00:00 2001 From: Heethesh Vhavle Date: Tue, 21 Jun 2022 17:28:19 -0700 Subject: [PATCH 03/14] update changelog --- CHANGELOG.md | 1 + 1 file changed, 1 insertion(+) diff --git a/CHANGELOG.md b/CHANGELOG.md index 4a0ae1eb913..503d22d5e5a 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -6,6 +6,7 @@ * Corrected documentation for KDTree (typo in Notebook) (PR #4744) * Remove `setuptools` and `wheel` from requirements for end users (PR #5020) * Fix various typos (PR #5070) +* Add Doppler ICP in tensor registration pipeline (PR #5237) ## 0.13 From e249e3de606476c954581834a8323b7c59d5f329 Mon Sep 17 00:00:00 2001 From: Heethesh Vhavle Date: Mon, 6 Feb 2023 01:10:51 +0530 Subject: [PATCH 04/14] Refactor Doppler ICP registration --- .../t/pipelines/kernel/Registration.cpp | 20 +- cpp/open3d/t/pipelines/kernel/Registration.h | 22 +- .../t/pipelines/kernel/RegistrationCPU.cpp | 2 +- .../t/pipelines/kernel/RegistrationCUDA.cu | 2 +- .../t/pipelines/kernel/RegistrationImpl.h | 4 +- .../t/pipelines/registration/Registration.cpp | 197 +++--------------- .../t/pipelines/registration/Registration.h | 35 ---- .../registration/TransformationEstimation.cpp | 32 ++- .../registration/TransformationEstimation.h | 109 ++++++---- .../t/pipelines/registration/registration.cpp | 83 ++++---- 10 files changed, 182 insertions(+), 324 deletions(-) diff --git a/cpp/open3d/t/pipelines/kernel/Registration.cpp b/cpp/open3d/t/pipelines/kernel/Registration.cpp index c7de2b729b0..eb2d5bf713a 100644 --- a/cpp/open3d/t/pipelines/kernel/Registration.cpp +++ b/cpp/open3d/t/pipelines/kernel/Registration.cpp @@ -122,17 +122,17 @@ core::Tensor ComputePoseDopplerICP( const core::Tensor &target_normals, const core::Tensor &correspondence_indices, const core::Tensor ¤t_transform, - const core::Tensor &T_V_to_S, + const core::Tensor &transform_vehicle_to_sensor, + const std::size_t iteration, const double period, - const size_t iteration, + const double lambda_doppler, const bool reject_dynamic_outliers, const double doppler_outlier_threshold, - const size_t outlier_rejection_min_iteration, - const size_t geometric_robust_loss_min_iteration, - const size_t doppler_robust_loss_min_iteration, + const std::size_t outlier_rejection_min_iteration, + const std::size_t geometric_robust_loss_min_iteration, + const std::size_t doppler_robust_loss_min_iteration, const registration::RobustKernel &geometric_kernel, - const registration::RobustKernel &doppler_kernel, - const double &lambda_doppler) { + const registration::RobustKernel &doppler_kernel) { const core::Device device = source_points.GetDevice(); const core::Dtype dtype = source_points.GetDtype(); @@ -160,13 +160,15 @@ core::Tensor ComputePoseDopplerICP( // Extract the rotation and translation parts from the matrix. const core::Tensor R_S_to_V = - T_V_to_S.GetItem({core::TensorKey::Slice(0, 3, 1), + transform_vehicle_to_sensor + .GetItem({core::TensorKey::Slice(0, 3, 1), core::TensorKey::Slice(0, 3, 1)}) .Inverse() .Flatten() .To(device, dtype); const core::Tensor r_v_to_s_in_V = - T_V_to_S.GetItem({core::TensorKey::Slice(0, 3, 1), + transform_vehicle_to_sensor + .GetItem({core::TensorKey::Slice(0, 3, 1), core::TensorKey::Slice(3, 4, 1)}) .Flatten() .To(device, dtype); diff --git a/cpp/open3d/t/pipelines/kernel/Registration.h b/cpp/open3d/t/pipelines/kernel/Registration.h index 9db035cf83d..e84fdb147e1 100644 --- a/cpp/open3d/t/pipelines/kernel/Registration.h +++ b/cpp/open3d/t/pipelines/kernel/Registration.h @@ -103,11 +103,12 @@ core::Tensor ComputePoseColoredICP(const core::Tensor &source_positions, /// index of the value itself is the source index. It contains -1 as value at /// index with no correspondence. /// \param current_transform The current pose estimate of ICP. -/// \param T_V_to_S The 4x4 transformation matrix to transform -/// sensor to vehicle frame. +/// \param transform_vehicle_to_sensor The 4x4 extrinsic transformation matrix +/// between the vehicle and the sensor frames. +/// \param iteration current iteration number of the ICP algorithm. /// \param period Time period (in seconds) between the source and the target /// point clouds. -/// \param iteration current iteration number of the ICP algorithm. +/// \param lambda_doppler weight for the Doppler objective. /// \param reject_dynamic_outliers Whether or not to prune dynamic point /// outlier correspondences. /// \param doppler_outlier_threshold Correspondences with Doppler error @@ -120,7 +121,6 @@ core::Tensor ComputePoseColoredICP(const core::Tensor &source_positions, /// after which robust loss for Doppler term kicks in. /// \param geometric_kernel statistical robust kernel for outlier rejection. /// \param doppler_kernel statistical robust kernel for outlier rejection. -/// \param lambda_doppler weight for the Doppler objective. /// \return Pose [alpha beta gamma, tx, ty, tz], a shape {6} tensor of dtype /// Float64, where alpha, beta, gamma are the Euler angles in the ZYX order. core::Tensor ComputePoseDopplerICP( @@ -131,17 +131,17 @@ core::Tensor ComputePoseDopplerICP( const core::Tensor &target_normals, const core::Tensor &correspondence_indices, const core::Tensor ¤t_transform, - const core::Tensor &T_V_to_S, + const core::Tensor &transform_vehicle_to_sensor, + const std::size_t iteration, const double period, - const size_t iteration, + const double lambda_doppler, const bool reject_dynamic_outliers, const double doppler_outlier_threshold, - const size_t outlier_rejection_min_iteration, - const size_t geometric_robust_loss_min_iteration, - const size_t doppler_robust_loss_min_iteration, + const std::size_t outlier_rejection_min_iteration, + const std::size_t geometric_robust_loss_min_iteration, + const std::size_t doppler_robust_loss_min_iteration, const registration::RobustKernel &geometric_kernel, - const registration::RobustKernel &doppler_kernel, - const double &lambda_doppler); + const registration::RobustKernel &doppler_kernel); /// \brief Computes (R) Rotation {3,3} and (t) translation {3,} /// for point to point registration method. diff --git a/cpp/open3d/t/pipelines/kernel/RegistrationCPU.cpp b/cpp/open3d/t/pipelines/kernel/RegistrationCPU.cpp index e5b1979ba19..4bf36b6887c 100644 --- a/cpp/open3d/t/pipelines/kernel/RegistrationCPU.cpp +++ b/cpp/open3d/t/pipelines/kernel/RegistrationCPU.cpp @@ -388,7 +388,7 @@ void ComputePoseDopplerICPCPU( const double doppler_outlier_threshold, const registration::RobustKernel &kernel_geometric, const registration::RobustKernel &kernel_doppler, - const double &lambda_doppler) { + const double lambda_doppler) { const int n = source_points.GetLength(); core::Tensor global_sum = core::Tensor::Zeros({29}, dtype, device); diff --git a/cpp/open3d/t/pipelines/kernel/RegistrationCUDA.cu b/cpp/open3d/t/pipelines/kernel/RegistrationCUDA.cu index 135eec66603..499732e3dcb 100644 --- a/cpp/open3d/t/pipelines/kernel/RegistrationCUDA.cu +++ b/cpp/open3d/t/pipelines/kernel/RegistrationCUDA.cu @@ -347,7 +347,7 @@ void ComputePoseDopplerICPCUDA( const double doppler_outlier_threshold, const registration::RobustKernel &kernel_geometric, const registration::RobustKernel &kernel_doppler, - const double &lambda_doppler) { + const double lambda_doppler) { int n = source_points.GetLength(); const dim3 blocks((n + kThread1DUnit - 1) / kThread1DUnit); const dim3 threads(kThread1DUnit); diff --git a/cpp/open3d/t/pipelines/kernel/RegistrationImpl.h b/cpp/open3d/t/pipelines/kernel/RegistrationImpl.h index 18c460d4c0c..59364895419 100644 --- a/cpp/open3d/t/pipelines/kernel/RegistrationImpl.h +++ b/cpp/open3d/t/pipelines/kernel/RegistrationImpl.h @@ -92,7 +92,7 @@ void ComputePoseDopplerICPCPU( const double doppler_outlier_threshold, const registration::RobustKernel &kernel_geometric, const registration::RobustKernel &kernel_doppler, - const double &lambda_doppler); + const double lambda_doppler); #ifdef BUILD_CUDA_MODULE void ComputePosePointToPlaneCUDA(const core::Tensor &source_points, @@ -142,7 +142,7 @@ void ComputePoseDopplerICPCUDA( const double doppler_outlier_threshold, const registration::RobustKernel &kernel_geometric, const registration::RobustKernel &kernel_doppler, - const double &lambda_doppler); + const double lambda_doppler); #endif void ComputeRtPointToPointCPU(const core::Tensor &source_points, diff --git a/cpp/open3d/t/pipelines/registration/Registration.cpp b/cpp/open3d/t/pipelines/registration/Registration.cpp index a03984a099f..3f88938070a 100644 --- a/cpp/open3d/t/pipelines/registration/Registration.cpp +++ b/cpp/open3d/t/pipelines/registration/Registration.cpp @@ -40,13 +40,6 @@ namespace t { namespace pipelines { namespace registration { -namespace { - -// Minimum time period (sec) between two sequential scans for Doppler ICP. -constexpr double kMinTimePeriod = 1e-3; - -} // namespace - static RegistrationResult ComputeRegistrationResult( const geometry::PointCloud &source, const core::nns::NearestNeighborSearch &target_nns, @@ -185,6 +178,26 @@ static void AssertInputMultiScaleICP( } } + // Doppler ICP requires Doppler velocities and pre-computed directions for + // source point cloud. + if (estimation.GetTransformationEstimationType() == + TransformationEstimationType::DopplerICP) { + if (!target.HasPointNormals()) { + utility::LogError( + "DopplerICP requires target pointcloud to have normals."); + } + if (!source.HasPointAttr("dopplers")) { + utility::LogError( + "DopplerICP requires source pointcloud to have Doppler " + "velocities."); + } + if (!source.HasPointAttr("directions")) { + utility::LogError( + "DopplerICP requires source pointcloud to have " + "pre-computed direction vectors."); + } + } + if (max_correspondence_distances[0] <= 0.0) { utility::LogError( " Max correspondence distance must be greater than 0, but" @@ -280,11 +293,15 @@ static std::tuple DoSingleScaleICPIterations( int iteration_count = 0; for (iteration_count = 0; iteration_count < criteria.max_iteration_; ++iteration_count) { + // Update the results and find correspondences. result = ComputeRegistrationResult( source.GetPointPositions(), target_nns, max_correspondence_distance, result.transformation_); + // No correspondences. if (result.fitness_ <= std::numeric_limits::min()) { + result.converged_ = false; + result.num_iterations_ = iteration_count; return std::make_tuple(result, prev_iteration_count + iteration_count); } @@ -292,10 +309,11 @@ static std::tuple DoSingleScaleICPIterations( // Computing Transform between source and target, given // correspondences. ComputeTransformation returns {4,4} shaped // Float64 transformation tensor on CPU device. - core::Tensor update = + const core::Tensor update = estimation - .ComputeTransformation(source, target, - result.correspondences_) + .ComputeTransformation( + source, target, result.correspondences_, + result.transformation_, iteration_count) .To(core::Float64); // Multiply the transform to the cumulative transformation (update). @@ -333,6 +351,7 @@ static std::tuple DoSingleScaleICPIterations( criteria.relative_fitness_ && std::abs(prev_inlier_rmse - result.inlier_rmse_) < criteria.relative_rmse_) { + result.converged_ = true; break; } prev_fitness = result.fitness_; @@ -410,172 +429,16 @@ RegistrationResult MultiScaleICP( result.transformation_); } - // No correspondences. - if (result.fitness_ <= std::numeric_limits::min()) { - break; - } - } - // ---- Iterating over different resolution scale END -------------------- - - result.num_iterations_ = iteration_count; - - return result; -} - -RegistrationResult DopplerICP( - const geometry::PointCloud &source, - const geometry::PointCloud &target, - const double max_correspondence_distance, - const core::Tensor &init_source_to_target, - const TransformationEstimationForDopplerICP &estimation, - const ICPConvergenceCriteria &criteria, - const double period, - const core::Tensor &T_V_to_S, - const std::function< - void(const std::unordered_map &)> - &callback_after_iteration) { - core::AssertTensorShape(init_source_to_target, {4, 4}); - core::AssertTensorShape(T_V_to_S, {4, 4}); - - geometry::PointCloud source_copy = source; - core::AssertTensorDtypes(source_copy.GetPointPositions(), - {core::Float64, core::Float32}); - - const core::Device device = source_copy.GetDevice(); - const core::Dtype dtype = source_copy.GetPointPositions().GetDtype(); - - if (!target.HasPointPositions() || !source_copy.HasPointPositions()) { - utility::LogError("Source and/or Target pointcloud is empty."); - } - core::AssertTensorDtype(target.GetPointPositions(), dtype); - core::AssertTensorDevice(target.GetPointPositions(), device); - - if (dtype == core::Float64 && - device.GetType() == core::Device::DeviceType::CUDA) { - utility::LogDebug( - "Use Float32 pointcloud for best performance on CUDA device."); - } - - if (estimation.GetTransformationEstimationType() != - TransformationEstimationType::DopplerICP) { - utility::LogError( - "DopplerICP requires TransformationEstimationDopplerICP."); - } - - if (!target.HasPointNormals()) { - utility::LogError( - "TransformationEstimationDopplerICP require pre-computed " - "normal vectors for target PointCloud."); - } - if (!source_copy.HasPointAttr("dopplers")) { - utility::LogError("Source pointcloud missing dopplers attribute."); - } - if (!source_copy.HasPointAttr("directions")) { - utility::LogError("Source pointcloud missing directions attribute."); - } - - if (max_correspondence_distance <= 0.0) { - utility::LogError( - " Max correspondence distance must be greater than 0, but" - " got {} in scale: {}.", - max_correspondence_distance, 0); - } - - if (std::abs(period) < kMinTimePeriod) { - utility::LogError("Time period too small."); - } - - // Transformation tensor is always of shape {4,4}, type Float64 on CPU:0. - core::Tensor transformation = - init_source_to_target.To(core::Device("CPU:0"), core::Float64); - RegistrationResult result(transformation); - - // Apply the initial transform on source pointcloud. - source_copy.Transform(transformation); - - // Initialize Neighbor Search. - core::nns::NearestNeighborSearch target_nns(target.GetPointPositions()); - bool check = target_nns.HybridIndex(max_correspondence_distance); - if (!check) { - utility::LogError("Index is not set."); - } - - int iteration_count = 0; - for (iteration_count = 0; iteration_count < criteria.max_iteration_; - ++iteration_count) { - double prev_fitness = result.fitness_; - double prev_inlier_rmse = result.inlier_rmse_; - - // Update the results and find correspondences. - result = ComputeRegistrationResult( - source_copy.GetPointPositions(), target_nns, - max_correspondence_distance, result.transformation_); - // No correspondences. if (result.fitness_ <= std::numeric_limits::min()) { result.converged_ = false; - result.num_iterations_ = iteration_count; - return result; - } - - // Computing Transform between source and target, given - // correspondences. ComputeTransformation returns {4,4} shaped - // Float64 transformation tensor on CPU device. - core::Tensor update = - estimation - .ComputeTransformation(source_copy, target, - result.correspondences_, - result.transformation_, T_V_to_S, - period, iteration_count) - .To(core::Float64); - - // Multiply the transform to the cumulative transformation (update). - result.transformation_ = update.Matmul(result.transformation_); - - // Apply the transform on source pointcloud. - source_copy.Transform(update); - - utility::LogDebug("ICP Iteration #{:d}: Fitness {:.4f}, RMSE {:.4f}", - iteration_count, result.fitness_, - result.inlier_rmse_); - - // Additional per-iteration results can recorded in this callback. - if (callback_after_iteration) { - const core::Device host("CPU:0"); - - std::unordered_map loss_attribute_map{ - {"iteration_index", - core::Tensor::Init(iteration_count)}, - {"inlier_rmse", - core::Tensor::Init(result.inlier_rmse_)}, - {"fitness", core::Tensor::Init(result.fitness_)}, - {"transformation", result.transformation_.To(host)}}; - callback_after_iteration(loss_attribute_map); - } - - // ICPConvergenceCriteria, to terminate iteration. - if (iteration_count != 0 && - std::abs(prev_fitness - result.fitness_) < - criteria.relative_fitness_ && - std::abs(prev_inlier_rmse - result.inlier_rmse_) < - criteria.relative_rmse_) { - // Calculate final `fitness` and `inlier_rmse` for the current - // `transformation` stored in `result`. - result = ComputeRegistrationResult(source_copy, target_nns, - max_correspondence_distance, - result.transformation_); - result.converged_ = true; break; } } + // ---- Iterating over different resolution scale END -------------------- result.num_iterations_ = iteration_count; - // No correspondences. - if (result.fitness_ <= std::numeric_limits::epsilon()) { - result.converged_ = false; - } - return result; } diff --git a/cpp/open3d/t/pipelines/registration/Registration.h b/cpp/open3d/t/pipelines/registration/Registration.h index 0a1b3b55973..2a2b0d3a17a 100644 --- a/cpp/open3d/t/pipelines/registration/Registration.h +++ b/cpp/open3d/t/pipelines/registration/Registration.h @@ -201,41 +201,6 @@ RegistrationResult MultiScaleICP( void(const std::unordered_map &)> &callback_after_iteration = nullptr); -/// \brief Functions for DopplerICP registration. -/// -/// \param source The source point cloud. (Float32 or Float64 type). -/// \param target The target point cloud. (Float32 or Float64 type). -/// \param max_correspondence_distance Maximum correspondence points-pair -/// distance. -/// \param init_source_to_target Initial transformation estimation of type -/// Float64 on CPU. -/// \param estimation Estimation method. -/// \param criteria Convergence criteria. -/// \param period Time period (in seconds) between the source and the target -/// point clouds. Default value: 0.1. -/// \param T_V_to_S The 4x4 transformation matrix to transform sensor to vehicle -/// frame. Defaults to identity transform. -/// \param callback_after_iteration Optional lambda function, saves string to -/// tensor map of attributes such as "iteration_index", "scale_index", -/// "scale_iteration_index", "inlier_rmse", "fitness", "transformation", on CPU -/// device, updated after each iteration. -RegistrationResult DopplerICP( - const geometry::PointCloud &source, - const geometry::PointCloud &target, - const double max_correspondence_distance, - const core::Tensor &init_source_to_target = - core::Tensor::Eye(4, core::Float64, core::Device("CPU:0")), - const TransformationEstimationForDopplerICP &estimation = - TransformationEstimationForDopplerICP(), - const ICPConvergenceCriteria &criteria = ICPConvergenceCriteria(), - const double period = 0.1F, - const core::Tensor &T_V_to_S = core::Tensor::Eye(4, - core::Float64, - core::Device("CPU:0")), - const std::function< - void(const std::unordered_map &)> - &callback_after_iteration = nullptr); - /// \brief Computes `Information Matrix`, from the transfromation between source /// and target pointcloud. It returns the `Information Matrix` of shape {6, 6}, /// of dtype `Float64` on device `CPU:0`. diff --git a/cpp/open3d/t/pipelines/registration/TransformationEstimation.cpp b/cpp/open3d/t/pipelines/registration/TransformationEstimation.cpp index b2efee922ca..5666d3f05a2 100644 --- a/cpp/open3d/t/pipelines/registration/TransformationEstimation.cpp +++ b/cpp/open3d/t/pipelines/registration/TransformationEstimation.cpp @@ -89,7 +89,9 @@ double TransformationEstimationPointToPoint::ComputeRMSE( core::Tensor TransformationEstimationPointToPoint::ComputeTransformation( const geometry::PointCloud &source, const geometry::PointCloud &target, - const core::Tensor &correspondences) const { + const core::Tensor &correspondences, + const core::Tensor ¤t_transform, + const std::size_t iteration) const { if (!target.HasPointPositions() || !source.HasPointPositions()) { utility::LogError("Source and/or Target pointcloud is empty."); } @@ -151,7 +153,9 @@ double TransformationEstimationPointToPlane::ComputeRMSE( core::Tensor TransformationEstimationPointToPlane::ComputeTransformation( const geometry::PointCloud &source, const geometry::PointCloud &target, - const core::Tensor &correspondences) const { + const core::Tensor &correspondences, + const core::Tensor ¤t_transform, + const std::size_t iteration) const { if (!target.HasPointPositions() || !source.HasPointPositions()) { utility::LogError("Source and/or Target pointcloud is empty."); } @@ -270,7 +274,9 @@ double TransformationEstimationForColoredICP::ComputeRMSE( core::Tensor TransformationEstimationForColoredICP::ComputeTransformation( const geometry::PointCloud &source, const geometry::PointCloud &target, - const core::Tensor &correspondences) const { + const core::Tensor &correspondences, + const core::Tensor ¤t_transform, + const std::size_t iteration) const { if (!target.HasPointPositions() || !source.HasPointPositions()) { utility::LogError("Source and/or Target pointcloud is empty."); } @@ -352,25 +358,12 @@ double TransformationEstimationForDopplerICP::ComputeRMSE( static_cast(neighbour_indices.GetLength())); } -core::Tensor TransformationEstimationForDopplerICP::ComputeTransformation( - const geometry::PointCloud &source, - const geometry::PointCloud &target, - const core::Tensor &correspondences) const { - utility::LogError( - "This method should not be called for DopplerICP. DopplerICP " - "requires the period, current transformation, and T_V_to_S " - "calibration."); - return core::Tensor::Eye(4, core::Float64, core::Device("CPU:0")); -} - core::Tensor TransformationEstimationForDopplerICP::ComputeTransformation( const geometry::PointCloud &source, const geometry::PointCloud &target, const core::Tensor &correspondences, const core::Tensor ¤t_transform, - const core::Tensor &T_V_to_S, - const double period, - const size_t iteration) const { + const std::size_t iteration) const { if (!source.HasPointPositions() || !target.HasPointPositions()) { utility::LogError("Source and/or Target pointcloud is empty."); } @@ -403,12 +396,13 @@ core::Tensor TransformationEstimationForDopplerICP::ComputeTransformation( source.GetPointPositions(), source.GetPointAttr("dopplers"), source.GetPointAttr("directions"), target.GetPointPositions(), target.GetPointNormals(), correspondences, current_transform, - T_V_to_S, period, iteration, this->reject_dynamic_outliers_, + this->transform_vehicle_to_sensor_, iteration, this->period_, + this->lambda_doppler_, this->reject_dynamic_outliers_, this->doppler_outlier_threshold_, this->outlier_rejection_min_iteration_, this->geometric_robust_loss_min_iteration_, this->doppler_robust_loss_min_iteration_, this->geometric_kernel_, - this->doppler_kernel_, this->lambda_doppler_); + this->doppler_kernel_); // Get transformation {4,4} of type Float64 from pose {6}. core::Tensor transform = pipelines::kernel::PoseToTransformation(pose); diff --git a/cpp/open3d/t/pipelines/registration/TransformationEstimation.h b/cpp/open3d/t/pipelines/registration/TransformationEstimation.h index 951330f2b36..8045ce04aaf 100644 --- a/cpp/open3d/t/pipelines/registration/TransformationEstimation.h +++ b/cpp/open3d/t/pipelines/registration/TransformationEstimation.h @@ -26,6 +26,7 @@ #pragma once +#include #include #include #include @@ -35,6 +36,7 @@ #include "open3d/t/geometry/PointCloud.h" #include "open3d/t/pipelines/kernel/TransformationConverter.h" #include "open3d/t/pipelines/registration/RobustKernel.h" +#include "open3d/utility/Logging.h" namespace open3d { @@ -46,6 +48,13 @@ class PointCloud; namespace pipelines { namespace registration { +namespace { + +// Minimum time period (sec) between two sequential scans for Doppler ICP. +constexpr double kMinTimePeriod{1e-3}; + +} // namespace + enum class TransformationEstimationType { Unspecified = 0, PointToPoint = 1, @@ -90,12 +99,16 @@ class TransformationEstimation { /// corresponding target points, where the value is the target index and the /// index of the value itself is the source index. It contains -1 as value /// at index with no correspondence. + /// \param current_transform The current pose estimate of ICP. + /// \param iteration The current iteration number of the ICP algorithm. /// \return transformation between source to target, a tensor of shape {4, /// 4}, type Float64 on CPU device. virtual core::Tensor ComputeTransformation( const geometry::PointCloud &source, const geometry::PointCloud &target, - const core::Tensor &correspondences) const = 0; + const core::Tensor &correspondences, + const core::Tensor ¤t_transform, + const std::size_t iteration) const = 0; }; /// \class TransformationEstimationPointToPoint @@ -135,12 +148,16 @@ class TransformationEstimationPointToPoint : public TransformationEstimation { /// corresponding target points, where the value is the target index and the /// index of the value itself is the source index. It contains -1 as value /// at index with no correspondence. + /// \param current_transform The current pose estimate of ICP. + /// \param iteration The current iteration number of the ICP algorithm. /// \return transformation between source to target, a tensor of shape {4, /// 4}, type Float64 on CPU device. core::Tensor ComputeTransformation( const geometry::PointCloud &source, const geometry::PointCloud &target, - const core::Tensor &correspondences) const override; + const core::Tensor &correspondences, + const core::Tensor ¤t_transform, + const std::size_t iteration) const override; private: const TransformationEstimationType type_ = @@ -194,12 +211,16 @@ class TransformationEstimationPointToPlane : public TransformationEstimation { /// corresponding target points, where the value is the target index and the /// index of the value itself is the source index. It contains -1 as value /// at index with no correspondence. - /// \return transformation between source to target, a tensor - /// of shape {4, 4}, type Float64 on CPU device. + /// \param current_transform The current pose estimate of ICP. + /// \param iteration The current iteration number of the ICP algorithm. + /// \return transformation between source to target, a tensor of shape {4, + /// 4}, type Float64 on CPU device. core::Tensor ComputeTransformation( const geometry::PointCloud &source, const geometry::PointCloud &target, - const core::Tensor &correspondences) const override; + const core::Tensor &correspondences, + const core::Tensor ¤t_transform, + const std::size_t iteration) const override; public: /// RobustKernel for outlier rejection. @@ -270,12 +291,16 @@ class TransformationEstimationForColoredICP : public TransformationEstimation { /// corresponding target points, where the value is the target index and the /// index of the value itself is the source index. It contains -1 as value /// at index with no correspondence. + /// \param current_transform The current pose estimate of ICP. + /// \param iteration The current iteration number of the ICP algorithm. /// \return transformation between source to target, a tensor of shape {4, /// 4}, type Float64 on CPU device. core::Tensor ComputeTransformation( const geometry::PointCloud &source, const geometry::PointCloud &target, - const core::Tensor &correspondences) const override; + const core::Tensor &correspondences, + const core::Tensor ¤t_transform, + const std::size_t iteration) const override; public: double lambda_geometric_ = 0.968; @@ -301,6 +326,8 @@ class TransformationEstimationForDopplerICP : public TransformationEstimation { /// \brief Constructor. /// + /// \param period Time period (in seconds) between the source and the target + /// point clouds. Default value: 0.1. /// \param lambda_doppler `λ ∈ [0, 1]` in the overall energy `(1−λ)EG + /// λED`. Refer the documentation of DopplerICP for more information. /// \param reject_dynamic_outliers Whether or not to reject dynamic point @@ -317,18 +344,25 @@ class TransformationEstimationForDopplerICP : public TransformationEstimation { /// robust kernel for outlier rejection for the geometric term. /// \param doppler_kernel (optional) Any of the implemented statistical /// robust kernel for outlier rejection for the Doppler term. + /// \param transform_vehicle_to_sensor The 4x4 extrinsic transformation + /// matrix between the vehicle and the sensor frames. Defaults to identity + /// transform. explicit TransformationEstimationForDopplerICP( - double lambda_doppler = 0.01, - bool reject_dynamic_outliers = false, - double doppler_outlier_threshold = 2.0, - size_t outlier_rejection_min_iteration = 2, - size_t geometric_robust_loss_min_iteration = 0, - size_t doppler_robust_loss_min_iteration = 2, + const double period = 0.1, + const double lambda_doppler = 0.01, + const bool reject_dynamic_outliers = false, + const double doppler_outlier_threshold = 2.0, + const std::size_t outlier_rejection_min_iteration = 2, + const std::size_t geometric_robust_loss_min_iteration = 0, + const std::size_t doppler_robust_loss_min_iteration = 2, const RobustKernel &geometric_kernel = RobustKernel(RobustKernelMethod::L2Loss, 1.0, 1.0), const RobustKernel &doppler_kernel = - RobustKernel(RobustKernelMethod::L2Loss, 1.0, 1.0)) - : lambda_doppler_(lambda_doppler), + RobustKernel(RobustKernelMethod::L2Loss, 1.0, 1.0), + const core::Tensor &transform_vehicle_to_sensor = + core::Tensor::Eye(4, core::Float64, core::Device("CPU:0"))) + : period_(period), + lambda_doppler_(lambda_doppler), reject_dynamic_outliers_(reject_dynamic_outliers), doppler_outlier_threshold_(doppler_outlier_threshold), outlier_rejection_min_iteration_(outlier_rejection_min_iteration), @@ -336,7 +370,14 @@ class TransformationEstimationForDopplerICP : public TransformationEstimation { geometric_robust_loss_min_iteration), doppler_robust_loss_min_iteration_(doppler_robust_loss_min_iteration), geometric_kernel_(geometric_kernel), - doppler_kernel_(doppler_kernel) { + doppler_kernel_(doppler_kernel), + transform_vehicle_to_sensor_(transform_vehicle_to_sensor) { + core::AssertTensorShape(transform_vehicle_to_sensor, {4, 4}); + + if (std::abs(period) < kMinTimePeriod) { + utility::LogError("Time period too small."); + } + if (lambda_doppler_ < 0 || lambda_doppler_ > 1.0) { lambda_doppler_ = 0.01; } @@ -363,13 +404,6 @@ class TransformationEstimationForDopplerICP : public TransformationEstimation { const geometry::PointCloud &target, const core::Tensor &correspondences) const override; - /// \brief This method is only present to override the method in the base - /// class and must not be used. Use the other method below. - core::Tensor ComputeTransformation( - const geometry::PointCloud &source, - const geometry::PointCloud &target, - const core::Tensor &correspondences) const override; - /// \brief Estimates the transformation matrix for DopplerICP method, /// a tensor of shape {4, 4}, and dtype Float64 on CPU device. /// @@ -382,22 +416,19 @@ class TransformationEstimationForDopplerICP : public TransformationEstimation { /// index of the value itself is the source index. It contains -1 as value /// at index with no correspondence. /// \param current_transform The current pose estimate of ICP. - /// \param T_V_to_S The 4x4 transformation matrix to transform - /// sensor to vehicle frame. - /// \param period Time period (in seconds) between the source and the target - /// point clouds. - /// \param iteration current iteration number of the ICP algorithm. + /// \param iteration The current iteration number of the ICP algorithm. /// \return transformation between source to target, a tensor of shape {4, /// 4}, type Float64 on CPU device. - core::Tensor ComputeTransformation(const geometry::PointCloud &source, - const geometry::PointCloud &target, - const core::Tensor &correspondences, - const core::Tensor ¤t_transform, - const core::Tensor &T_V_to_S, - const double period, - const size_t iteration) const; + core::Tensor ComputeTransformation( + const geometry::PointCloud &source, + const geometry::PointCloud &target, + const core::Tensor &correspondences, + const core::Tensor ¤t_transform, + const std::size_t iteration) const override; public: + /// Time period (in seconds) between the source and the target point clouds. + double period_{0.1}; /// Factor that weighs the Doppler residual term in DICP objective. double lambda_doppler_{0.01}; /// Whether or not to prune dynamic point outlier correspondences. @@ -406,15 +437,19 @@ class TransformationEstimationForDopplerICP : public TransformationEstimation { /// rejected from optimization. double doppler_outlier_threshold_{2.0}; /// Number of iterations of ICP after which outlier rejection is enabled. - size_t outlier_rejection_min_iteration_{2}; + std::size_t outlier_rejection_min_iteration_{2}; /// Number of iterations of ICP after which robust loss kicks in. - size_t geometric_robust_loss_min_iteration_{0}; - size_t doppler_robust_loss_min_iteration_{2}; + std::size_t geometric_robust_loss_min_iteration_{0}; + std::size_t doppler_robust_loss_min_iteration_{2}; /// RobustKernel for outlier rejection. RobustKernel geometric_kernel_ = RobustKernel(RobustKernelMethod::L2Loss, 1.0, 1.0); RobustKernel doppler_kernel_ = RobustKernel(RobustKernelMethod::L2Loss, 1.0, 1.0); + /// The 4x4 extrinsic transformation matrix between the vehicle and the + /// sensor frames. + core::Tensor transform_vehicle_to_sensor_ = + core::Tensor::Eye(4, core::Float64, core::Device("CPU:0")); private: const TransformationEstimationType type_ = diff --git a/cpp/pybind/t/pipelines/registration/registration.cpp b/cpp/pybind/t/pipelines/registration/registration.cpp index 6a6da0703e9..3b116c96295 100644 --- a/cpp/pybind/t/pipelines/registration/registration.cpp +++ b/cpp/pybind/t/pipelines/registration/registration.cpp @@ -54,12 +54,14 @@ class PyTransformationEstimation : public TransformationEstimationBase { PYBIND11_OVERLOAD_PURE(double, TransformationEstimationBase, source, target, correspondences); } - core::Tensor ComputeTransformation( - const t::geometry::PointCloud &source, - const t::geometry::PointCloud &target, - const core::Tensor &correspondences) const { + core::Tensor ComputeTransformation(const t::geometry::PointCloud &source, + const t::geometry::PointCloud &target, + const core::Tensor &correspondences, + const core::Tensor ¤t_transform, + const std::size_t iteration) const { PYBIND11_OVERLOAD_PURE(core::Tensor, TransformationEstimationBase, - source, target, correspondences); + source, target, correspondences, + current_transform, iteration); } }; @@ -153,7 +155,8 @@ void pybind_registration_classes(py::module &m) { "correspondences."); te.def("compute_transformation", &TransformationEstimation::ComputeTransformation, "source"_a, - "target"_a, "correspondences"_a, + "target"_a, "correspondences"_a, "current_transform"_a, + "iteration"_a, "Compute transformation from source to target point cloud given " "correspondences."); docstring::ClassMethodDocInject(m, "TransformationEstimation", @@ -176,7 +179,10 @@ void pybind_registration_classes(py::module &m) { "Tensor of type Int64 containing indices of corresponding target " "points, where the value is the target index and the index of " "the value itself is the source index. It contains -1 as value " - "at index with no correspondence."}}); + "at index with no correspondence."}, + {"current_transform", "The current pose estimate of ICP."}, + {"iteration", + "The current iteration number of the ICP algorithm."}}); // open3d.t.pipelines.registration.TransformationEstimationPointToPoint // TransformationEstimation @@ -275,27 +281,29 @@ void pybind_registration_classes(py::module &m) { te_dop); py::detail::bind_copy_functions( te_dop); - te_dop.def(py::init([](double lambda_doppler, bool reject_dynamic_outliers, + te_dop.def(py::init([](double period, double lambda_doppler, + bool reject_dynamic_outliers, double doppler_outlier_threshold, - size_t outlier_rejection_min_iteration, - size_t geometric_robust_loss_min_iteration, - size_t doppler_robust_loss_min_iteration, + std::size_t outlier_rejection_min_iteration, + std::size_t geometric_robust_loss_min_iteration, + std::size_t doppler_robust_loss_min_iteration, RobustKernel &geometric_kernel, - RobustKernel &doppler_kernel) { + RobustKernel &doppler_kernel, + core::Tensor &transform_vehicle_to_sensor) { return new TransformationEstimationForDopplerICP( - lambda_doppler, reject_dynamic_outliers, + period, lambda_doppler, reject_dynamic_outliers, doppler_outlier_threshold, outlier_rejection_min_iteration, geometric_robust_loss_min_iteration, doppler_robust_loss_min_iteration, geometric_kernel, - doppler_kernel); + doppler_kernel, transform_vehicle_to_sensor); }), - "lambda_doppler"_a, "reject_dynamic_outliers"_a, + "period"_a, "lambda_doppler"_a, "reject_dynamic_outliers"_a, "doppler_outlier_threshold"_a, "outlier_rejection_min_iteration"_a, "geometric_robust_loss_min_iteration"_a, "doppler_robust_loss_min_iteration"_a, "goemetric_kernel"_a, - "doppler_kernel"_a) + "doppler_kernel"_a, "transform_vehicle_to_sensor"_a) .def(py::init([](const double lambda_doppler) { return new TransformationEstimationForDopplerICP( lambda_doppler); @@ -305,8 +313,7 @@ void pybind_registration_classes(py::module &m) { py::overload_cast( + const std::size_t>( &TransformationEstimationForDopplerICP:: ComputeTransformation, py::const_), @@ -319,19 +326,25 @@ void pybind_registration_classes(py::module &m) { "with lambda_doppler: ") + std::to_string(te.lambda_doppler_); }) + .def_readwrite("period", + &TransformationEstimationForDopplerICP::period_, + "Time period (in seconds) between the source and " + "the target point clouds.") .def_readwrite( "lambda_doppler", &TransformationEstimationForDopplerICP::lambda_doppler_, - "lambda_doppler") + "`λ ∈ [0, 1]` in the overall energy `(1−λ)EG + λED`. Refer " + "the documentation of DopplerICP for more information.") .def_readwrite("reject_dynamic_outliers", &TransformationEstimationForDopplerICP:: reject_dynamic_outliers_, - "Performs dynamic point outlier rejection of " - "correspondences") + "Whether or not to reject dynamic point outlier " + "correspondences.") .def_readwrite("doppler_outlier_threshold", &TransformationEstimationForDopplerICP:: doppler_outlier_threshold_, - "doppler_outlier_threshold") + "Correspondences with Doppler error greater than " + "this threshold are rejected from optimization.") .def_readwrite("outlier_rejection_min_iteration", &TransformationEstimationForDopplerICP:: outlier_rejection_min_iteration_, @@ -354,7 +367,12 @@ void pybind_registration_classes(py::module &m) { .def_readwrite( "doppler_kernel", &TransformationEstimationForDopplerICP::doppler_kernel_, - "Robust Kernel used in the Doppler Error Optimization"); + "Robust Kernel used in the Doppler Error Optimization") + .def_readwrite("transform_vehicle_to_sensor", + &TransformationEstimationForDopplerICP:: + transform_vehicle_to_sensor_, + "The 4x4 extrinsic transformation matrix between " + "the vehicle and the sensor frames."); } // Registration functions have similar arguments, sharing arg docstrings. @@ -382,17 +400,11 @@ static const std::unordered_map "o3d.utility.DoubleVector of maximum correspondence " "points-pair distances for multi-scale icp."}, {"option", "Registration option"}, - {"period", - "Time period (in seconds) between the source and the target " - "point clouds."}, {"source", "The source point cloud."}, {"target", "The target point cloud."}, {"transformation", "The 4x4 transformation matrix of type Float64 " "to transform ``source`` to ``target``"}, - {"T_V_to_S", - "The 4x4 transformation matrix to transform ``sensor`` to " - "``vehicle`` frame."}, {"voxel_size", "The input pointclouds will be down-sampled to this " "`voxel_size` scale. If `voxel_size` < 0, original scale will " @@ -438,19 +450,6 @@ void pybind_registration_methods(py::module &m) { docstring::FunctionDocInject(m, "multi_scale_icp", map_shared_argument_docstrings); - m.def("doppler_icp", &DopplerICP, py::call_guard(), - "Function for Doppler ICP registration", "source"_a, "target"_a, - "max_correspondence_distance"_a, - "init_source_to_target"_a = - core::Tensor::Eye(4, core::Float64, core::Device("CPU:0")), - "estimation_method"_a = TransformationEstimationForDopplerICP(), - "criteria"_a = ICPConvergenceCriteria(), "period"_a = 0.1F, - "T_V_to_S"_a = - core::Tensor::Eye(4, core::Float64, core::Device("CPU:0")), - "callback_after_iteration"_a = py::none()); - docstring::FunctionDocInject(m, "doppler_icp", - map_shared_argument_docstrings); - m.def("get_information_matrix", &GetInformationMatrix, py::call_guard(), "Function for computing information matrix from transformation " From ea0dfdec601dcb5f3faee131ce8e45de8a3a19b6 Mon Sep 17 00:00:00 2001 From: Heethesh Vhavle Date: Mon, 13 Feb 2023 03:43:18 +0400 Subject: [PATCH 05/14] fix tests and build --- .../t/pipelines/kernel/RegistrationCUDA.cu | 79 ++++++++++--------- .../registration/TransformationEstimation.h | 25 +++--- 2 files changed, 57 insertions(+), 47 deletions(-) diff --git a/cpp/open3d/t/pipelines/kernel/RegistrationCUDA.cu b/cpp/open3d/t/pipelines/kernel/RegistrationCUDA.cu index 499732e3dcb..8722c5e933d 100644 --- a/cpp/open3d/t/pipelines/kernel/RegistrationCUDA.cu +++ b/cpp/open3d/t/pipelines/kernel/RegistrationCUDA.cu @@ -269,51 +269,54 @@ __global__ void ComputePoseDopplerICPKernelCUDA( funct1_t GetWeightFromRobustKernelFirst, // Geometric kernel funct2_t GetWeightFromRobustKernelSecond // Doppler kernel ) { - __shared__ scalar_t local_sum0[kThread1DUnit]; - __shared__ scalar_t local_sum1[kThread1DUnit]; - __shared__ scalar_t local_sum2[kThread1DUnit]; - - const int tid = threadIdx.x; - - local_sum0[tid] = 0; - local_sum1[tid] = 0; - local_sum2[tid] = 0; + typedef utility::MiniVec ReduceVec; + // Create shared memory. + typedef cub::BlockReduce BlockReduce; + __shared__ typename BlockReduce::TempStorage temp_storage; + ReduceVec local_sum(static_cast(0)); const int workload_idx = threadIdx.x + blockIdx.x * blockDim.x; + if (workload_idx < n) { + scalar_t J_G[6] = {0}, J_D[6] = {0}; + scalar_t r_G = 0, r_D = 0; - if (workload_idx >= n) return; - - scalar_t J_G[6] = {0}, J_D[6] = {0}, reduction[29] = {0}; - scalar_t r_G = 0, r_D = 0; - - bool valid = GetJacobianDopplerICP( - workload_idx, source_points_ptr, source_dopplers_ptr, - source_directions_ptr, target_points_ptr, target_normals_ptr, - correspondence_indices, R_S_to_V, r_v_to_s_in_V, v_s_in_S, - prune_correspondences, doppler_outlier_threshold, - sqrt_lambda_geometric, sqrt_lambda_doppler, - sqrt_lambda_doppler_by_dt, J_G, J_D, r_G, r_D); + const bool valid = GetJacobianDopplerICP( + workload_idx, source_points_ptr, source_dopplers_ptr, + source_directions_ptr, target_points_ptr, target_normals_ptr, + correspondence_indices, R_S_to_V, r_v_to_s_in_V, v_s_in_S, + prune_correspondences, doppler_outlier_threshold, + sqrt_lambda_geometric, sqrt_lambda_doppler, + sqrt_lambda_doppler_by_dt, J_G, J_D, r_G, r_D); - scalar_t w_G = GetWeightFromRobustKernelFirst(r_G); - scalar_t w_D = GetWeightFromRobustKernelSecond(r_D); + if (valid) { + const scalar_t w_G = GetWeightFromRobustKernelFirst(r_G); + const scalar_t w_D = GetWeightFromRobustKernelSecond(r_D); - if (valid) { - // Dump J, r into JtJ and Jtr - int i = 0; - for (int j = 0; j < 6; ++j) { - for (int k = 0; k <= j; ++k) { - reduction[i] += J_G[j] * w_G * J_G[k] + J_D[j] * w_D * J_D[k]; - ++i; + // Dump J, r into JtJ and Jtr + int i = 0; + for (int j = 0; j < 6; ++j) { + for (int k = 0; k <= j; ++k) { + local_sum[i] += + J_G[j] * w_G * J_G[k] + J_D[j] * w_D * J_D[k]; + ++i; + } + local_sum[21 + j] += J_G[j] * w_G * r_G + J_D[j] * w_D * r_D; } - reduction[21 + j] += J_G[j] * w_G * r_G + J_D[j] * w_D * r_D; + local_sum[27] += r_G * r_G + r_D * r_D; + local_sum[28] += 1; } - reduction[27] += r_G * r_G + r_D * r_D; - reduction[28] += 1; } - ReduceSum6x6LinearSystem(tid, valid, reduction, - local_sum0, local_sum1, - local_sum2, global_sum); + // Reduction. + auto result = BlockReduce(temp_storage).Sum(local_sum); + + // Add result to global_sum. + if (threadIdx.x == 0) { +#pragma unroll + for (int i = 0; i < kReduceDim; ++i) { + atomicAdd(&global_sum[i], result[i]); + } + } } template @@ -348,11 +351,13 @@ void ComputePoseDopplerICPCUDA( const registration::RobustKernel &kernel_geometric, const registration::RobustKernel &kernel_doppler, const double lambda_doppler) { + core::CUDAScopedDevice scoped_device(source_points.GetDevice()); int n = source_points.GetLength(); + + core::Tensor global_sum = core::Tensor::Zeros({29}, dtype, device); const dim3 blocks((n + kThread1DUnit - 1) / kThread1DUnit); const dim3 threads(kThread1DUnit); - core::Tensor global_sum = core::Tensor::Zeros({29}, dtype, device); core::Tensor v_s_in_S = core::Tensor::Zeros({3}, dtype, device); DISPATCH_FLOAT_DTYPE_TO_TEMPLATE(dtype, [&]() { diff --git a/cpp/open3d/t/pipelines/registration/TransformationEstimation.h b/cpp/open3d/t/pipelines/registration/TransformationEstimation.h index 8045ce04aaf..3d8fe93d899 100644 --- a/cpp/open3d/t/pipelines/registration/TransformationEstimation.h +++ b/cpp/open3d/t/pipelines/registration/TransformationEstimation.h @@ -107,8 +107,9 @@ class TransformationEstimation { const geometry::PointCloud &source, const geometry::PointCloud &target, const core::Tensor &correspondences, - const core::Tensor ¤t_transform, - const std::size_t iteration) const = 0; + const core::Tensor ¤t_transform = + core::Tensor::Eye(4, core::Float64, core::Device("CPU:0")), + const std::size_t iteration = 0) const = 0; }; /// \class TransformationEstimationPointToPoint @@ -156,8 +157,9 @@ class TransformationEstimationPointToPoint : public TransformationEstimation { const geometry::PointCloud &source, const geometry::PointCloud &target, const core::Tensor &correspondences, - const core::Tensor ¤t_transform, - const std::size_t iteration) const override; + const core::Tensor ¤t_transform = + core::Tensor::Eye(4, core::Float64, core::Device("CPU:0")), + const std::size_t iteration = 0) const override; private: const TransformationEstimationType type_ = @@ -219,8 +221,9 @@ class TransformationEstimationPointToPlane : public TransformationEstimation { const geometry::PointCloud &source, const geometry::PointCloud &target, const core::Tensor &correspondences, - const core::Tensor ¤t_transform, - const std::size_t iteration) const override; + const core::Tensor ¤t_transform = + core::Tensor::Eye(4, core::Float64, core::Device("CPU:0")), + const std::size_t iteration = 0) const override; public: /// RobustKernel for outlier rejection. @@ -299,8 +302,9 @@ class TransformationEstimationForColoredICP : public TransformationEstimation { const geometry::PointCloud &source, const geometry::PointCloud &target, const core::Tensor &correspondences, - const core::Tensor ¤t_transform, - const std::size_t iteration) const override; + const core::Tensor ¤t_transform = + core::Tensor::Eye(4, core::Float64, core::Device("CPU:0")), + const std::size_t iteration = 0) const override; public: double lambda_geometric_ = 0.968; @@ -423,8 +427,9 @@ class TransformationEstimationForDopplerICP : public TransformationEstimation { const geometry::PointCloud &source, const geometry::PointCloud &target, const core::Tensor &correspondences, - const core::Tensor ¤t_transform, - const std::size_t iteration) const override; + const core::Tensor ¤t_transform = + core::Tensor::Eye(4, core::Float64, core::Device("CPU:0")), + const std::size_t iteration = 0) const override; public: /// Time period (in seconds) between the source and the target point clouds. From 6d4cc4064bde7639bde38887b9ba78363e2b97e3 Mon Sep 17 00:00:00 2001 From: Heethesh Vhavle Date: Wed, 1 Mar 2023 14:26:20 +0530 Subject: [PATCH 06/14] Add DemoDopplerICPSequence dataset --- cpp/open3d/data/CMakeLists.txt | 1 + cpp/open3d/data/Dataset.h | 26 ++++++++ .../data/dataset/DemoDopplerICPSequence.cpp | 66 +++++++++++++++++++ cpp/tests/io/CMakeLists.txt | 1 + cpp/tests/io/file_format/FileXYZD.cpp | 37 +++++++++++ 5 files changed, 131 insertions(+) create mode 100644 cpp/open3d/data/dataset/DemoDopplerICPSequence.cpp create mode 100644 cpp/tests/io/file_format/FileXYZD.cpp diff --git a/cpp/open3d/data/CMakeLists.txt b/cpp/open3d/data/CMakeLists.txt index 660db6034e1..84bd2dab44e 100644 --- a/cpp/open3d/data/CMakeLists.txt +++ b/cpp/open3d/data/CMakeLists.txt @@ -14,6 +14,7 @@ target_sources(data PRIVATE dataset/DemoColoredICPPointClouds.cpp dataset/DemoCropPointCloud.cpp dataset/DemoCustomVisualization.cpp + dataset/DemoDopplerICPSequence.cpp dataset/DemoFeatureMatchingPointClouds.cpp dataset/DemoICPPointClouds.cpp dataset/DemoPoseGraphOptimization.cpp diff --git a/cpp/open3d/data/Dataset.h b/cpp/open3d/data/Dataset.h index 7d9eb8b88f3..166cf1b4cc0 100644 --- a/cpp/open3d/data/Dataset.h +++ b/cpp/open3d/data/Dataset.h @@ -362,6 +362,32 @@ class DemoCustomVisualization : public DownloadDataset { std::string render_option_path_; }; +/// \class DemoDopplerICPSequence +/// \brief Data class for `DemoDopplerICPSequence` contains an example sequence +/// of 100 point cloud with Doppler velocity channel and corresponding ground +/// truth poses. The sequence was generated using the CARLA simulator. +class DemoDopplerICPSequence : public DownloadDataset { +public: + DemoDopplerICPSequence(const std::string& data_root = ""); + + /// \brief Returns list of the first 10 point cloud paths in the sequence. + std::vector GetPaths() const { return paths_; } + /// \brief Path to the point cloud at index. + std::string GetPath(std::size_t index) const; + /// \brief Path to the calibration metadata file, containing transformation + /// between the vehicle and sensor frames and the time period. + std::string GetCalibrationPath() const { + return calibration_path_; + } + /// \brief Path to the ground truth poses for the entire sequence. + std::string GetTrajectoryPath() const { return trajectory_path_; } + +private: + std::vector paths_; + std::string calibration_path_; + std::string trajectory_path_; +}; + /// \class DemoFeatureMatchingPointClouds /// \brief Data class for `DemoFeatureMatchingPointClouds` contains 2 /// point cloud fragments and their respective FPFH features and L32D features. diff --git a/cpp/open3d/data/dataset/DemoDopplerICPSequence.cpp b/cpp/open3d/data/dataset/DemoDopplerICPSequence.cpp new file mode 100644 index 00000000000..5c369d3d629 --- /dev/null +++ b/cpp/open3d/data/dataset/DemoDopplerICPSequence.cpp @@ -0,0 +1,66 @@ +// ---------------------------------------------------------------------------- +// - 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 +#include +#include +#include + +#include "open3d/data/Dataset.h" +#include "open3d/utility/Logging.h" + +namespace open3d { +namespace data { + +const static DataDescriptor data_descriptor = { + Open3DDownloadsPrefix() + + "doppler-icp-data/carla-town05-curved-walls.zip", + "73a9828fb7790481168124c02398ee01"}; + +DemoDopplerICPSequence::DemoDopplerICPSequence(const std::string& data_root) + : DownloadDataset("DemoDopplerICPSequence", data_descriptor, data_root) { + for (int i = 1; i <= 10; ++i) { + std::stringstream ss; + ss << std::setw(5) << std::setfill('0') << i; + paths_.push_back(GetExtractDir() + "/xyzd_sequence/" + ss.str() + + ".xyzd"); + } + + calibration_path_ = GetExtractDir() + "/calibration.json"; + trajectory_path_ = GetExtractDir() + "/ground_truth_poses.txt"; +} + +std::string DemoDopplerICPSequence::GetPath(std::size_t index) const { + if (index > 9) { + utility::LogError( + "Invalid index. Expected index between 0 to 9 but got {}.", + index); + } + return paths_[index]; +} + +} // namespace data +} // namespace open3d diff --git a/cpp/tests/io/CMakeLists.txt b/cpp/tests/io/CMakeLists.txt index c6ed76f3e54..5f63045024e 100644 --- a/cpp/tests/io/CMakeLists.txt +++ b/cpp/tests/io/CMakeLists.txt @@ -22,6 +22,7 @@ target_sources(tests PRIVATE file_format/FilePTS.cpp file_format/FileSTL.cpp file_format/FileXYZ.cpp + file_format/FileXYZD.cpp file_format/FileXYZN.cpp file_format/FileXYZRGB.cpp ) diff --git a/cpp/tests/io/file_format/FileXYZD.cpp b/cpp/tests/io/file_format/FileXYZD.cpp new file mode 100644 index 00000000000..63db6044a76 --- /dev/null +++ b/cpp/tests/io/file_format/FileXYZD.cpp @@ -0,0 +1,37 @@ +// ---------------------------------------------------------------------------- +// - 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 "tests/Tests.h" + +namespace open3d { +namespace tests { + +TEST(FileXYZD, DISABLED_ReadPointCloudFromXYZD) { NotImplemented(); } + +TEST(FileXYZD, DISABLED_WritePointCloudToXYZD) { NotImplemented(); } + +} // namespace tests +} // namespace open3d From 5f2ec3434bb13ca2e1138188793918bad384c23e Mon Sep 17 00:00:00 2001 From: Heethesh Vhavle Date: Wed, 1 Mar 2023 19:02:23 +0530 Subject: [PATCH 07/14] Added unit test --- cpp/open3d/data/Dataset.h | 12 +- .../data/dataset/DemoDopplerICPSequence.cpp | 74 ++++++++++++ .../t/pipelines/registration/Registration.cpp | 114 ++++++++++++++++++ 3 files changed, 197 insertions(+), 3 deletions(-) diff --git a/cpp/open3d/data/Dataset.h b/cpp/open3d/data/Dataset.h index 166cf1b4cc0..b92620875e3 100644 --- a/cpp/open3d/data/Dataset.h +++ b/cpp/open3d/data/Dataset.h @@ -31,6 +31,8 @@ #include #include +#include "open3d/utility/Eigen.h" + namespace open3d { namespace data { @@ -376,11 +378,15 @@ class DemoDopplerICPSequence : public DownloadDataset { std::string GetPath(std::size_t index) const; /// \brief Path to the calibration metadata file, containing transformation /// between the vehicle and sensor frames and the time period. - std::string GetCalibrationPath() const { - return calibration_path_; - } + std::string GetCalibrationPath() const { return calibration_path_; } /// \brief Path to the ground truth poses for the entire sequence. std::string GetTrajectoryPath() const { return trajectory_path_; } + /// \brief Returns the vehicle to sensor calibration transformation and the + /// time period (in secs) between sequential point cloud scans. + bool GetCalibration(Eigen::Matrix4d& calibration, double& period) const; + /// \brief Returns a list of (timestamp, pose) representing the ground truth + /// trajectory of the sequence. + std::vector> GetTrajectory() const; private: std::vector paths_; diff --git a/cpp/open3d/data/dataset/DemoDopplerICPSequence.cpp b/cpp/open3d/data/dataset/DemoDopplerICPSequence.cpp index 5c369d3d629..9b26a2cc3a9 100644 --- a/cpp/open3d/data/dataset/DemoDopplerICPSequence.cpp +++ b/cpp/open3d/data/dataset/DemoDopplerICPSequence.cpp @@ -24,17 +24,71 @@ // IN THE SOFTWARE. // ---------------------------------------------------------------------------- +#include + +#include +#include #include #include #include #include #include "open3d/data/Dataset.h" +#include "open3d/utility/IJsonConvertible.h" #include "open3d/utility/Logging.h" namespace open3d { namespace data { +namespace { + +bool ReadJSONFromFile(const std::string& path, Json::Value& json) { + std::ifstream file(path); + if (!file.is_open()) { + utility::LogWarning("Failed to open: {}", path); + return false; + } + + Json::CharReaderBuilder builder; + builder["collectComments"] = false; + Json::String errs; + bool is_parse_successful = parseFromStream(builder, file, &json, &errs); + if (!is_parse_successful) { + utility::LogWarning("Read JSON failed: {}.", errs); + return false; + } + return true; +} + +std::vector> LoadTUMTrajectory( + const std::string& filename) { + std::vector> trajectory; + + std::ifstream file(filename); + if (!file.is_open()) { + utility::LogError("Failed to open: {}", filename); + } + + std::string line; + while (std::getline(file, line)) { + std::istringstream iss(line); + double timestamp, x, y, z, qx, qy, qz, qw; + if (!(iss >> timestamp >> x >> y >> z >> qx >> qy >> qz >> qw)) { + utility::LogError("Error parsing line: {}", line); + } + + Eigen::Affine3d transform = Eigen::Affine3d::Identity(); + transform.translate(Eigen::Vector3d(x, y, z)); + transform.rotate(Eigen::Quaterniond(qw, qx, qy, qz)); + + trajectory.emplace_back(timestamp, transform.matrix()); + } + + return trajectory; +} + +} // namespace + const static DataDescriptor data_descriptor = { Open3DDownloadsPrefix() + "doppler-icp-data/carla-town05-curved-walls.zip", @@ -62,5 +116,25 @@ std::string DemoDopplerICPSequence::GetPath(std::size_t index) const { return paths_[index]; } +bool DemoDopplerICPSequence::GetCalibration(Eigen::Matrix4d& calibration, + double& period) const { + Json::Value calibration_data; + Eigen::Matrix4d calibration_temp; + if (ReadJSONFromFile(calibration_path_, calibration_data) && + utility::IJsonConvertible::EigenMatrix4dFromJsonArray( + calibration_temp, + calibration_data["transform_vehicle_to_sensor"])) { + calibration = calibration_temp.transpose(); + period = calibration_data["period"].asDouble(); + return true; + } + return false; +} + +std::vector> +DemoDopplerICPSequence::GetTrajectory() const { + return LoadTUMTrajectory(trajectory_path_); +} + } // namespace data } // namespace open3d diff --git a/cpp/tests/t/pipelines/registration/Registration.cpp b/cpp/tests/t/pipelines/registration/Registration.cpp index ed569248df9..c49f868a4da 100644 --- a/cpp/tests/t/pipelines/registration/Registration.cpp +++ b/cpp/tests/t/pipelines/registration/Registration.cpp @@ -302,6 +302,120 @@ TEST_P(RegistrationPermuteDevices, ICPColored) { } } +core::Tensor ComputeDirectionVectors(const core::Tensor& positions) { + core::Tensor directions = core::Tensor::Empty( + positions.GetShape(), positions.GetDtype(), positions.GetDevice()); + for (int64_t i = 0; i < positions.GetLength(); ++i) { + // Compute the norm of the position vector. + core::Tensor norm = (positions[i][0] * positions[i][0] + + positions[i][1] * positions[i][1] + + positions[i][2] * positions[i][2]) + .Sqrt(); + + // If the norm is zero, set the direction vector to zero. + if (norm.Item() == 0.0) { + directions[i].Fill(0.0); + } else { + // Otherwise, compute the direction vector by dividing the position + // vector by its norm. + directions[i] = positions[i] / norm; + } + } + return directions; +} + +static std::tuple +GetDopplerICPRegistrationTestData(core::Dtype& dtype, core::Device& device) { + t::geometry::PointCloud source_tpcd, target_tpcd; + data::DemoDopplerICPSequence demo_sequence; + t::io::ReadPointCloud(demo_sequence.GetPath(0), source_tpcd); + t::io::ReadPointCloud(demo_sequence.GetPath(1), target_tpcd); + + source_tpcd.SetPointAttr( + "directions", + ComputeDirectionVectors(source_tpcd.GetPointPositions())); + + source_tpcd = source_tpcd.To(device).UniformDownSample(5); + target_tpcd = target_tpcd.To(device).UniformDownSample(5); + + Eigen::Matrix4d calibration{Eigen::Matrix4d::Identity()}; + double period{0.0}; + demo_sequence.GetCalibration(calibration, period); + + // Calibration transformation input. + const core::Tensor calibration_t = + core::eigen_converter::EigenMatrixToTensor(calibration) + .To(device, dtype); + + // Get the ground truth pose for the pair<0, 1> (on CPU:0). + auto trajectory = demo_sequence.GetTrajectory(); + const core::Tensor pose_t = + core::eigen_converter::EigenMatrixToTensor(trajectory[1].second); + ; + + const double max_correspondence_dist = 0.3; + const double normals_search_radius = 10.0; + const int normals_max_neighbors = 30; + + target_tpcd.EstimateNormals(normals_search_radius, normals_max_neighbors); + + return std::make_tuple(source_tpcd, target_tpcd, calibration_t, pose_t, + period, max_correspondence_dist); +} + +TEST_P(RegistrationPermuteDevices, ICPDoppler) { + core::Device device = GetParam(); + + for (auto dtype : {core::Float32, core::Float64}) { + // Get data and parameters. + t::geometry::PointCloud source_tpcd, target_tpcd; + // Calibration transformation input. + core::Tensor calibration_t; + // Ground truth pose. + core::Tensor pose_t; + // Time period between each point cloud scan. + double period{0.0}; + // Search radius. + double max_correspondence_dist{0.0}; + std::tie(source_tpcd, target_tpcd, calibration_t, pose_t, period, + max_correspondence_dist) = + GetDopplerICPRegistrationTestData(dtype, device); + + const double relative_fitness = 1e-6; + const double relative_rmse = 1e-6; + const int max_iterations = 20; + + t_reg::TransformationEstimationForDopplerICP estimation_dicp; + estimation_dicp.period_ = period; + estimation_dicp.transform_vehicle_to_sensor_ = calibration_t; + + // DopplerICP - Tensor. + t_reg::RegistrationResult reg_doppler_t = t_reg::ICP( + source_tpcd, target_tpcd, max_correspondence_dist, + core::Tensor::Eye(4, dtype, device), estimation_dicp, + t_reg::ICPConvergenceCriteria(relative_fitness, relative_rmse, + max_iterations), + -1.0); + + core::Tensor estimated_pose = + t::pipelines::kernel::TransformationToPose( + reg_doppler_t.transformation_.Inverse()); + core::Tensor expected_pose = + t::pipelines::kernel::TransformationToPose(pose_t); + + const double pose_diff = + (expected_pose - estimated_pose).Abs().Sum({0}).Item(); + + EXPECT_NEAR(reg_doppler_t.fitness_ - 0.9, 0.0, 0.05); + EXPECT_NEAR(pose_diff - 0.017, 0.0, 0.005); + } +} + TEST_P(RegistrationPermuteDevices, RobustKernel) { double scaling_parameter = 1.0; double shape_parameter = 1.0; From 8f17eccf672686ee51fe1ecf394669f4972f3b2f Mon Sep 17 00:00:00 2001 From: Heethesh Vhavle Date: Wed, 1 Mar 2023 19:37:55 +0530 Subject: [PATCH 08/14] Add benchmarks --- .../t/pipelines/registration/Registration.cpp | 154 ++++++++++++++++-- cpp/open3d/data/Dataset.h | 5 +- .../t/pipelines/registration/Registration.cpp | 1 - 3 files changed, 143 insertions(+), 17 deletions(-) diff --git a/cpp/benchmarks/t/pipelines/registration/Registration.cpp b/cpp/benchmarks/t/pipelines/registration/Registration.cpp index 4d97429b2ff..ffc0ec2e5b2 100644 --- a/cpp/benchmarks/t/pipelines/registration/Registration.cpp +++ b/cpp/benchmarks/t/pipelines/registration/Registration.cpp @@ -29,6 +29,7 @@ #include #include "open3d/core/CUDAUtils.h" +#include "open3d/core/EigenConverter.h" #include "open3d/core/nns/NearestNeighborSearch.h" #include "open3d/data/Dataset.h" #include "open3d/t/io/PointCloudIO.h" @@ -50,7 +51,11 @@ static const double voxel_downsampling_factor = 0.02; // NNS parameter. static const double max_correspondence_distance = 0.05; -// Initial transformation guess for registation. +// Normal estimation parameters. +static const double normals_search_radius = 10.0; +static const int normals_max_neighbors = 30; + +// Initial transformation guess for registration. static const std::vector initial_transform_flat{ 0.862, 0.011, -0.507, 0.5, -0.139, 0.967, -0.215, 0.7, 0.487, 0.255, 0.835, -1.4, 0.0, 0.0, 0.0, 1.0}; @@ -137,36 +142,155 @@ static void BenchmarkICP(benchmark::State& state, } } -#define ENUM_ICP_METHOD_DEVICE(METHOD_NAME, TRANSFORMATION_TYPE, DEVICE) \ - BENCHMARK_CAPTURE(BenchmarkICP, DEVICE METHOD_NAME##_Float32, \ - core::Device(DEVICE), core::Float32, \ - TRANSFORMATION_TYPE) \ - ->Unit(benchmark::kMillisecond); \ - BENCHMARK_CAPTURE(BenchmarkICP, DEVICE METHOD_NAME##_Float64, \ - core::Device(DEVICE), core::Float64, \ - TRANSFORMATION_TYPE) \ +core::Tensor ComputeDirectionVectors(const core::Tensor& positions) { + core::Tensor directions = core::Tensor::Empty( + positions.GetShape(), positions.GetDtype(), positions.GetDevice()); + for (int64_t i = 0; i < positions.GetLength(); ++i) { + // Compute the norm of the position vector. + core::Tensor norm = (positions[i][0] * positions[i][0] + + positions[i][1] * positions[i][1] + + positions[i][2] * positions[i][2]) + .Sqrt(); + + // If the norm is zero, set the direction vector to zero. + if (norm.Item() == 0.0) { + directions[i].Fill(0.0); + } else { + // Otherwise, compute the direction vector by dividing the position + // vector by its norm. + directions[i] = positions[i] / norm; + } + } + return directions; +} + +static std::tuple +LoadTensorDopplerPointCloudFromFile( + const std::string& source_pointcloud_filename, + const std::string& target_pointcloud_filename, + const double voxel_downsample_factor, + const core::Dtype& dtype, + const core::Device& device) { + geometry::PointCloud source, target; + + io::ReadPointCloud(source_pointcloud_filename, source, + {"auto", false, false, true}); + io::ReadPointCloud(target_pointcloud_filename, target, + {"auto", false, false, true}); + + source.SetPointAttr("directions", + ComputeDirectionVectors(source.GetPointPositions())); + + source = source.To(device); + target = target.To(device); + + // Eliminates the case of impractical values (including negative). + if (voxel_downsample_factor > 0.001) { + source = source.VoxelDownSample(voxel_downsample_factor); + target = target.VoxelDownSample(voxel_downsample_factor); + } else { + utility::LogWarning( + "VoxelDownsample: Impractical voxel size [< 0.001], skipping " + "downsampling."); + } + + source.SetPointPositions(source.GetPointPositions().To(dtype)); + source.SetPointAttr("dopplers", source.GetPointAttr("dopplers").To(dtype)); + source.SetPointAttr("directions", + source.GetPointAttr("directions").To(dtype)); + + target.SetPointPositions(target.GetPointPositions().To(dtype)); + target.EstimateNormals(normals_search_radius, normals_max_neighbors); + + return std::make_tuple(source, target); +} + +static void BenchmarkDopplerICP(benchmark::State& state, + const core::Device& device, + const core::Dtype& dtype, + const TransformationEstimationType& type) { + utility::SetVerbosityLevel(utility::VerbosityLevel::Error); + data::DemoDopplerICPSequence demo_sequence; + geometry::PointCloud source, target; + std::tie(source, target) = LoadTensorDopplerPointCloudFromFile( + demo_sequence.GetPath(0), demo_sequence.GetPath(1), + voxel_downsampling_factor, dtype, device); + + Eigen::Matrix4d calibration{Eigen::Matrix4d::Identity()}; + double period{0.0}; + demo_sequence.GetCalibration(calibration, period); + + const core::Tensor calibration_t = + core::eigen_converter::EigenMatrixToTensor(calibration) + .To(device, dtype); + + TransformationEstimationForDopplerICP estimation_dicp; + estimation_dicp.period_ = period; + estimation_dicp.transform_vehicle_to_sensor_ = calibration_t; + + core::Tensor init_trans = core::Tensor::Eye(4, dtype, device); + RegistrationResult reg_result(init_trans); + + // Warm up. + reg_result = ICP(source, target, max_correspondence_distance, init_trans, + estimation_dicp, + ICPConvergenceCriteria(relative_fitness, relative_rmse, + max_iterations)); + + for (auto _ : state) { + reg_result = ICP(source, target, max_correspondence_distance, + init_trans, estimation_dicp, + ICPConvergenceCriteria(relative_fitness, relative_rmse, + max_iterations)); + core::cuda::Synchronize(device); + } +} + +#define ENUM_ICP_METHOD_DEVICE(BENCHMARK_FUNCTION, METHOD_NAME, \ + TRANSFORMATION_TYPE, DEVICE) \ + BENCHMARK_CAPTURE(BENCHMARK_FUNCTION, DEVICE METHOD_NAME##_Float32, \ + core::Device(DEVICE), core::Float32, \ + TRANSFORMATION_TYPE) \ + ->Unit(benchmark::kMillisecond); \ + BENCHMARK_CAPTURE(BENCHMARK_FUNCTION, DEVICE METHOD_NAME##_Float64, \ + core::Device(DEVICE), core::Float64, \ + TRANSFORMATION_TYPE) \ ->Unit(benchmark::kMillisecond); -ENUM_ICP_METHOD_DEVICE(PointToPoint, +ENUM_ICP_METHOD_DEVICE(BenchmarkICP, + PointToPoint, TransformationEstimationType::PointToPoint, "CPU:0") -ENUM_ICP_METHOD_DEVICE(PointToPlane, +ENUM_ICP_METHOD_DEVICE(BenchmarkICP, + PointToPlane, TransformationEstimationType::PointToPlane, "CPU:0") -ENUM_ICP_METHOD_DEVICE(ColoredICP, +ENUM_ICP_METHOD_DEVICE(BenchmarkICP, + ColoredICP, TransformationEstimationType::ColoredICP, "CPU:0") +ENUM_ICP_METHOD_DEVICE(BenchmarkDopplerICP, + DopplerICP, + TransformationEstimationType::DopplerICP, + "CPU:0") #ifdef BUILD_CUDA_MODULE -ENUM_ICP_METHOD_DEVICE(PointToPoint, +ENUM_ICP_METHOD_DEVICE(BenchmarkICP, + PointToPoint, TransformationEstimationType::PointToPoint, "CUDA:0") -ENUM_ICP_METHOD_DEVICE(PointToPlane, +ENUM_ICP_METHOD_DEVICE(BenchmarkICP, + PointToPlane, TransformationEstimationType::PointToPlane, "CUDA:0") -ENUM_ICP_METHOD_DEVICE(ColoredICP, +ENUM_ICP_METHOD_DEVICE(BenchmarkICP, + ColoredICP, TransformationEstimationType::ColoredICP, "CUDA:0") +ENUM_ICP_METHOD_DEVICE(BenchmarkDopplerICP, + DopplerICP, + TransformationEstimationType::DopplerICP, + "CUDA:0") #endif } // namespace registration diff --git a/cpp/open3d/data/Dataset.h b/cpp/open3d/data/Dataset.h index b92620875e3..8cc505584c8 100644 --- a/cpp/open3d/data/Dataset.h +++ b/cpp/open3d/data/Dataset.h @@ -366,7 +366,7 @@ class DemoCustomVisualization : public DownloadDataset { /// \class DemoDopplerICPSequence /// \brief Data class for `DemoDopplerICPSequence` contains an example sequence -/// of 100 point cloud with Doppler velocity channel and corresponding ground +/// of 100 point clouds with Doppler velocity channel and corresponding ground /// truth poses. The sequence was generated using the CARLA simulator. class DemoDopplerICPSequence : public DownloadDataset { public: @@ -389,8 +389,11 @@ class DemoDopplerICPSequence : public DownloadDataset { std::vector> GetTrajectory() const; private: + /// List of paths to the point clouds. std::vector paths_; + /// Path to the calibration JSON file. std::string calibration_path_; + /// Path to the TUM ground truth trajectory text file. std::string trajectory_path_; }; diff --git a/cpp/tests/t/pipelines/registration/Registration.cpp b/cpp/tests/t/pipelines/registration/Registration.cpp index c49f868a4da..41dbde38df4 100644 --- a/cpp/tests/t/pipelines/registration/Registration.cpp +++ b/cpp/tests/t/pipelines/registration/Registration.cpp @@ -356,7 +356,6 @@ GetDopplerICPRegistrationTestData(core::Dtype& dtype, core::Device& device) { auto trajectory = demo_sequence.GetTrajectory(); const core::Tensor pose_t = core::eigen_converter::EigenMatrixToTensor(trajectory[1].second); - ; const double max_correspondence_dist = 0.3; const double normals_search_radius = 10.0; From 0273f037c1fe4752c1d31b6d8ed774ddc3d6b73d Mon Sep 17 00:00:00 2001 From: Heethesh Vhavle Date: Wed, 1 Mar 2023 21:05:41 +0530 Subject: [PATCH 09/14] Add cpp example --- cpp/open3d/data/Dataset.h | 2 +- .../data/dataset/DemoDopplerICPSequence.cpp | 6 +- cpp/pybind/data/dataset.cpp | 35 +++ examples/cpp/CMakeLists.txt | 1 + examples/cpp/RegistrationDopplerICP.cpp | 209 ++++++++++++++++++ 5 files changed, 249 insertions(+), 4 deletions(-) create mode 100644 examples/cpp/RegistrationDopplerICP.cpp diff --git a/cpp/open3d/data/Dataset.h b/cpp/open3d/data/Dataset.h index 8cc505584c8..9263c47dc1a 100644 --- a/cpp/open3d/data/Dataset.h +++ b/cpp/open3d/data/Dataset.h @@ -372,7 +372,7 @@ class DemoDopplerICPSequence : public DownloadDataset { public: DemoDopplerICPSequence(const std::string& data_root = ""); - /// \brief Returns list of the first 10 point cloud paths in the sequence. + /// \brief Returns the list of the point cloud paths in the sequence. std::vector GetPaths() const { return paths_; } /// \brief Path to the point cloud at index. std::string GetPath(std::size_t index) const; diff --git a/cpp/open3d/data/dataset/DemoDopplerICPSequence.cpp b/cpp/open3d/data/dataset/DemoDopplerICPSequence.cpp index 9b26a2cc3a9..357da565b25 100644 --- a/cpp/open3d/data/dataset/DemoDopplerICPSequence.cpp +++ b/cpp/open3d/data/dataset/DemoDopplerICPSequence.cpp @@ -96,7 +96,7 @@ const static DataDescriptor data_descriptor = { DemoDopplerICPSequence::DemoDopplerICPSequence(const std::string& data_root) : DownloadDataset("DemoDopplerICPSequence", data_descriptor, data_root) { - for (int i = 1; i <= 10; ++i) { + for (int i = 1; i <= 100; ++i) { std::stringstream ss; ss << std::setw(5) << std::setfill('0') << i; paths_.push_back(GetExtractDir() + "/xyzd_sequence/" + ss.str() + @@ -108,9 +108,9 @@ DemoDopplerICPSequence::DemoDopplerICPSequence(const std::string& data_root) } std::string DemoDopplerICPSequence::GetPath(std::size_t index) const { - if (index > 9) { + if (index > 99) { utility::LogError( - "Invalid index. Expected index between 0 to 9 but got {}.", + "Invalid index. Expected index between 0 to 99 but got {}.", index); } return paths_[index]; diff --git a/cpp/pybind/data/dataset.cpp b/cpp/pybind/data/dataset.cpp index 25659fd424c..3a265c94a38 100644 --- a/cpp/pybind/data/dataset.cpp +++ b/cpp/pybind/data/dataset.cpp @@ -186,6 +186,40 @@ void pybind_demo_crop_pointcloud(py::module& m) { "cropped_json_path"); } +void pybind_demo_doppler_icp_sequence(py::module& m) { + // open3d.data.DemoDopplerICPSequence + py::class_, + std::shared_ptr, DownloadDataset> + demo_doppler_icp_sequence( + m, "DemoDopplerICPSequence", + "Data class for `DemoDopplerICPSequence` contains an " + "example sequence of 100 point clouds with Doppler " + "velocity channel and corresponding ground truth poses. " + "The sequence was generated using the CARLA simulator."); + demo_doppler_icp_sequence + .def(py::init(), "data_root"_a = "") + .def_property_readonly( + "paths", &DemoDopplerICPSequence::GetPaths, + "Returns list of the point cloud paths in the sequence.") + .def_property_readonly( + "calibration_path", + &DemoDopplerICPSequence::GetCalibrationPath, + "Path to the calibration metadata file, containing " + "transformation between the vehicle and sensor frames and " + "the time period.") + .def_property_readonly( + "trajectory_path", + &DemoDopplerICPSequence::GetTrajectoryPath, + "Path to the ground truth poses for the entire sequence."); + docstring::ClassMethodDocInject(m, "DemoDopplerICPSequence", + "point_cloud_path"); + docstring::ClassMethodDocInject(m, "DemoDopplerICPSequence", + "calibration_path"); + docstring::ClassMethodDocInject(m, "DemoDopplerICPSequence", + "trajectory_path"); +} + void pybind_demo_feature_matching_point_clouds(py::module& m) { // open3d.data.DemoFeatureMatchingPointClouds py::class_ +#include +#include + +#include "open3d/Open3D.h" + +using namespace open3d; + +namespace { + +// Parameters for Doppler ICP registration. +constexpr double kLambdaDoppler{0.01}; +constexpr bool kRejectDynamicOutliers{false}; +constexpr double kDopplerOutlierThreshold{2.0}; +constexpr std::size_t kOutlierRejectionMinIteration{2}; +constexpr std::size_t kGeometricRobustLossMinIteration{0}; +constexpr std::size_t kDopplerRobustLossMinIteration{2}; +constexpr double kTukeyLossScale{0.5}; +constexpr double kNormalsSearchRadius{10.0}; +constexpr int kNormalsMaxNeighbors{30}; +constexpr double kMaxCorrespondenceDist{0.3}; +constexpr std::size_t kUniformDownsampleFactor{2}; +constexpr double kFitnessEpsilon{1e-6}; +constexpr std::size_t kMaxIters{200}; + +} // namespace + +void VisualizeRegistration(const geometry::PointCloud &source, + const geometry::PointCloud &target, + const Eigen::Matrix4d &Transformation) { + std::shared_ptr source_transformed_ptr( + new geometry::PointCloud); + std::shared_ptr target_ptr(new geometry::PointCloud); + *source_transformed_ptr = source; + *target_ptr = target; + source_transformed_ptr->Transform(Transformation); + visualization::DrawGeometries({source_transformed_ptr, target_ptr}, + "Registration result"); +} + +core::Tensor ComputeDirectionVectors(const core::Tensor &positions) { + utility::LogDebug("ComputeDirectionVectors"); + + core::Tensor directions = core::Tensor::Empty( + positions.GetShape(), positions.GetDtype(), positions.GetDevice()); + + for (int64_t i = 0; i < positions.GetLength(); ++i) { + // Compute the norm of the position vector. + core::Tensor norm = (positions[i][0] * positions[i][0] + + positions[i][1] * positions[i][1] + + positions[i][2] * positions[i][2]) + .Sqrt(); + + // If the norm is zero, set the direction vector to zero. + if (norm.Item() == 0.0) { + directions[i].Fill(0.0); + } else { + // Otherwise, compute the direction vector by dividing the position + // vector by its norm. + directions[i] = positions[i] / norm; + } + } + + return directions; +} + +void PrintHelp() { + PrintOpen3DVersion(); + // clang-format off + utility::LogInfo("Usage:"); + utility::LogInfo(" > RegistrationDopplerICP source_idx target_idx [--visualize --cuda]"); + // clang-format on + utility::LogInfo(""); +} + +int main(int argc, char *argv[]) { + utility::SetVerbosityLevel(utility::VerbosityLevel::Debug); + + if (argc < 3 || + utility::ProgramOptionExistsAny(argc, argv, {"-h", "--help"})) { + PrintHelp(); + return 1; + } + + bool visualize = false; + if (utility::ProgramOptionExists(argc, argv, "--visualize")) { + visualize = true; + } + + // Device. + core::Device device("CPU:0"); + if (utility::ProgramOptionExistsAny(argc, argv, {"--cuda"})) { + device = core::Device("CUDA:0"); + } + utility::LogInfo("Selected device: {}", device.ToString()); + + data::DemoDopplerICPSequence demo_sequence; + + t::geometry::PointCloud source; + t::geometry::PointCloud target; + + // Read point clouds, t::io::ReadPointCloud copies the pointcloud to CPU. + t::io::ReadPointCloud(demo_sequence.GetPath(std::stoi(argv[1])), source, + {"auto", false, false, true}); + t::io::ReadPointCloud(demo_sequence.GetPath(std::stoi(argv[2])), target, + {"auto", false, false, true}); + + // Set direction vectors. + source.SetPointAttr("directions", + ComputeDirectionVectors(source.GetPointPositions())); + + source = source.To(device).UniformDownSample(kUniformDownsampleFactor); + target = target.To(device).UniformDownSample(kUniformDownsampleFactor); + + target.EstimateNormals(kNormalsSearchRadius, kNormalsMaxNeighbors); + + Eigen::Matrix4d calibration{Eigen::Matrix4d::Identity()}; + double period{0.0}; + demo_sequence.GetCalibration(calibration, period); + + // Vehicle to sensor frame calibration (T_V_S). + const core::Tensor calibration_vehicle_to_sensor = + core::eigen_converter::EigenMatrixToTensor(calibration).To(device); + + // Initial transform from source to target, to initialize ICP. + core::Tensor initial_transform = + core::Tensor::Eye(4, core::Dtype::Float64, device); + + auto kernel = t::pipelines::registration::RobustKernel( + t::pipelines::registration::RobustKernelMethod::TukeyLoss, + kTukeyLossScale); + + t::pipelines::registration::RegistrationResult result = + t::pipelines::registration::ICP( + source, target, kMaxCorrespondenceDist, initial_transform, + t::pipelines::registration:: + TransformationEstimationForDopplerICP( + period, kLambdaDoppler, + kRejectDynamicOutliers, + kDopplerOutlierThreshold, + kOutlierRejectionMinIteration, + kGeometricRobustLossMinIteration, + kDopplerRobustLossMinIteration, kernel, + kernel, calibration_vehicle_to_sensor), + t::pipelines::registration::ICPConvergenceCriteria( + kFitnessEpsilon, kFitnessEpsilon, kMaxIters)); + + if (visualize) { + VisualizeRegistration(source.ToLegacy(), target.ToLegacy(), + core::eigen_converter::TensorToEigenMatrixXd( + result.transformation_)); + } + + // Get the ground truth pose. + auto trajectory = demo_sequence.GetTrajectory(); + const core::Tensor pose_source = core::eigen_converter::EigenMatrixToTensor( + trajectory[std::stoi(argv[1])].second); + const core::Tensor pose_target = core::eigen_converter::EigenMatrixToTensor( + trajectory[std::stoi(argv[2])].second); + const core::Tensor target_to_source = + pose_target.Inverse().Matmul(pose_source); + + utility::LogInfo( + "Estimated pose [rx ry rz tx ty tz]: \n{}", + t::pipelines::kernel::TransformationToPose(result.transformation_) + .ToString(false)); + + utility::LogInfo( + "Ground truth pose [rx ry rz tx ty tz]: \n{}", + t::pipelines::kernel::TransformationToPose(target_to_source) + .ToString(false)); + + return EXIT_SUCCESS; +} From 7ffff61153e03853c63e53f5fcc1cbab1f43315f Mon Sep 17 00:00:00 2001 From: Heethesh Vhavle Date: Wed, 1 Mar 2023 23:59:09 +0530 Subject: [PATCH 10/14] update license format --- .../data/dataset/DemoDopplerICPSequence.cpp | 23 ++----------------- cpp/tests/io/file_format/FileXYZD.cpp | 23 ++----------------- examples/cpp/RegistrationDopplerICP.cpp | 23 ++----------------- 3 files changed, 6 insertions(+), 63 deletions(-) diff --git a/cpp/open3d/data/dataset/DemoDopplerICPSequence.cpp b/cpp/open3d/data/dataset/DemoDopplerICPSequence.cpp index 357da565b25..f4f29a17645 100644 --- a/cpp/open3d/data/dataset/DemoDopplerICPSequence.cpp +++ b/cpp/open3d/data/dataset/DemoDopplerICPSequence.cpp @@ -1,27 +1,8 @@ // ---------------------------------------------------------------------------- // - 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. +// Copyright (c) 2018-2023 www.open3d.org +// SPDX-License-Identifier: MIT // ---------------------------------------------------------------------------- #include diff --git a/cpp/tests/io/file_format/FileXYZD.cpp b/cpp/tests/io/file_format/FileXYZD.cpp index 63db6044a76..11df66e533b 100644 --- a/cpp/tests/io/file_format/FileXYZD.cpp +++ b/cpp/tests/io/file_format/FileXYZD.cpp @@ -1,27 +1,8 @@ // ---------------------------------------------------------------------------- // - 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. +// Copyright (c) 2018-2023 www.open3d.org +// SPDX-License-Identifier: MIT // ---------------------------------------------------------------------------- #include "tests/Tests.h" diff --git a/examples/cpp/RegistrationDopplerICP.cpp b/examples/cpp/RegistrationDopplerICP.cpp index e185e0f0456..331bd8dfd56 100644 --- a/examples/cpp/RegistrationDopplerICP.cpp +++ b/examples/cpp/RegistrationDopplerICP.cpp @@ -1,27 +1,8 @@ // ---------------------------------------------------------------------------- // - 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. +// Copyright (c) 2018-2023 www.open3d.org +// SPDX-License-Identifier: MIT // ---------------------------------------------------------------------------- // Program that runs the tensor version of Doppler ICP registration. From 23a4542121ba61bc617950144f18b5f3b75e6614 Mon Sep 17 00:00:00 2001 From: Heethesh Vhavle Date: Thu, 2 Mar 2023 13:11:42 +0530 Subject: [PATCH 11/14] Fix python import and add Python example for DICP --- cpp/pybind/data/dataset.cpp | 3 +- .../pipelines/doppler_icp_registration.py | 253 ++++++++++++++++++ 2 files changed, 254 insertions(+), 2 deletions(-) create mode 100644 examples/python/pipelines/doppler_icp_registration.py diff --git a/cpp/pybind/data/dataset.cpp b/cpp/pybind/data/dataset.cpp index 3259ea5559f..f6e39d90163 100644 --- a/cpp/pybind/data/dataset.cpp +++ b/cpp/pybind/data/dataset.cpp @@ -193,8 +193,7 @@ void pybind_demo_doppler_icp_sequence(py::module& m) { "trajectory_path", &DemoDopplerICPSequence::GetTrajectoryPath, "Path to the ground truth poses for the entire sequence."); - docstring::ClassMethodDocInject(m, "DemoDopplerICPSequence", - "point_cloud_path"); + docstring::ClassMethodDocInject(m, "DemoDopplerICPSequence", "paths"); docstring::ClassMethodDocInject(m, "DemoDopplerICPSequence", "calibration_path"); docstring::ClassMethodDocInject(m, "DemoDopplerICPSequence", diff --git a/examples/python/pipelines/doppler_icp_registration.py b/examples/python/pipelines/doppler_icp_registration.py new file mode 100644 index 00000000000..5133d01582d --- /dev/null +++ b/examples/python/pipelines/doppler_icp_registration.py @@ -0,0 +1,253 @@ +# ---------------------------------------------------------------------------- +# - 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. +# ---------------------------------------------------------------------------- +"""Example script to run Doppler ICP point cloud registration. + +This script runs Doppler ICP and point-to-plane ICP on DemoDopplerICPSequence. + +This is the implementation of the following paper: +B. Hexsel, H. Vhavle, Y. Chen, +DICP: Doppler Iterative Closest Point Algorithm, RSS 2022. + +Usage: +python doppler_icp_registration.py [-h] \ + --source SOURCE --target TARGET [--device {cpu,cuda}] +""" + +import argparse +import json +import os + +import numpy as np +import open3d as o3d +import transformations as tf + + +def translation_quaternion_to_transform(translation, + quaternion, + inverse=False, + quat_xyzw=False): + """Converts translation and WXYZ quaternion to a transformation matrix. + + Args: + translation: (3,) ndarray representing the translation vector. + quaternion: (4,) ndarray representing the quaternion. + inverse: If True, returns the inverse transformation. + quat_xyzw: If True, this indicates that quaternion is in XYZW format. + + Returns: + (4, 4) ndarray representing the transformation matrix. + """ + if quat_xyzw: + quaternion = np.roll(quaternion, 1) + R = tf.quaternion_matrix(quaternion) # [w, x, y, z] + T = tf.translation_matrix(translation) # [x, y, z] + transform = tf.concatenate_matrices(T, R) + return np.linalg.inv(transform) if inverse else transform + + +def load_tum_file(filename): + """Loads poses in TUM RGBD format: [timestamp, x, y, z, qx, qy, qz, qw]. + + Args: + filename (string): Path to the TUM poses file. + + Returns: + A tuple containing an array of 4x4 poses and timestamps. + """ + # Load the TUM text file. + data = np.loadtxt(filename, delimiter=' ') + print('Loaded %d poses from %s (%.2f secs)' % + (len(data), os.path.basename(filename), data[-1][0] - data[0][0])) + + # Parse timestamps and poses. + timestamps = data[:, 0] + poses = np.array([ + translation_quaternion_to_transform(tq[:3], tq[3:], quat_xyzw=True) + for tq in data[:, 1:] + ]) + return poses, timestamps + + +def get_calibration(demo_sequence): + """Returns the vehicle to sensor calibration transformation and the time + period (in secs) between sequential point cloud scans. + + Args: + demo_sequence (DemoDopplerICPSequence): Doppler ICP dataset. + + Returns: + A tuple of 4x4 array representing the transform, and the period. + """ + with open(demo_sequence.calibration_path) as f: + data = json.load(f) + + transform_vehicle_to_sensor = np.array( + data['transform_vehicle_to_sensor']).reshape(4, 4) + period = data['period'] + + return transform_vehicle_to_sensor, period + + +def get_trajectory(demo_sequence): + """Returns the ground truth trajectory of the dataset. + + Args: + demo_sequence (DemoDopplerICPSequence): Doppler ICP dataset. + + Returns: + An array of 4x4 poses for this sequence. + """ + return load_tum_file(demo_sequence.trajectory_path)[0] + + +def get_ground_truth_pose(demo_sequence, source_idx, target_idx): + """Returns the ground truth poses from the dataset. + + Args: + demo_sequence (DemoDopplerICPSequence): Doppler ICP dataset. + source_idx (int): Index of the source point cloud pose. + target_idx (int): Index of the target point cloud pose. + + Returns: + 4x4 array representing the transformation between target and source. + """ + poses = get_trajectory(demo_sequence) + return np.linalg.inv(poses[target_idx]) @ poses[source_idx] + + +def run_doppler_icp(args): + """Runs Doppler ICP on a given pair of point clouds. + + Args: + args: Command line arguments. + """ + # Setup data type and device. + dtype = o3d.core.float32 + device = o3d.core.Device('CUDA:0' if args.device == 'cuda' else 'CPU:0') + + # Load the point clouds. + demo_sequence = o3d.data.DemoDopplerICPSequence() + source = o3d.t.io.read_point_cloud(demo_sequence.paths[args.source]) + target = o3d.t.io.read_point_cloud(demo_sequence.paths[args.target]) + + # Load the calibration parameters. + transform_vehicle_to_sensor, period = get_calibration(demo_sequence) + + # Downsample the pointcloud. + source_in_S = source.uniform_down_sample(5) + target_in_S = target.uniform_down_sample(5) + + # Transform the Open3D point cloud from sensor to vehicle frame. + source_in_V = source_in_S.to(device).transform(transform_vehicle_to_sensor) + target_in_V = target_in_S.to(device).transform(transform_vehicle_to_sensor) + + # Move tensor to device. + init_transform = o3d.core.Tensor(np.eye(4), device=device) + transform_vehicle_to_sensor = o3d.core.Tensor(transform_vehicle_to_sensor, + device=device) + + # Compute normals for target. + target_in_V.estimate_normals(radius=10.0, max_nn=30) + + # Compute direction vectors on source point cloud frame in sensor frame. + directions = tf.unit_vector(source_in_S.point.positions.numpy(), axis=1) + source_in_V.point['directions'] = o3d.core.Tensor(directions, dtype, device) + + # Setup robust kernels. + o3d_reg = o3d.t.pipelines.registration + kernel = o3d_reg.robust_kernel.RobustKernel(o3d_reg.robust_kernel.TukeyLoss, + scaling_parameter=0.5) + + # Setup convergence criteria. + criteria = o3d_reg.ICPConvergenceCriteria(relative_fitness=1e-6, + relative_rmse=1e-6, + max_iteration=200) + + # Setup transformation estimator. + estimator_p2l = o3d_reg.TransformationEstimationPointToPlane(kernel) + estimator_dicp = o3d_reg.TransformationEstimationForDopplerICP( + period=period, + lambda_doppler=0.01, + reject_dynamic_outliers=False, + doppler_outlier_threshold=2.0, + outlier_rejection_min_iteration=2, + geometric_robust_loss_min_iteration=0, + doppler_robust_loss_min_iteration=2, + goemetric_kernel=kernel, + doppler_kernel=kernel, + transform_vehicle_to_sensor=transform_vehicle_to_sensor) + + # Run Doppler ICP and point-to-plane ICP registration for comparison. + max_neighbor_distance = 0.3 + results = [ + o3d_reg.icp(source_in_V, target_in_V, max_neighbor_distance, + init_transform, estimator, criteria) + for estimator in [estimator_p2l, estimator_dicp] + ] + + # Display the poses. + np.set_printoptions(suppress=True, precision=4) + print('Estimated pose from Point-to-Plane ICP [%s iterations]:' % + results[0].num_iterations) + print(results[0].transformation.numpy()) + + print('\nEstimated pose from Doppler ICP [%s iterations]:' % + results[1].num_iterations) + print(results[1].transformation.numpy()) + + print('\nGround truth pose:') + print(get_ground_truth_pose(demo_sequence, args.source, args.target)) + + +def parse_args(): + """Parses the command line arguments. + + Returns: + The parsed command line arguments. + """ + parser = argparse.ArgumentParser() + parser.add_argument('--source', + '-s', + type=int, + required=True, + help='Source point cloud index') + parser.add_argument('--target', + '-t', + type=int, + required=True, + help='Target point cloud index') + parser.add_argument('--device', + '-d', + default='cpu', + help='Device backend for the tensor', + choices=['cpu', 'cuda']) + + return parser.parse_args() + + +if __name__ == '__main__': + args = parse_args() + run_doppler_icp(args) From 6951a57a8788a70fb4ebed90cc63259fe88d0f60 Mon Sep 17 00:00:00 2001 From: Heethesh Vhavle Date: Thu, 2 Mar 2023 13:44:02 +0530 Subject: [PATCH 12/14] Minor updates --- .../t/pipelines/kernel/RegistrationCPU.cpp | 8 +++---- .../t/pipelines/kernel/RegistrationCUDA.cu | 8 +++---- .../t/pipelines/kernel/RegistrationImpl.h | 18 +++++---------- .../pipelines/doppler_icp_registration.py | 23 ++----------------- 4 files changed, 16 insertions(+), 41 deletions(-) diff --git a/cpp/open3d/t/pipelines/kernel/RegistrationCPU.cpp b/cpp/open3d/t/pipelines/kernel/RegistrationCPU.cpp index 7caedb15eb6..d2ea8b584ab 100644 --- a/cpp/open3d/t/pipelines/kernel/RegistrationCPU.cpp +++ b/cpp/open3d/t/pipelines/kernel/RegistrationCPU.cpp @@ -260,7 +260,7 @@ static void ComputePoseDopplerICPKernelCPU( const scalar_t *R_S_to_V, const scalar_t *r_v_to_s_in_V, const scalar_t *v_s_in_S, - const bool prune_correspondences, + const bool reject_dynamic_outliers, const scalar_t doppler_outlier_threshold, const scalar_t sqrt_lambda_geometric, const scalar_t sqrt_lambda_doppler, @@ -296,7 +296,7 @@ static void ComputePoseDopplerICPKernelCPU( source_dopplers_ptr, source_directions_ptr, target_points_ptr, target_normals_ptr, correspondence_indices, R_S_to_V, r_v_to_s_in_V, - v_s_in_S, prune_correspondences, + v_s_in_S, reject_dynamic_outliers, doppler_outlier_threshold, sqrt_lambda_geometric, sqrt_lambda_doppler, sqrt_lambda_doppler_by_dt, J_G, J_D, r_G, r_D); @@ -365,7 +365,7 @@ void ComputePoseDopplerICPCPU( const core::Tensor &w_v_in_V, const core::Tensor &v_v_in_V, const double period, - const bool prune_correspondences, + const bool reject_dynamic_outliers, const double doppler_outlier_threshold, const registration::RobustKernel &kernel_geometric, const registration::RobustKernel &kernel_doppler, @@ -403,7 +403,7 @@ void ComputePoseDopplerICPCPU( R_S_to_V.GetDataPtr(), r_v_to_s_in_V.GetDataPtr(), v_s_in_S.GetDataPtr(), - prune_correspondences, + reject_dynamic_outliers, static_cast(doppler_outlier_threshold), sqrt_lambda_geometric, sqrt_lambda_doppler, sqrt_lambda_doppler_by_dt, n, diff --git a/cpp/open3d/t/pipelines/kernel/RegistrationCUDA.cu b/cpp/open3d/t/pipelines/kernel/RegistrationCUDA.cu index 22a8d5274f5..20532dde857 100644 --- a/cpp/open3d/t/pipelines/kernel/RegistrationCUDA.cu +++ b/cpp/open3d/t/pipelines/kernel/RegistrationCUDA.cu @@ -240,7 +240,7 @@ __global__ void ComputePoseDopplerICPKernelCUDA( const scalar_t *R_S_to_V, const scalar_t *r_v_to_s_in_V, const scalar_t *v_s_in_S, - const bool prune_correspondences, + const bool reject_dynamic_outliers, const scalar_t doppler_outlier_threshold, const scalar_t sqrt_lambda_geometric, const scalar_t sqrt_lambda_doppler, @@ -265,7 +265,7 @@ __global__ void ComputePoseDopplerICPKernelCUDA( workload_idx, source_points_ptr, source_dopplers_ptr, source_directions_ptr, target_points_ptr, target_normals_ptr, correspondence_indices, R_S_to_V, r_v_to_s_in_V, v_s_in_S, - prune_correspondences, doppler_outlier_threshold, + reject_dynamic_outliers, doppler_outlier_threshold, sqrt_lambda_geometric, sqrt_lambda_doppler, sqrt_lambda_doppler_by_dt, J_G, J_D, r_G, r_D); @@ -327,7 +327,7 @@ void ComputePoseDopplerICPCUDA( const core::Tensor &w_v_in_V, const core::Tensor &v_v_in_V, const double period, - const bool prune_correspondences, + const bool reject_dynamic_outliers, const double doppler_outlier_threshold, const registration::RobustKernel &kernel_geometric, const registration::RobustKernel &kernel_doppler, @@ -371,7 +371,7 @@ void ComputePoseDopplerICPCUDA( R_S_to_V.GetDataPtr(), r_v_to_s_in_V.GetDataPtr(), v_s_in_S.GetDataPtr(), - prune_correspondences, + reject_dynamic_outliers, static_cast(doppler_outlier_threshold), sqrt_lambda_geometric, sqrt_lambda_doppler, sqrt_lambda_doppler_by_dt, n, diff --git a/cpp/open3d/t/pipelines/kernel/RegistrationImpl.h b/cpp/open3d/t/pipelines/kernel/RegistrationImpl.h index 34f71b54b7b..9d0309b16c7 100644 --- a/cpp/open3d/t/pipelines/kernel/RegistrationImpl.h +++ b/cpp/open3d/t/pipelines/kernel/RegistrationImpl.h @@ -69,7 +69,7 @@ void ComputePoseDopplerICPCPU( const core::Tensor &w_v_in_V, const core::Tensor &v_v_in_V, const double period, - const bool prune_correspondences, + const bool reject_dynamic_outliers, const double doppler_outlier_threshold, const registration::RobustKernel &kernel_geometric, const registration::RobustKernel &kernel_doppler, @@ -119,7 +119,7 @@ void ComputePoseDopplerICPCUDA( const core::Tensor &w_v_in_V, const core::Tensor &v_v_in_V, const double period, - const bool prune_correspondences, + const bool reject_dynamic_outliers, const double doppler_outlier_threshold, const registration::RobustKernel &kernel_geometric, const registration::RobustKernel &kernel_doppler, @@ -315,12 +315,6 @@ template bool GetJacobianColoredICP(const int64_t workload_idx, double &r_G, double &r_I); -template -static void DisplayVector(std::string name, const scalar_T *vector) { - utility::LogDebug("{} = [{}, {}, {}]", name, vector[0], vector[1], - vector[2]); -} - template OPEN3D_HOST_DEVICE inline void PreComputeForDopplerICP( const scalar_t *R_S_to_V, @@ -363,7 +357,7 @@ OPEN3D_HOST_DEVICE inline bool GetJacobianDopplerICP( const scalar_t *R_S_to_V, const scalar_t *r_v_to_s_in_V, const scalar_t *v_s_in_S, - const bool prune_correspondences, + const bool reject_dynamic_outliers, const scalar_t doppler_outlier_threshold, const scalar_t &sqrt_lambda_geometric, const scalar_t &sqrt_lambda_doppler, @@ -395,7 +389,7 @@ OPEN3D_HOST_DEVICE inline bool GetJacobianDopplerICP( const double doppler_error = doppler_in_S - doppler_pred_in_S; // Dynamic point outlier rejection. - if (prune_correspondences && + if (reject_dynamic_outliers && abs(doppler_error) > doppler_outlier_threshold) { // Jacobian and residual are set to 0 by default. return true; @@ -451,7 +445,7 @@ template bool GetJacobianDopplerICP(const int64_t workload_idx, const float *R_S_to_V, const float *r_v_to_s_in_V, const float *v_s_in_S, - const bool prune_correspondences, + const bool reject_dynamic_outliers, const float doppler_outlier_threshold, const float &sqrt_lambda_geometric, const float &sqrt_lambda_doppler, @@ -471,7 +465,7 @@ template bool GetJacobianDopplerICP(const int64_t workload_idx, const double *R_S_to_V, const double *r_v_to_s_in_V, const double *v_s_in_S, - const bool prune_correspondences, + const bool reject_dynamic_outliers, const double doppler_outlier_threshold, const double &sqrt_lambda_geometric, const double &sqrt_lambda_doppler, diff --git a/examples/python/pipelines/doppler_icp_registration.py b/examples/python/pipelines/doppler_icp_registration.py index 5133d01582d..590c8615df6 100644 --- a/examples/python/pipelines/doppler_icp_registration.py +++ b/examples/python/pipelines/doppler_icp_registration.py @@ -1,27 +1,8 @@ # ---------------------------------------------------------------------------- # - 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. +# Copyright (c) 2018-2023 www.open3d.org +# SPDX-License-Identifier: MIT # ---------------------------------------------------------------------------- """Example script to run Doppler ICP point cloud registration. From 3d453a5af24c29a0af632883a1b7409c4e582563 Mon Sep 17 00:00:00 2001 From: Heethesh Vhavle Date: Sat, 4 Mar 2023 17:37:01 +0530 Subject: [PATCH 13/14] fix python test --- cpp/pybind/t/pipelines/registration/registration.cpp | 6 ++++-- 1 file changed, 4 insertions(+), 2 deletions(-) diff --git a/cpp/pybind/t/pipelines/registration/registration.cpp b/cpp/pybind/t/pipelines/registration/registration.cpp index dc38522d174..6dbe3f1d27a 100644 --- a/cpp/pybind/t/pipelines/registration/registration.cpp +++ b/cpp/pybind/t/pipelines/registration/registration.cpp @@ -136,8 +136,10 @@ void pybind_registration_classes(py::module &m) { "correspondences."); te.def("compute_transformation", &TransformationEstimation::ComputeTransformation, "source"_a, - "target"_a, "correspondences"_a, "current_transform"_a, - "iteration"_a, + "target"_a, "correspondences"_a, + "current_transform"_a = + core::Tensor::Eye(4, core::Float64, core::Device("CPU:0")), + "iteration"_a = 0, "Compute transformation from source to target point cloud given " "correspondences."); docstring::ClassMethodDocInject(m, "TransformationEstimation", From 2c5511aca3ce851699c8243968eff7c19b0b61ca Mon Sep 17 00:00:00 2001 From: Heethesh Vhavle Date: Thu, 9 Mar 2023 02:09:35 +0530 Subject: [PATCH 14/14] PR comments, remove tf --- cpp/tests/io/CMakeLists.txt | 1 - cpp/tests/io/file_format/FileXYZD.cpp | 18 ------------------ .../pipelines/doppler_icp_registration.py | 15 ++++++++------- 3 files changed, 8 insertions(+), 26 deletions(-) delete mode 100644 cpp/tests/io/file_format/FileXYZD.cpp diff --git a/cpp/tests/io/CMakeLists.txt b/cpp/tests/io/CMakeLists.txt index 5f63045024e..c6ed76f3e54 100644 --- a/cpp/tests/io/CMakeLists.txt +++ b/cpp/tests/io/CMakeLists.txt @@ -22,7 +22,6 @@ target_sources(tests PRIVATE file_format/FilePTS.cpp file_format/FileSTL.cpp file_format/FileXYZ.cpp - file_format/FileXYZD.cpp file_format/FileXYZN.cpp file_format/FileXYZRGB.cpp ) diff --git a/cpp/tests/io/file_format/FileXYZD.cpp b/cpp/tests/io/file_format/FileXYZD.cpp deleted file mode 100644 index 11df66e533b..00000000000 --- a/cpp/tests/io/file_format/FileXYZD.cpp +++ /dev/null @@ -1,18 +0,0 @@ -// ---------------------------------------------------------------------------- -// - Open3D: www.open3d.org - -// ---------------------------------------------------------------------------- -// Copyright (c) 2018-2023 www.open3d.org -// SPDX-License-Identifier: MIT -// ---------------------------------------------------------------------------- - -#include "tests/Tests.h" - -namespace open3d { -namespace tests { - -TEST(FileXYZD, DISABLED_ReadPointCloudFromXYZD) { NotImplemented(); } - -TEST(FileXYZD, DISABLED_WritePointCloudToXYZD) { NotImplemented(); } - -} // namespace tests -} // namespace open3d diff --git a/examples/python/pipelines/doppler_icp_registration.py b/examples/python/pipelines/doppler_icp_registration.py index 590c8615df6..5fface38e6e 100644 --- a/examples/python/pipelines/doppler_icp_registration.py +++ b/examples/python/pipelines/doppler_icp_registration.py @@ -23,7 +23,8 @@ import numpy as np import open3d as o3d -import transformations as tf +import open3d.t.pipelines.registration as o3d_reg +from pyquaternion import Quaternion def translation_quaternion_to_transform(translation, @@ -43,9 +44,8 @@ def translation_quaternion_to_transform(translation, """ if quat_xyzw: quaternion = np.roll(quaternion, 1) - R = tf.quaternion_matrix(quaternion) # [w, x, y, z] - T = tf.translation_matrix(translation) # [x, y, z] - transform = tf.concatenate_matrices(T, R) + transform = Quaternion(quaternion).transformation_matrix # [w, x, y, z] + transform[:3, -1] = translation # [x, y, z] return np.linalg.inv(transform) if inverse else transform @@ -154,11 +154,12 @@ def run_doppler_icp(args): target_in_V.estimate_normals(radius=10.0, max_nn=30) # Compute direction vectors on source point cloud frame in sensor frame. - directions = tf.unit_vector(source_in_S.point.positions.numpy(), axis=1) + directions = source_in_S.point.positions.numpy() + norms = np.tile(np.linalg.norm(directions, axis=1), (3, 1)).T + directions = directions / norms source_in_V.point['directions'] = o3d.core.Tensor(directions, dtype, device) # Setup robust kernels. - o3d_reg = o3d.t.pipelines.registration kernel = o3d_reg.robust_kernel.RobustKernel(o3d_reg.robust_kernel.TukeyLoss, scaling_parameter=0.5) @@ -170,7 +171,7 @@ def run_doppler_icp(args): # Setup transformation estimator. estimator_p2l = o3d_reg.TransformationEstimationPointToPlane(kernel) estimator_dicp = o3d_reg.TransformationEstimationForDopplerICP( - period=period, + period=period * (args.target - args.source), lambda_doppler=0.01, reject_dynamic_outliers=False, doppler_outlier_threshold=2.0,