From fa1a52da447a1d443b9ec4d87e6c240fc5e32669 Mon Sep 17 00:00:00 2001 From: Heethesh Vhavle Date: Mon, 14 Feb 2022 10:37:39 -0800 Subject: [PATCH 1/7] EVE-7732 Support Doppler channel in point cloud and Doppler ICP registration (#1) --- cpp/open3d/Open3D.h.in | 1 + cpp/open3d/geometry/PointCloud.cpp | 32 +++ cpp/open3d/geometry/PointCloud.h | 16 ++ cpp/open3d/io/CMakeLists.txt | 1 + cpp/open3d/io/FileFormatIO.cpp | 1 + cpp/open3d/io/FileFormatIO.h | 1 + cpp/open3d/io/PointCloudIO.cpp | 2 + cpp/open3d/io/PointCloudIO.h | 8 + cpp/open3d/io/file_format/FileXYZND.cpp | 122 +++++++++ cpp/open3d/pipelines/CMakeLists.txt | 1 + .../pipelines/registration/DopplerICP.cpp | 256 ++++++++++++++++++ .../pipelines/registration/DopplerICP.h | 156 +++++++++++ .../pipelines/registration/Registration.cpp | 17 +- .../pipelines/registration/Registration.h | 29 +- .../registration/TransformationEstimation.h | 1 + cpp/pybind/geometry/pointcloud.cpp | 7 + .../pipelines/registration/registration.cpp | 139 +++++++++- examples/cpp/CMakeLists.txt | 5 + examples/cpp/RegistrationDopplerICP.cpp | 145 ++++++++++ 19 files changed, 933 insertions(+), 7 deletions(-) create mode 100644 cpp/open3d/io/file_format/FileXYZND.cpp create mode 100644 cpp/open3d/pipelines/registration/DopplerICP.cpp create mode 100644 cpp/open3d/pipelines/registration/DopplerICP.h create mode 100644 examples/cpp/RegistrationDopplerICP.cpp diff --git a/cpp/open3d/Open3D.h.in b/cpp/open3d/Open3D.h.in index 8efc022c829..b54d1cf8d3e 100644 --- a/cpp/open3d/Open3D.h.in +++ b/cpp/open3d/Open3D.h.in @@ -78,6 +78,7 @@ #include "open3d/pipelines/integration/UniformTSDFVolume.h" #include "open3d/pipelines/odometry/Odometry.h" #include "open3d/pipelines/registration/ColoredICP.h" +#include "open3d/pipelines/registration/DopplerICP.h" #include "open3d/pipelines/registration/Feature.h" #include "open3d/pipelines/registration/GeneralizedICP.h" #include "open3d/pipelines/registration/Registration.h" diff --git a/cpp/open3d/geometry/PointCloud.cpp b/cpp/open3d/geometry/PointCloud.cpp index 8f988f75a1b..48da80cde28 100644 --- a/cpp/open3d/geometry/PointCloud.cpp +++ b/cpp/open3d/geometry/PointCloud.cpp @@ -47,6 +47,7 @@ PointCloud &PointCloud::Clear() { points_.clear(); normals_.clear(); colors_.clear(); + dopplers_.clear(); covariances_.clear(); return *this; } @@ -119,6 +120,13 @@ PointCloud &PointCloud::operator+=(const PointCloud &cloud) { } else { colors_.clear(); } + if ((!HasPoints() || HasDopplers()) && cloud.HasDopplers()) { + dopplers_.resize(new_vert_num); + for (size_t i = 0; i < add_vert_num; i++) + dopplers_[old_vert_num + i] = cloud.dopplers_[i]; + } else { + dopplers_.clear(); + } if ((!HasPoints() || HasCovariances()) && cloud.HasCovariances()) { covariances_.resize(new_vert_num); for (size_t i = 0; i < add_vert_num; i++) @@ -162,6 +170,7 @@ PointCloud &PointCloud::RemoveNonFinitePoints(bool remove_nan, bool remove_infinite) { bool has_normal = HasNormals(); bool has_color = HasColors(); + bool has_doppler = HasDopplers(); bool has_covariance = HasCovariances(); size_t old_point_num = points_.size(); size_t k = 0; // new index @@ -176,6 +185,7 @@ PointCloud &PointCloud::RemoveNonFinitePoints(bool remove_nan, points_[k] = points_[i]; if (has_normal) normals_[k] = normals_[i]; if (has_color) colors_[k] = colors_[i]; + if (has_doppler) dopplers_[k] = dopplers_[i]; if (has_covariance) covariances_[k] = covariances_[i]; k++; } @@ -183,6 +193,7 @@ PointCloud &PointCloud::RemoveNonFinitePoints(bool remove_nan, points_.resize(k); if (has_normal) normals_.resize(k); if (has_color) colors_.resize(k); + if (has_doppler) dopplers_.resize(k); if (has_covariance) covariances_.resize(k); utility::LogDebug( "[RemoveNonFinitePoints] {:d} nan points have been removed.", @@ -195,6 +206,7 @@ std::shared_ptr PointCloud::SelectByIndex( auto output = std::make_shared(); bool has_normals = HasNormals(); bool has_colors = HasColors(); + bool has_doppler = HasDopplers(); bool has_covariance = HasCovariances(); std::vector mask = std::vector(points_.size(), invert); @@ -207,6 +219,7 @@ std::shared_ptr PointCloud::SelectByIndex( output->points_.push_back(points_[i]); if (has_normals) output->normals_.push_back(normals_[i]); if (has_colors) output->colors_.push_back(colors_[i]); + if (has_doppler) output->dopplers_.push_back(dopplers_[i]); if (has_covariance) output->covariances_.push_back(covariances_[i]); } } @@ -232,6 +245,9 @@ class AccumulatedPoint { if (cloud.HasColors()) { color_ += cloud.colors_[index]; } + if (cloud.HasDopplers()) { + doppler_ += cloud.dopplers_[index]; + } if (cloud.HasCovariances()) { covariance_ += cloud.covariances_[index]; } @@ -251,6 +267,10 @@ class AccumulatedPoint { return color_ / double(num_of_points_); } + double GetAverageDoppler() const { + return doppler_ / double(num_of_points_); + } + Eigen::Matrix3d GetAverageCovariance() const { return covariance_ / double(num_of_points_); } @@ -261,6 +281,7 @@ class AccumulatedPoint { Eigen::Vector3d normal_ = Eigen::Vector3d::Zero(); Eigen::Vector3d color_ = Eigen::Vector3d::Zero(); Eigen::Matrix3d covariance_ = Eigen::Matrix3d::Zero(); + double doppler_ = 0; }; class point_cubic_id { @@ -294,6 +315,9 @@ class AccumulatedPointForTrace : public AccumulatedPoint { color_ += cloud.colors_[index]; } } + if (cloud.HasDopplers()) { + doppler_ += cloud.dopplers_[index]; + } if (cloud.HasCovariances()) { covariance_ += cloud.covariances_[index]; } @@ -353,6 +377,7 @@ std::shared_ptr PointCloud::VoxelDownSample( } bool has_normals = HasNormals(); bool has_colors = HasColors(); + bool has_doppler = HasDopplers(); bool has_covariances = HasCovariances(); for (auto accpoint : voxelindex_to_accpoint) { output->points_.push_back(accpoint.second.GetAveragePoint()); @@ -362,6 +387,9 @@ std::shared_ptr PointCloud::VoxelDownSample( if (has_colors) { output->colors_.push_back(accpoint.second.GetAverageColor()); } + if (has_doppler) { + output->dopplers_.push_back(accpoint.second.GetAverageDoppler()); + } if (has_covariances) { output->covariances_.emplace_back( accpoint.second.GetAverageCovariance()); @@ -413,6 +441,7 @@ PointCloud::VoxelDownSampleAndTrace(double voxel_size, } bool has_normals = HasNormals(); bool has_colors = HasColors(); + bool has_doppler = HasDopplers(); bool has_covariances = HasCovariances(); int cnt = 0; cubic_id.resize(voxelindex_to_accpoint.size(), 8); @@ -431,6 +460,9 @@ PointCloud::VoxelDownSampleAndTrace(double voxel_size, output->colors_.push_back(accpoint.second.GetAverageColor()); } } + if (has_doppler) { + output->dopplers_.emplace_back(accpoint.second.GetAverageDoppler()); + } if (has_covariances) { output->covariances_.emplace_back( accpoint.second.GetAverageCovariance()); diff --git a/cpp/open3d/geometry/PointCloud.h b/cpp/open3d/geometry/PointCloud.h index 681214470e4..8193a90db42 100644 --- a/cpp/open3d/geometry/PointCloud.h +++ b/cpp/open3d/geometry/PointCloud.h @@ -61,6 +61,15 @@ class PointCloud : public Geometry3D { /// \param points Points coordinates. PointCloud(const std::vector &points) : Geometry3D(Geometry::GeometryType::PointCloud), points_(points) {} + /// \brief Parameterized Constructor. + /// + /// \param points Points coordinates. + /// \param dopplers Doppler velocitues. + PointCloud(const std::vector &points, + const std::vector &dopplers) + : Geometry3D(Geometry::GeometryType::PointCloud), + points_(points), + dopplers_(dopplers) {} ~PointCloud() override {} public: @@ -96,6 +105,11 @@ class PointCloud : public Geometry3D { return points_.size() > 0 && colors_.size() == points_.size(); } + /// Returns 'true' if the point cloud contains Doppler velocities. + bool HasDopplers() const { + return points_.size() > 0 && dopplers_.size() == points_.size(); + } + /// Returns 'true' if the point cloud contains per-point covariance matrix. bool HasCovariances() const { return !points_.empty() && covariances_.size() == points_.size(); @@ -420,6 +434,8 @@ class PointCloud : public Geometry3D { std::vector normals_; /// RGB colors of points. std::vector colors_; + /// Doppler velocity of points. + std::vector dopplers_; /// Covariance Matrix for each point std::vector covariances_; }; diff --git a/cpp/open3d/io/CMakeLists.txt b/cpp/open3d/io/CMakeLists.txt index b4348583f77..eef2c7659e1 100644 --- a/cpp/open3d/io/CMakeLists.txt +++ b/cpp/open3d/io/CMakeLists.txt @@ -33,6 +33,7 @@ target_sources(io PRIVATE file_format/FileTUM.cpp file_format/FileXYZ.cpp file_format/FileXYZN.cpp + file_format/FileXYZND.cpp file_format/FileXYZRGB.cpp ) diff --git a/cpp/open3d/io/FileFormatIO.cpp b/cpp/open3d/io/FileFormatIO.cpp index ebe41d5ef5e..a22b4692dfd 100644 --- a/cpp/open3d/io/FileFormatIO.cpp +++ b/cpp/open3d/io/FileFormatIO.cpp @@ -45,6 +45,7 @@ static std::map gExt2Func = { {"stl", ReadFileGeometryTypeSTL}, {"xyz", ReadFileGeometryTypeXYZ}, {"xyzn", ReadFileGeometryTypeXYZN}, + {"xyznd", ReadFileGeometryTypeXYZND}, {"xyzrgb", ReadFileGeometryTypeXYZRGB}, }; diff --git a/cpp/open3d/io/FileFormatIO.h b/cpp/open3d/io/FileFormatIO.h index 4986d409f51..17feadcff3f 100644 --- a/cpp/open3d/io/FileFormatIO.h +++ b/cpp/open3d/io/FileFormatIO.h @@ -53,6 +53,7 @@ FileGeometry ReadFileGeometryTypePTS(const std::string& path); FileGeometry ReadFileGeometryTypeSTL(const std::string& path); FileGeometry ReadFileGeometryTypeXYZ(const std::string& path); FileGeometry ReadFileGeometryTypeXYZN(const std::string& path); +FileGeometry ReadFileGeometryTypeXYZND(const std::string& path); FileGeometry ReadFileGeometryTypeXYZRGB(const std::string& path); } // namespace io diff --git a/cpp/open3d/io/PointCloudIO.cpp b/cpp/open3d/io/PointCloudIO.cpp index 10fcc362e36..1c31f66aa0a 100644 --- a/cpp/open3d/io/PointCloudIO.cpp +++ b/cpp/open3d/io/PointCloudIO.cpp @@ -45,6 +45,7 @@ static const std::unordered_map< file_extension_to_pointcloud_read_function{ {"xyz", ReadPointCloudFromXYZ}, {"xyzn", ReadPointCloudFromXYZN}, + {"xyznd", ReadPointCloudFromXYZND}, {"xyzrgb", ReadPointCloudFromXYZRGB}, {"ply", ReadPointCloudFromPLY}, {"pcd", ReadPointCloudFromPCD}, @@ -59,6 +60,7 @@ static const std::unordered_map< file_extension_to_pointcloud_write_function{ {"xyz", WritePointCloudToXYZ}, {"xyzn", WritePointCloudToXYZN}, + {"xyznd", WritePointCloudToXYZND}, {"xyzrgb", WritePointCloudToXYZRGB}, {"ply", WritePointCloudToPLY}, {"pcd", WritePointCloudToPCD}, diff --git a/cpp/open3d/io/PointCloudIO.h b/cpp/open3d/io/PointCloudIO.h index e427069d682..c0173f8103b 100644 --- a/cpp/open3d/io/PointCloudIO.h +++ b/cpp/open3d/io/PointCloudIO.h @@ -155,6 +155,14 @@ bool WritePointCloudToXYZN(const std::string &filename, const geometry::PointCloud &pointcloud, const WritePointCloudOption ¶ms); +bool ReadPointCloudFromXYZND(const std::string &filename, + geometry::PointCloud &pointcloud, + const ReadPointCloudOption ¶ms); + +bool WritePointCloudToXYZND(const std::string &filename, + const geometry::PointCloud &pointcloud, + const WritePointCloudOption ¶ms); + bool ReadPointCloudFromXYZRGB(const std::string &filename, geometry::PointCloud &pointcloud, const ReadPointCloudOption ¶ms); diff --git a/cpp/open3d/io/file_format/FileXYZND.cpp b/cpp/open3d/io/file_format/FileXYZND.cpp new file mode 100644 index 00000000000..2bd2f2bbf9b --- /dev/null +++ b/cpp/open3d/io/file_format/FileXYZND.cpp @@ -0,0 +1,122 @@ +// ---------------------------------------------------------------------------- +// - 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 "open3d/io/FileFormatIO.h" +#include "open3d/io/PointCloudIO.h" +#include "open3d/utility/FileSystem.h" +#include "open3d/utility/Logging.h" +#include "open3d/utility/ProgressReporters.h" + +namespace open3d { +namespace io { + +FileGeometry ReadFileGeometryTypeXYZND(const std::string &path) { + return CONTAINS_POINTS; +} + +bool ReadPointCloudFromXYZND(const std::string &filename, + geometry::PointCloud &pointcloud, + const ReadPointCloudOption ¶ms) { + try { + utility::filesystem::CFile file; + if (!file.Open(filename, "r")) { + utility::LogWarning("Read XYZND failed: unable to open file: {}", + filename); + return false; + } + utility::CountingProgressReporter reporter(params.update_progress); + reporter.SetTotal(file.GetFileSize()); + + pointcloud.Clear(); + int i = 0; + double x, y, z, nx, ny, nz, doppler; + const char *line_buffer; + while ((line_buffer = file.ReadLine())) { + if (sscanf(line_buffer, "%lf %lf %lf %lf %lf %lf %lf", &x, &y, &z, + &nx, &ny, &nz, &doppler) == 7) { + pointcloud.points_.push_back(Eigen::Vector3d(x, y, z)); + pointcloud.normals_.push_back(Eigen::Vector3d(nx, ny, nz)); + pointcloud.dopplers_.push_back(doppler); + } + if (++i % 1000 == 0) { + reporter.Update(file.CurPos()); + } + } + reporter.Finish(); + + return true; + } catch (const std::exception &e) { + utility::LogWarning("Read XYZND failed with exception: {}", e.what()); + return false; + } +} + +bool WritePointCloudToXYZND(const std::string &filename, + const geometry::PointCloud &pointcloud, + const WritePointCloudOption ¶ms) { + if (!pointcloud.HasNormals() || !pointcloud.HasDopplers()) { + return false; + } + + try { + utility::filesystem::CFile file; + if (!file.Open(filename, "w")) { + utility::LogWarning("Write XYZND failed: unable to open file: {}", + filename); + return false; + } + utility::CountingProgressReporter reporter(params.update_progress); + reporter.SetTotal(pointcloud.points_.size()); + + for (size_t i = 0; i < pointcloud.points_.size(); i++) { + const Eigen::Vector3d &point = pointcloud.points_[i]; + const Eigen::Vector3d &normal = pointcloud.normals_[i]; + const double &doppler = pointcloud.dopplers_[i]; + if (fprintf(file.GetFILE(), + "%.10f %.10f %.10f %.10f %.10f %.10f %.10f\n", point(0), + point(1), point(2), normal(0), normal(1), normal(2), + doppler) < 0) { + utility::LogWarning( + "Write XYZND 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 XYZND failed with exception: {}", e.what()); + return false; + } +} + +} // namespace io +} // namespace open3d diff --git a/cpp/open3d/pipelines/CMakeLists.txt b/cpp/open3d/pipelines/CMakeLists.txt index 7173b64b515..e177db5fb79 100644 --- a/cpp/open3d/pipelines/CMakeLists.txt +++ b/cpp/open3d/pipelines/CMakeLists.txt @@ -20,6 +20,7 @@ target_sources(pipelines PRIVATE target_sources(pipelines PRIVATE registration/ColoredICP.cpp registration/CorrespondenceChecker.cpp + registration/DopplerICP.cpp registration/FastGlobalRegistration.cpp registration/Feature.cpp registration/GeneralizedICP.cpp diff --git a/cpp/open3d/pipelines/registration/DopplerICP.cpp b/cpp/open3d/pipelines/registration/DopplerICP.cpp new file mode 100644 index 00000000000..b6f5fae999b --- /dev/null +++ b/cpp/open3d/pipelines/registration/DopplerICP.cpp @@ -0,0 +1,256 @@ +// ---------------------------------------------------------------------------- +// - Open3D: www.open3d.org - +// ---------------------------------------------------------------------------- +// The MIT License (MIT) +// +// Copyright (c) 2018-2021 www.open3d.org +// +// Permission is hereby granted, free of charge, to any person obtaining a copy +// of this software and associated documentation files (the "Software"), to deal +// in the Software without restriction, including without limitation the rights +// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell +// copies of the Software, and to permit persons to whom the Software is +// furnished to do so, subject to the following conditions: +// +// The above copyright notice and this permission notice shall be included in +// all copies or substantial portions of the Software. +// +// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE +// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING +// FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS +// IN THE SOFTWARE. +// ---------------------------------------------------------------------------- + +#include "open3d/pipelines/registration/DopplerICP.h" + +#include +#include + +#include "open3d/geometry/KDTreeFlann.h" +#include "open3d/geometry/KDTreeSearchParam.h" +#include "open3d/geometry/PointCloud.h" +#include "open3d/pipelines/registration/Registration.h" +#include "open3d/pipelines/registration/RobustKernel.h" +#include "open3d/utility/Eigen.h" +#include "open3d/utility/Logging.h" + +namespace open3d { +namespace pipelines { +namespace registration { + +Eigen::Matrix4d TransformationEstimationForDopplerICP::ComputeTransformation( + const geometry::PointCloud &source, + const geometry::PointCloud &target, + const CorrespondenceSet &corres) const { + utility::LogError( + "This method should not be called for DopplerICP. DopplerICP " + "requires the period, current transformation, and T_V_to_S " + "calibration."); + return Eigen::Matrix4d::Identity(); +} + +Eigen::Matrix4d TransformationEstimationForDopplerICP::ComputeTransformation( + const geometry::PointCloud &source, + const geometry::PointCloud &target, + const CorrespondenceSet &corres, + const std::vector &source_directions, + const double period, + const Eigen::Matrix4d &transformation, + const Eigen::Matrix4d &T_V_to_S, + const size_t iteration) const { + if (corres.empty()) { + utility::LogError( + "No correspondences found between source and target " + "pointcloud."); + } + if (!target.HasNormals()) { + utility::LogError( + "DopplerICP requires target pointcloud to have normals."); + } + if (!source.HasDopplers()) { + utility::LogError( + "DopplerICP requires source pointcloud to have Dopplers."); + } + if (std::abs(period) < 1e-3) { + utility::LogError("Time period too small"); + } + + const double lambda_geometric = 1.0 - lambda_doppler_; + const double sqrt_lambda_doppler = std::sqrt(lambda_doppler_); + const double sqrt_lambda_geometric = std::sqrt(lambda_geometric); + const double sqrt_lambda_doppler_by_dt = sqrt_lambda_doppler / period; + + const Eigen::Vector6d state_vector = + utility::TransformMatrix4dToVector6d(transformation); + const Eigen::Matrix3d R_S_to_V = T_V_to_S.block<3, 3>(0, 0).inverse(); + const Eigen::Vector3d r_v_to_s_in_V = T_V_to_S.block<3, 1>(0, 3); + const Eigen::Vector3d w_v_in_V = -state_vector.block<3, 1>(0, 0) / period; + const Eigen::Vector3d v_v_in_V = -state_vector.block<3, 1>(3, 0) / period; + const Eigen::Vector3d v_s_in_V = v_v_in_V + w_v_in_V.cross(r_v_to_s_in_V); + const Eigen::Vector3d v_s_in_S = R_S_to_V * v_s_in_V; + + auto compute_jacobian_and_residual = + [&](int i, + std::vector &J_r, + std::vector &r, std::vector &w) { + const size_t cs = corres[i][0]; + const size_t ct = corres[i][1]; + const Eigen::Vector3d &ps_in_V = source.points_[cs]; + const Eigen::Vector3d &pt_in_V = target.points_[ct]; + const Eigen::Vector3d &nt_in_V = target.normals_[ct]; + const Eigen::Vector3d &ds_in_V = source_directions[cs]; + const double &doppler_in_S = source.dopplers_[cs]; + + J_r.resize(2); + r.resize(2); + w.resize(2); + + // Compute predicted Doppler velocity. + const Eigen::Vector3d ds_in_S = R_S_to_V * ds_in_V; + const double doppler_pred_in_S = -ds_in_S.dot(v_s_in_S); + const double doppler_error = doppler_in_S - doppler_pred_in_S; + + // Dynamic point outlier pruning of correspondences. + bool optimize{true}; + if (prune_correspondences_ && + std::abs(doppler_error) > doppler_outlier_threshold_) { + optimize = false; + } + + if (optimize) { + // Compute geometric point-to-plane error and Jacobian. + const double geometric_error = (ps_in_V - pt_in_V).dot(nt_in_V); + r[0] = sqrt_lambda_geometric * geometric_error; + w[0] = (iteration >= geometric_robust_loss_min_iteration_) + ? geometric_kernel_->Weight(r[0]) + : default_kernel_->Weight(r[0]); + J_r[0].block<3, 1>(0, 0) = + sqrt_lambda_geometric * ps_in_V.cross(nt_in_V); + J_r[0].block<3, 1>(3, 0) = sqrt_lambda_geometric * nt_in_V; + + // Compute Doppler error and Jacobian. + r[1] = sqrt_lambda_doppler * doppler_error; + w[1] = (iteration >= doppler_robust_loss_min_iteration_) + ? doppler_kernel_->Weight(r[1]) + : default_kernel_->Weight(r[1]); + J_r[1].block<3, 1>(0, 0) = sqrt_lambda_doppler_by_dt * + ds_in_V.cross(r_v_to_s_in_V); + J_r[1].block<3, 1>(3, 0) = sqrt_lambda_doppler_by_dt * -ds_in_V; + } else { + r[0] = 0.F; + w[0] = 0.F; + J_r[0].block<3, 1>(0, 0) = Eigen::Vector3d::Zero(); + J_r[0].block<3, 1>(3, 0) = Eigen::Vector3d::Zero(); + + r[1] = 0.F; + w[1] = 0.F; + J_r[1].block<3, 1>(0, 0) = Eigen::Vector3d::Zero(); + J_r[1].block<3, 1>(3, 0) = Eigen::Vector3d::Zero(); + } + }; + + Eigen::Matrix6d JTJ; + Eigen::Vector6d JTr; + double r2; + std::tie(JTJ, JTr, r2) = + utility::ComputeJTJandJTr( + compute_jacobian_and_residual, (int)corres.size()); + + bool is_success; + Eigen::Matrix4d extrinsic; + std::tie(is_success, extrinsic) = + utility::SolveJacobianSystemAndObtainExtrinsicMatrix(JTJ, JTr); + + return is_success ? extrinsic : Eigen::Matrix4d::Identity(); +} + +double TransformationEstimationForDopplerICP::ComputeRMSE( + const geometry::PointCloud &source, + const geometry::PointCloud &target, + const CorrespondenceSet &corres) const { + if (corres.empty() || !target.HasNormals()) return 0.0; + double err = 0.0, r; + for (const auto &c : corres) { + r = (source.points_[c[0]] - target.points_[c[1]]) + .dot(target.normals_[c[1]]); + err += r * r; + } + return std::sqrt(err / (double)corres.size()); +}; + +RegistrationResult RegistrationDopplerICP( + const geometry::PointCloud &source, + const geometry::PointCloud &target, + const std::vector &source_directions, + double max_correspondence_distance, + const Eigen::Matrix4d &init /* = Eigen::Matrix4d::Identity()*/, + const TransformationEstimationForDopplerICP &estimation + /* = TransformationEstimationForDopplerICP()*/, + const ICPConvergenceCriteria &criteria /* = ICPConvergenceCriteria()*/, + const double period /* = 0.1F*/, + const Eigen::Matrix4d &T_V_to_S /* = Eigen::Matrix4d::Identity()*/) { + if (max_correspondence_distance <= 0.0) { + utility::LogError("Invalid max_correspondence_distance."); + } + + if ((estimation.GetTransformationEstimationType() == + TransformationEstimationType::DopplerICP) && + (!target.HasNormals() || !source.HasDopplers())) { + utility::LogError( + "TransformationEstimationDopplerICP requires Doppler " + "velocities for source PointCloud and pre-computed normal " + "vectors for target PointCloud."); + } + + Eigen::Matrix4d transformation = init; + geometry::KDTreeFlann kdtree; + kdtree.SetGeometry(target); + geometry::PointCloud pcd = source; + if (!init.isIdentity()) { + pcd.Transform(init); + } + + RegistrationResult result; + result = GetRegistrationResultAndCorrespondences( + pcd, target, kdtree, max_correspondence_distance, transformation); + + int i; + bool converged{false}; + for (i = 0; i < criteria.max_iteration_; i++) { + utility::LogDebug("ICP Iteration #{:d}: Fitness {:.4f}, RMSE {:.4f}", i, + result.fitness_, result.inlier_rmse_); + + // Compute the transform update. + Eigen::Matrix4d update = estimation.ComputeTransformation( + pcd, target, result.correspondence_set_, source_directions, + period, transformation, T_V_to_S, i); + transformation = update * transformation; + pcd.Transform(update); + + // Update the registration result. + RegistrationResult backup = result; + result = GetRegistrationResultAndCorrespondences( + pcd, target, kdtree, max_correspondence_distance, + transformation); + + // Check for convergence. + if (std::abs(backup.fitness_ - result.fitness_) < + criteria.relative_fitness_ && + std::abs(backup.inlier_rmse_ - result.inlier_rmse_) < + criteria.relative_rmse_) { + converged = true; + break; + } + } + + result.num_iterations_ = i; + result.converged_ = converged; + return result; +} + +} // namespace registration +} // namespace pipelines +} // namespace open3d diff --git a/cpp/open3d/pipelines/registration/DopplerICP.h b/cpp/open3d/pipelines/registration/DopplerICP.h new file mode 100644 index 00000000000..2b192180347 --- /dev/null +++ b/cpp/open3d/pipelines/registration/DopplerICP.h @@ -0,0 +1,156 @@ +// ---------------------------------------------------------------------------- +// - Open3D: www.open3d.org - +// ---------------------------------------------------------------------------- +// The MIT License (MIT) +// +// Copyright (c) 2018-2021 www.open3d.org +// +// Permission is hereby granted, free of charge, to any person obtaining a copy +// of this software and associated documentation files (the "Software"), to deal +// in the Software without restriction, including without limitation the rights +// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell +// copies of the Software, and to permit persons to whom the Software is +// furnished to do so, subject to the following conditions: +// +// The above copyright notice and this permission notice shall be included in +// all copies or substantial portions of the Software. +// +// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE +// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING +// FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS +// IN THE SOFTWARE. +// ---------------------------------------------------------------------------- + +#pragma once + +#include +#include + +#include "open3d/pipelines/registration/Registration.h" +#include "open3d/pipelines/registration/RobustKernel.h" +#include "open3d/pipelines/registration/TransformationEstimation.h" + +namespace open3d { + +namespace geometry { +class PointCloud; +} + +namespace pipelines { +namespace registration { + +class RegistrationResult; + +/// \class TransformationEstimationForDopplerICP +/// +/// Class to estimate a transformation for DICP with point to plane distance. +class TransformationEstimationForDopplerICP : public TransformationEstimation { +public: + ~TransformationEstimationForDopplerICP() override{}; + + /// \brief Constructor that takes as input a RobustKernel \param kernel Any + /// of the implemented statistical robust kernel for outlier rejection. + explicit TransformationEstimationForDopplerICP( + double lambda_doppler = 0.01, + bool prune_correspondences = false, + double doppler_outlier_threshold = 2.0, + size_t geometric_robust_loss_min_iteration = 0, + size_t doppler_robust_loss_min_iteration = 2, + std::shared_ptr geometric_kernel = + std::make_shared(), + std::shared_ptr doppler_kernel = + std::make_shared()) + : lambda_doppler_(lambda_doppler), + prune_correspondences_(prune_correspondences), + doppler_outlier_threshold_(doppler_outlier_threshold), + geometric_robust_loss_min_iteration_( + geometric_robust_loss_min_iteration), + doppler_robust_loss_min_iteration_(doppler_robust_loss_min_iteration), + geometric_kernel_(std::move(geometric_kernel)), + doppler_kernel_(std::move(doppler_kernel)) { + if (lambda_doppler_ < 0 || lambda_doppler_ > 1.0) { + lambda_doppler_ = 0.01; + } + } + +public: + TransformationEstimationType GetTransformationEstimationType() + const override { + return type_; + }; + double ComputeRMSE(const geometry::PointCloud &source, + const geometry::PointCloud &target, + const CorrespondenceSet &corres) const override; + Eigen::Matrix4d ComputeTransformation( + const geometry::PointCloud &source, + const geometry::PointCloud &target, + const CorrespondenceSet &corres) const override; + Eigen::Matrix4d ComputeTransformation( + const geometry::PointCloud &source, + const geometry::PointCloud &target, + const CorrespondenceSet &corres, + const std::vector &source_directions, + const double period, + const Eigen::Matrix4d &transformation, + const Eigen::Matrix4d &T_V_to_S, + 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 prune_correspondences_{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 robust loss kicks in. + size_t geometric_robust_loss_min_iteration_{0}; + size_t doppler_robust_loss_min_iteration_{2}; + + /// shared_ptr to an Abstract RobustKernel that could mutate at runtime. + std::shared_ptr default_kernel_ = std::make_shared(); + std::shared_ptr geometric_kernel_ = + std::make_shared(); + std::shared_ptr doppler_kernel_ = std::make_shared(); + +private: + const TransformationEstimationType type_ = + TransformationEstimationType::DopplerICP; +}; + +/// \brief Function for Doppler ICP registration. +/// +/// \param source The source point cloud. +/// \param target The target point cloud. +/// \param max_distance Maximum correspondence points-pair distance (meters). +/// \param init Initial transformation estimation. +/// Default value: array([[1., 0., 0., 0.], [0., 1., 0., 0.], [0., 0., 1., 0.], +/// [0., 0., 0., 1.]]). +/// \param estimation TransformationEstimationForDopplerICP method. Can only +/// change the lambda_doppler value and the robust kernel used in the +/// optimization. +/// \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. +/// Default value: array([[1., 0., 0., 0.], [0., 1., 0., 0.], [0., 0., 1., 0.], +/// [0., 0., 0., 1.]]) +RegistrationResult RegistrationDopplerICP( + const geometry::PointCloud &source, + const geometry::PointCloud &target, + const std::vector &source_directions, + double max_distance, + const Eigen::Matrix4d &init = Eigen::Matrix4d::Identity(), + const TransformationEstimationForDopplerICP &estimation = + TransformationEstimationForDopplerICP(), + const ICPConvergenceCriteria &criteria = ICPConvergenceCriteria(), + const double period = 0.1F, + const Eigen::Matrix4d &T_V_to_S = Eigen::Matrix4d::Identity()); + +} // namespace registration +} // namespace pipelines +} // namespace open3d diff --git a/cpp/open3d/pipelines/registration/Registration.cpp b/cpp/open3d/pipelines/registration/Registration.cpp index e82fcb32027..8420b4e8b16 100644 --- a/cpp/open3d/pipelines/registration/Registration.cpp +++ b/cpp/open3d/pipelines/registration/Registration.cpp @@ -26,7 +26,6 @@ #include "open3d/pipelines/registration/Registration.h" -#include "open3d/geometry/KDTreeFlann.h" #include "open3d/geometry/PointCloud.h" #include "open3d/pipelines/registration/Feature.h" #include "open3d/utility/Helper.h" @@ -37,7 +36,7 @@ namespace open3d { namespace pipelines { namespace registration { -static RegistrationResult GetRegistrationResultAndCorrespondences( +RegistrationResult GetRegistrationResultAndCorrespondences( const geometry::PointCloud &source, const geometry::PointCloud &target, const geometry::KDTreeFlann &target_kdtree, @@ -154,6 +153,12 @@ RegistrationResult RegistrationICP( "TransformationEstimationColoredICP " "require pre-computed normal vectors for target PointCloud."); } + if (estimation.GetTransformationEstimationType() == + TransformationEstimationType::DopplerICP) { + utility::LogError( + "Use RegistrationDopplerICP along with " + "TransformationEstimationDopplerICP."); + } if ((estimation.GetTransformationEstimationType() == TransformationEstimationType::GeneralizedICP) && (!target.HasCovariances() || !source.HasCovariances())) { @@ -173,7 +178,9 @@ RegistrationResult RegistrationICP( RegistrationResult result; result = GetRegistrationResultAndCorrespondences( pcd, target, kdtree, max_correspondence_distance, transformation); - for (int i = 0; i < criteria.max_iteration_; i++) { + int i; + bool converged{false}; + for (i = 0; i < criteria.max_iteration_; i++) { utility::LogDebug("ICP Iteration #{:d}: Fitness {:.4f}, RMSE {:.4f}", i, result.fitness_, result.inlier_rmse_); Eigen::Matrix4d update = estimation.ComputeTransformation( @@ -189,9 +196,13 @@ RegistrationResult RegistrationICP( criteria.relative_fitness_ && std::abs(backup.inlier_rmse_ - result.inlier_rmse_) < criteria.relative_rmse_) { + converged = true; break; } } + + result.num_iterations_ = i; + result.converged_ = converged; return result; } diff --git a/cpp/open3d/pipelines/registration/Registration.h b/cpp/open3d/pipelines/registration/Registration.h index 386719b1dbf..57f5b2eb001 100644 --- a/cpp/open3d/pipelines/registration/Registration.h +++ b/cpp/open3d/pipelines/registration/Registration.h @@ -30,6 +30,7 @@ #include #include +#include "open3d/geometry/KDTreeFlann.h" #include "open3d/pipelines/registration/CorrespondenceChecker.h" #include "open3d/pipelines/registration/TransformationEstimation.h" #include "open3d/utility/Eigen.h" @@ -123,7 +124,11 @@ class RegistrationResult { /// \param transformation The estimated transformation matrix. RegistrationResult( const Eigen::Matrix4d &transformation = Eigen::Matrix4d::Identity()) - : transformation_(transformation), inlier_rmse_(0.0), fitness_(0.0) {} + : transformation_(transformation), + inlier_rmse_(0.0), + fitness_(0.0), + converged_(false), + num_iterations_(0) {} ~RegistrationResult() {} bool IsBetterRANSACThan(const RegistrationResult &other) const { return fitness_ > other.fitness_ || (fitness_ == other.fitness_ && @@ -142,8 +147,30 @@ class RegistrationResult { /// For RANSAC: inlier ratio (# of inlier correspondences / # of /// all correspondences) double fitness_; + /// Specifies whether the algorithm converged or not. + bool converged_; + /// Number of iterations the algorithm took to converge. + size_t num_iterations_; + std::vector errors_; }; +/// \brief Helper function for evaluating registration between point clouds. +/// +/// \param source The source point cloud. +/// \param target The target point cloud. +/// \param target_kdtree The k-d tree generated from target point cloud. +/// \param max_correspondence_distance Maximum correspondence points-pair +/// distance. +/// \param transformation The 4x4 transformation matrix to transform +/// source to target. Default value: array([[1., 0., 0., 0.], [0., 1., 0., 0.], +/// [0., 0., 1., 0.], [0., 0., 0., 1.]]). +RegistrationResult GetRegistrationResultAndCorrespondences( + const geometry::PointCloud &source, + const geometry::PointCloud &target, + const geometry::KDTreeFlann &target_kdtree, + double max_correspondence_distance, + const Eigen::Matrix4d &transformation); + /// \brief Function for evaluating registration between point clouds. /// /// \param source The source point cloud. diff --git a/cpp/open3d/pipelines/registration/TransformationEstimation.h b/cpp/open3d/pipelines/registration/TransformationEstimation.h index bac6774ca33..64bfc509a09 100644 --- a/cpp/open3d/pipelines/registration/TransformationEstimation.h +++ b/cpp/open3d/pipelines/registration/TransformationEstimation.h @@ -51,6 +51,7 @@ enum class TransformationEstimationType { PointToPlane = 2, ColoredICP = 3, GeneralizedICP = 4, + DopplerICP = 5, }; /// \class TransformationEstimation diff --git a/cpp/pybind/geometry/pointcloud.cpp b/cpp/pybind/geometry/pointcloud.cpp index c1478eb8a78..4d3cb447bc0 100644 --- a/cpp/pybind/geometry/pointcloud.cpp +++ b/cpp/pybind/geometry/pointcloud.cpp @@ -63,6 +63,8 @@ void pybind_pointcloud(py::module &m) { "Returns ``True`` if the point cloud contains point normals.") .def("has_colors", &PointCloud::HasColors, "Returns ``True`` if the point cloud contains point colors.") + .def("has_dopplers", &PointCloud::HasDopplers, + "Returns ``True`` if the point cloud contains Doppler velocities.") .def("has_covariances", &PointCloud::HasCovariances, "Returns ``True`` if the point cloud contains covariances.") .def("normalize_normals", &PointCloud::NormalizeNormals, @@ -240,11 +242,16 @@ camera. Given depth value d at (u, v) image coordinate, the corresponding 3d poi "``float64`` array of shape ``(num_points, 3)``, " "range ``[0, 1]`` , use ``numpy.asarray()`` to access " "data: RGB colors of points.") + .def_readwrite("dopplers", &PointCloud::dopplers_, + "``float64`` array of shape ``(num_points, 1)``, " + "use ``numpy.asarray()`` to access data: Points " + "Doppler velocities.") .def_readwrite("covariances", &PointCloud::covariances_, "``float64`` array of shape ``(num_points, 3, 3)``, " "use ``numpy.asarray()`` to access data: Points " "covariances."); docstring::ClassMethodDocInject(m, "PointCloud", "has_colors"); + docstring::ClassMethodDocInject(m, "PointCloud", "has_dopplers"); docstring::ClassMethodDocInject(m, "PointCloud", "has_normals"); docstring::ClassMethodDocInject(m, "PointCloud", "has_points"); docstring::ClassMethodDocInject(m, "PointCloud", "normalize_normals"); diff --git a/cpp/pybind/pipelines/registration/registration.cpp b/cpp/pybind/pipelines/registration/registration.cpp index e2f9eee280f..5f38839eae8 100644 --- a/cpp/pybind/pipelines/registration/registration.cpp +++ b/cpp/pybind/pipelines/registration/registration.cpp @@ -32,6 +32,7 @@ #include "open3d/geometry/PointCloud.h" #include "open3d/pipelines/registration/ColoredICP.h" #include "open3d/pipelines/registration/CorrespondenceChecker.h" +#include "open3d/pipelines/registration/DopplerICP.h" #include "open3d/pipelines/registration/FastGlobalRegistration.h" #include "open3d/pipelines/registration/Feature.h" #include "open3d/pipelines/registration/GeneralizedICP.h" @@ -295,6 +296,97 @@ Sets :math:`c = 1` if ``with_scaling`` is ``False``. &TransformationEstimationForColoredICP::kernel_, "Robust Kernel used in the Optimization"); + // open3d.registration.TransformationEstimationForDopplerICP: + // TransformationEstimation + py::class_< + TransformationEstimationForDopplerICP, + PyTransformationEstimation, + TransformationEstimation> + te_dop(m, "TransformationEstimationForDopplerICP", + "Class to estimate a transformation between two point " + "clouds using Doppler velocity information"); + py::detail::bind_default_constructor( + te_dop); + py::detail::bind_copy_functions( + te_dop); + te_dop.def(py::init([](double lambda_doppler, bool prune_correspondences, + double doppler_outlier_threshold, + size_t geometric_robust_loss_min_iteration, + size_t doppler_robust_loss_min_iteration, + std::shared_ptr geometric_kernel, + std::shared_ptr doppler_kernel) { + return new TransformationEstimationForDopplerICP( + lambda_doppler, + prune_correspondences, + doppler_outlier_threshold, + geometric_robust_loss_min_iteration, + doppler_robust_loss_min_iteration, + std::move(geometric_kernel), + std::move(doppler_kernel)); + }), + "lambda_doppler"_a, "prune_correspondences"_a, + "doppler_outlier_threshold"_a, + "geometric_robust_loss_min_iteration"_a, + "doppler_robust_loss_min_iteration"_a, "goemetric_kernel"_a, + "doppler_kernel"_a) + .def(py::init([](double lambda_doppler) { + return new TransformationEstimationForDopplerICP( + lambda_doppler); + }), + "lambda_doppler"_a) + .def("compute_transformation", + py::overload_cast &, + const double, const Eigen::Matrix4d &, + const Eigen::Matrix4d &, const size_t>( + &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("prune_correspondences", + &TransformationEstimationForDopplerICP:: + prune_correspondences_, + "Performs dynamic point outlier rejection of " + "correspondences") + .def_readwrite("doppler_outlier_threshold", + &TransformationEstimationForDopplerICP:: + doppler_outlier_threshold_, + "doppler_outlier_threshold") + .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"); + // open3d.registration.TransformationEstimationForGeneralizedICP: // TransformationEstimation py::class_ "(``TransformationEstimationPointToPoint``, " "``TransformationEstimationPointToPlane``, " "``TransformationEstimationForGeneralizedICP``, " + "``TransformationEstimationForDopplerICP``, " "``TransformationEstimationForColoredICP``)"}, {"init", "Initial transformation estimation"}, + {"lambda_doppler", "lambda_doppler value"}, {"lambda_geometric", "lambda_geometric value"}, {"epsilon", "epsilon value"}, {"kernel", "Robust Kernel used in the Optimization"}, @@ -607,10 +711,20 @@ static const std::unordered_map "Enables mutual filter such that the correspondence of the " "source point's correspondence is itself."}, {"option", "Registration option"}, + {"period", + "Time period (in seconds) between the source and the target " + "point clouds."}, + {"prune_correspondences", + "Prune dynamic point outlier correspondences."}, {"ransac_n", "Fit ransac with ``ransac_n`` correspondences"}, {"seed", "Random seed."}, {"source_feature", "Source point cloud feature."}, {"source", "The source point cloud."}, + {"source_directions", + "Direction vectors for the source point cloud."}, + {"T_V_to_S", + "The 4x4 transformation matrix to transform ``sensor`` to " + "``vehicle`` frame."}, {"target_feature", "Target point cloud feature."}, {"target", "The target point cloud."}, {"transformation", @@ -626,6 +740,14 @@ void pybind_registration_methods(py::module &m) { docstring::FunctionDocInject(m, "evaluate_registration", map_shared_argument_docstrings); + m.def("get_registration_result_and_correspondences", + &GetRegistrationResultAndCorrespondences, + py::call_guard(), + "Function for evaluating registration between point clouds", + "source"_a, "target"_a, "target_kdtree"_a, + "max_correspondence_distance"_a, + "transformation"_a = Eigen::Matrix4d::Identity()); + m.def("registration_icp", &RegistrationICP, py::call_guard(), "Function for ICP registration", "source"_a, "target"_a, @@ -646,6 +768,17 @@ void pybind_registration_methods(py::module &m) { docstring::FunctionDocInject(m, "registration_colored_icp", map_shared_argument_docstrings); + m.def("registration_doppler_icp", &RegistrationDopplerICP, + py::call_guard(), + "Function for Doppler ICP registration", "source"_a, "target"_a, + "source_directions"_a, "max_correspondence_distance"_a, + "init"_a = Eigen::Matrix4d::Identity(), + "estimation_method"_a = TransformationEstimationForDopplerICP(0.99), + "criteria"_a = ICPConvergenceCriteria(), "period"_a = 0.1F, + "T_V_to_S"_a = Eigen::Matrix4d::Identity()); + docstring::FunctionDocInject(m, "registration_doppler_icp", + map_shared_argument_docstrings); + m.def("registration_generalized_icp", &RegistrationGeneralizedICP, "Function for Generalized ICP registration", "source"_a, "target"_a, "max_correspondence_distance"_a, diff --git a/examples/cpp/CMakeLists.txt b/examples/cpp/CMakeLists.txt index 832e4d5ff1b..55b3a70da93 100644 --- a/examples/cpp/CMakeLists.txt +++ b/examples/cpp/CMakeLists.txt @@ -52,6 +52,11 @@ open3d_add_example(PoseGraph) open3d_add_example(ProgramOptions) open3d_add_example(GeneralizedICP) open3d_add_example(RegistrationColoredICP) +if (TARGET Open3D::3rdparty_openmp) + open3d_add_example(RegistrationDopplerICP Open3D::3rdparty_openmp) +else() + open3d_add_example(RegistrationDopplerICP) +endif() open3d_add_example(RegistrationRANSAC) open3d_add_example(RGBDOdometry) open3d_add_example(SLAC) diff --git a/examples/cpp/RegistrationDopplerICP.cpp b/examples/cpp/RegistrationDopplerICP.cpp new file mode 100644 index 00000000000..de11451543b --- /dev/null +++ b/examples/cpp/RegistrationDopplerICP.cpp @@ -0,0 +1,145 @@ +// ---------------------------------------------------------------------------- +// - 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 "open3d/Open3D.h" + +using namespace open3d; +using namespace open3d::pipelines::registration; + +std::vector ComputeDirectionVectors( + const geometry::PointCloud &pcd) { + utility::LogDebug("ComputeDirectionVectors"); + + std::vector directions; + size_t n_points = pcd.points_.size(); + directions.resize(n_points, Eigen::Vector3d::Zero()); + +#pragma omp parallel for schedule(static) + for (int i = 0; i < (int)pcd.points_.size(); ++i) { + directions[i] = pcd.points_[i].normalized(); + } + + return directions; +} + +void VisualizeRegistration(const open3d::geometry::PointCloud &source, + const open3d::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"); +} + +void PrintHelp() { + using namespace open3d; + + PrintOpen3DVersion(); + // clang-format off + utility::LogInfo("Usage:"); + utility::LogInfo(" > RegistrationDopplerICP source_pcd target_pcd [--visualize]"); + // clang-format on + utility::LogInfo(""); +} + +int main(int argc, char *argv[]) { + using namespace open3d; + + 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; + } + + // Prepare input. + std::shared_ptr source = + open3d::io::CreatePointCloudFromFile(argv[1]); + std::shared_ptr target = + open3d::io::CreatePointCloudFromFile(argv[2]); + if (source == nullptr || target == nullptr) { + utility::LogWarning("Unable to load source or target file."); + return -1; + } + + // Configure DICP parameters. + const double max_neighbor_distance = {0.3}; + const double lambda_doppler = {0.01}; + const bool prune_correspondences = {false}; + const double doppler_outlier_threshold = {2.0}; + const size_t geometric_robust_loss_min_iteration = {0}; + const size_t doppler_robust_loss_min_iteration = {2}; + const double period = {0.1}; // seconds + const double convergence_threshold = {1e-6}; + const size_t max_iters = {200}; + + std::shared_ptr geometric_kernel = + std::make_shared(0.5); + std::shared_ptr doppler_kernel = + std::make_shared(0.5); + + std::vector source_directions = + ComputeDirectionVectors(*source); + + Eigen::Matrix4d transform = Eigen::Matrix4d::Identity(); + auto result = RegistrationDopplerICP( + *source, *target, source_directions, max_neighbor_distance, + transform, + TransformationEstimationForDopplerICP( + lambda_doppler, prune_correspondences, + doppler_outlier_threshold, + geometric_robust_loss_min_iteration, + doppler_robust_loss_min_iteration, geometric_kernel, + doppler_kernel), + ICPConvergenceCriteria(convergence_threshold, convergence_threshold, + max_iters), + period, transform); + transform = result.transformation_; + + std::stringstream ss; + ss << transform; + utility::LogInfo("Final transformation = \n{}", ss.str()); + + if (visualize) { + VisualizeRegistration(*source, *target, transform); + } + + return 0; +} From e073ae5688fe8020efab5a4da39b1608ddd1fe4e Mon Sep 17 00:00:00 2001 From: Heethesh Vhavle Date: Sat, 28 May 2022 18:50:33 -0700 Subject: [PATCH 2/7] Doppler ICP python example and clean-up (#2) --- cpp/open3d/geometry/PointCloud.h | 2 +- cpp/open3d/io/CMakeLists.txt | 2 +- cpp/open3d/io/FileFormatIO.cpp | 2 +- cpp/open3d/io/FileFormatIO.h | 2 +- cpp/open3d/io/PointCloudIO.cpp | 4 +- cpp/open3d/io/PointCloudIO.h | 16 ++-- .../{FileXYZND.cpp => FileXYZD.cpp} | 40 ++++---- .../pipelines/registration/DopplerICP.cpp | 2 +- .../pipelines/registration/DopplerICP.h | 9 +- .../pipelines/registration/Registration.h | 1 - .../pipelines/registration/registration.cpp | 4 +- examples/python/pipelines/doppler_icp.py | 91 +++++++++++++++++++ 12 files changed, 132 insertions(+), 43 deletions(-) rename cpp/open3d/io/file_format/{FileXYZND.cpp => FileXYZD.cpp} (71%) create mode 100644 examples/python/pipelines/doppler_icp.py diff --git a/cpp/open3d/geometry/PointCloud.h b/cpp/open3d/geometry/PointCloud.h index 8193a90db42..3ffe3daeefc 100644 --- a/cpp/open3d/geometry/PointCloud.h +++ b/cpp/open3d/geometry/PointCloud.h @@ -64,7 +64,7 @@ class PointCloud : public Geometry3D { /// \brief Parameterized Constructor. /// /// \param points Points coordinates. - /// \param dopplers Doppler velocitues. + /// \param dopplers Doppler velocities. PointCloud(const std::vector &points, const std::vector &dopplers) : Geometry3D(Geometry::GeometryType::PointCloud), diff --git a/cpp/open3d/io/CMakeLists.txt b/cpp/open3d/io/CMakeLists.txt index eef2c7659e1..adef625f251 100644 --- a/cpp/open3d/io/CMakeLists.txt +++ b/cpp/open3d/io/CMakeLists.txt @@ -32,8 +32,8 @@ target_sources(io PRIVATE file_format/FileSTL.cpp file_format/FileTUM.cpp file_format/FileXYZ.cpp + file_format/FileXYZD.cpp file_format/FileXYZN.cpp - file_format/FileXYZND.cpp file_format/FileXYZRGB.cpp ) diff --git a/cpp/open3d/io/FileFormatIO.cpp b/cpp/open3d/io/FileFormatIO.cpp index a22b4692dfd..9421f913a30 100644 --- a/cpp/open3d/io/FileFormatIO.cpp +++ b/cpp/open3d/io/FileFormatIO.cpp @@ -44,8 +44,8 @@ static std::map gExt2Func = { {"pts", ReadFileGeometryTypePTS}, {"stl", ReadFileGeometryTypeSTL}, {"xyz", ReadFileGeometryTypeXYZ}, + {"xyzd", ReadFileGeometryTypeXYZD}, {"xyzn", ReadFileGeometryTypeXYZN}, - {"xyznd", ReadFileGeometryTypeXYZND}, {"xyzrgb", ReadFileGeometryTypeXYZRGB}, }; diff --git a/cpp/open3d/io/FileFormatIO.h b/cpp/open3d/io/FileFormatIO.h index 17feadcff3f..fb1c0ff4246 100644 --- a/cpp/open3d/io/FileFormatIO.h +++ b/cpp/open3d/io/FileFormatIO.h @@ -52,8 +52,8 @@ FileGeometry ReadFileGeometryTypePLY(const std::string& path); FileGeometry ReadFileGeometryTypePTS(const std::string& path); FileGeometry ReadFileGeometryTypeSTL(const std::string& path); FileGeometry ReadFileGeometryTypeXYZ(const std::string& path); +FileGeometry ReadFileGeometryTypeXYZD(const std::string& path); FileGeometry ReadFileGeometryTypeXYZN(const std::string& path); -FileGeometry ReadFileGeometryTypeXYZND(const std::string& path); FileGeometry ReadFileGeometryTypeXYZRGB(const std::string& path); } // namespace io diff --git a/cpp/open3d/io/PointCloudIO.cpp b/cpp/open3d/io/PointCloudIO.cpp index 1c31f66aa0a..5d51199ed66 100644 --- a/cpp/open3d/io/PointCloudIO.cpp +++ b/cpp/open3d/io/PointCloudIO.cpp @@ -44,8 +44,8 @@ static const std::unordered_map< const ReadPointCloudOption &)>> file_extension_to_pointcloud_read_function{ {"xyz", ReadPointCloudFromXYZ}, + {"xyzd", ReadPointCloudFromXYZD}, {"xyzn", ReadPointCloudFromXYZN}, - {"xyznd", ReadPointCloudFromXYZND}, {"xyzrgb", ReadPointCloudFromXYZRGB}, {"ply", ReadPointCloudFromPLY}, {"pcd", ReadPointCloudFromPCD}, @@ -59,8 +59,8 @@ static const std::unordered_map< const WritePointCloudOption &)>> file_extension_to_pointcloud_write_function{ {"xyz", WritePointCloudToXYZ}, + {"xyzd", WritePointCloudToXYZD}, {"xyzn", WritePointCloudToXYZN}, - {"xyznd", WritePointCloudToXYZND}, {"xyzrgb", WritePointCloudToXYZRGB}, {"ply", WritePointCloudToPLY}, {"pcd", WritePointCloudToPCD}, diff --git a/cpp/open3d/io/PointCloudIO.h b/cpp/open3d/io/PointCloudIO.h index c0173f8103b..a09d1226963 100644 --- a/cpp/open3d/io/PointCloudIO.h +++ b/cpp/open3d/io/PointCloudIO.h @@ -147,21 +147,21 @@ bool WritePointCloudToXYZ(const std::string &filename, const geometry::PointCloud &pointcloud, const WritePointCloudOption ¶ms); -bool ReadPointCloudFromXYZN(const std::string &filename, +bool ReadPointCloudFromXYZD(const std::string &filename, geometry::PointCloud &pointcloud, const ReadPointCloudOption ¶ms); -bool WritePointCloudToXYZN(const std::string &filename, +bool WritePointCloudToXYZD(const std::string &filename, const geometry::PointCloud &pointcloud, const WritePointCloudOption ¶ms); -bool ReadPointCloudFromXYZND(const std::string &filename, - geometry::PointCloud &pointcloud, - const ReadPointCloudOption ¶ms); +bool ReadPointCloudFromXYZN(const std::string &filename, + geometry::PointCloud &pointcloud, + const ReadPointCloudOption ¶ms); -bool WritePointCloudToXYZND(const std::string &filename, - const geometry::PointCloud &pointcloud, - const WritePointCloudOption ¶ms); +bool WritePointCloudToXYZN(const std::string &filename, + const geometry::PointCloud &pointcloud, + const WritePointCloudOption ¶ms); bool ReadPointCloudFromXYZRGB(const std::string &filename, geometry::PointCloud &pointcloud, diff --git a/cpp/open3d/io/file_format/FileXYZND.cpp b/cpp/open3d/io/file_format/FileXYZD.cpp similarity index 71% rename from cpp/open3d/io/file_format/FileXYZND.cpp rename to cpp/open3d/io/file_format/FileXYZD.cpp index 2bd2f2bbf9b..cb635a646f6 100644 --- a/cpp/open3d/io/file_format/FileXYZND.cpp +++ b/cpp/open3d/io/file_format/FileXYZD.cpp @@ -35,17 +35,17 @@ namespace open3d { namespace io { -FileGeometry ReadFileGeometryTypeXYZND(const std::string &path) { +FileGeometry ReadFileGeometryTypeXYZD(const std::string &path) { return CONTAINS_POINTS; } -bool ReadPointCloudFromXYZND(const std::string &filename, - geometry::PointCloud &pointcloud, - const ReadPointCloudOption ¶ms) { +bool ReadPointCloudFromXYZD(const std::string &filename, + geometry::PointCloud &pointcloud, + const ReadPointCloudOption ¶ms) { try { utility::filesystem::CFile file; if (!file.Open(filename, "r")) { - utility::LogWarning("Read XYZND failed: unable to open file: {}", + utility::LogWarning("Read XYZD failed: unable to open file: {}", filename); return false; } @@ -54,13 +54,12 @@ bool ReadPointCloudFromXYZND(const std::string &filename, pointcloud.Clear(); int i = 0; - double x, y, z, nx, ny, nz, doppler; + double x, y, z, doppler; const char *line_buffer; while ((line_buffer = file.ReadLine())) { - if (sscanf(line_buffer, "%lf %lf %lf %lf %lf %lf %lf", &x, &y, &z, - &nx, &ny, &nz, &doppler) == 7) { + if (sscanf(line_buffer, "%lf %lf %lf %lf", &x, &y, &z, &doppler) == + 4) { pointcloud.points_.push_back(Eigen::Vector3d(x, y, z)); - pointcloud.normals_.push_back(Eigen::Vector3d(nx, ny, nz)); pointcloud.dopplers_.push_back(doppler); } if (++i % 1000 == 0) { @@ -71,22 +70,22 @@ bool ReadPointCloudFromXYZND(const std::string &filename, return true; } catch (const std::exception &e) { - utility::LogWarning("Read XYZND failed with exception: {}", e.what()); + utility::LogWarning("Read XYZD failed with exception: {}", e.what()); return false; } } -bool WritePointCloudToXYZND(const std::string &filename, - const geometry::PointCloud &pointcloud, - const WritePointCloudOption ¶ms) { - if (!pointcloud.HasNormals() || !pointcloud.HasDopplers()) { +bool WritePointCloudToXYZD(const std::string &filename, + const geometry::PointCloud &pointcloud, + const WritePointCloudOption ¶ms) { + if (!pointcloud.HasDopplers()) { return false; } try { utility::filesystem::CFile file; if (!file.Open(filename, "w")) { - utility::LogWarning("Write XYZND failed: unable to open file: {}", + utility::LogWarning("Write XYZD failed: unable to open file: {}", filename); return false; } @@ -95,14 +94,11 @@ bool WritePointCloudToXYZND(const std::string &filename, for (size_t i = 0; i < pointcloud.points_.size(); i++) { const Eigen::Vector3d &point = pointcloud.points_[i]; - const Eigen::Vector3d &normal = pointcloud.normals_[i]; const double &doppler = pointcloud.dopplers_[i]; - if (fprintf(file.GetFILE(), - "%.10f %.10f %.10f %.10f %.10f %.10f %.10f\n", point(0), - point(1), point(2), normal(0), normal(1), normal(2), - doppler) < 0) { + if (fprintf(file.GetFILE(), "%.10f %.10f %.10f %.10f\n", point(0), + point(1), point(2), doppler) < 0) { utility::LogWarning( - "Write XYZND failed: unable to write file: {}", + "Write XYZD failed: unable to write file: {}", filename); return false; // error happened during writing. } @@ -113,7 +109,7 @@ bool WritePointCloudToXYZND(const std::string &filename, reporter.Finish(); return true; } catch (const std::exception &e) { - utility::LogWarning("Write XYZND failed with exception: {}", e.what()); + utility::LogWarning("Write XYZD failed with exception: {}", e.what()); return false; } } diff --git a/cpp/open3d/pipelines/registration/DopplerICP.cpp b/cpp/open3d/pipelines/registration/DopplerICP.cpp index b6f5fae999b..6953e36b49c 100644 --- a/cpp/open3d/pipelines/registration/DopplerICP.cpp +++ b/cpp/open3d/pipelines/registration/DopplerICP.cpp @@ -75,7 +75,7 @@ Eigen::Matrix4d TransformationEstimationForDopplerICP::ComputeTransformation( "DopplerICP requires source pointcloud to have Dopplers."); } if (std::abs(period) < 1e-3) { - utility::LogError("Time period too small"); + utility::LogError("Time period too small."); } const double lambda_geometric = 1.0 - lambda_doppler_; diff --git a/cpp/open3d/pipelines/registration/DopplerICP.h b/cpp/open3d/pipelines/registration/DopplerICP.h index 2b192180347..8ff07f60826 100644 --- a/cpp/open3d/pipelines/registration/DopplerICP.h +++ b/cpp/open3d/pipelines/registration/DopplerICP.h @@ -51,8 +51,9 @@ class TransformationEstimationForDopplerICP : public TransformationEstimation { public: ~TransformationEstimationForDopplerICP() override{}; - /// \brief Constructor that takes as input a RobustKernel \param kernel Any - /// of the implemented statistical robust kernel for outlier rejection. + /// \brief Constructor that takes as input a RobustKernel + /// \param kernel Any of the implemented statistical robust kernel for + /// outlier rejection. explicit TransformationEstimationForDopplerICP( double lambda_doppler = 0.01, bool prune_correspondences = false, @@ -123,6 +124,10 @@ class TransformationEstimationForDopplerICP : public TransformationEstimation { /// \brief Function for Doppler ICP registration. /// +/// This is the implementation of the following paper: +/// B. Hexsel, H. Vhavle, Y. Chen, +/// DICP: Doppler Iterative Closest Point Algorithm, RSS 2022. +/// /// \param source The source point cloud. /// \param target The target point cloud. /// \param max_distance Maximum correspondence points-pair distance (meters). diff --git a/cpp/open3d/pipelines/registration/Registration.h b/cpp/open3d/pipelines/registration/Registration.h index 095448d5f51..285faf6bc67 100644 --- a/cpp/open3d/pipelines/registration/Registration.h +++ b/cpp/open3d/pipelines/registration/Registration.h @@ -149,7 +149,6 @@ class RegistrationResult { bool converged_; /// Number of iterations the algorithm took to converge. size_t num_iterations_; - std::vector errors_; }; /// \brief Helper function for evaluating registration between point clouds. diff --git a/cpp/pybind/pipelines/registration/registration.cpp b/cpp/pybind/pipelines/registration/registration.cpp index b6c1094fa06..0af7bedf4c2 100644 --- a/cpp/pybind/pipelines/registration/registration.cpp +++ b/cpp/pybind/pipelines/registration/registration.cpp @@ -323,7 +323,7 @@ Sets :math:`c = 1` if ``with_scaling`` is ``False``. "lambda_doppler"_a, "prune_correspondences"_a, "doppler_outlier_threshold"_a, "geometric_robust_loss_min_iteration"_a, - "doppler_robust_loss_min_iteration"_a, "goemetric_kernel"_a, + "doppler_robust_loss_min_iteration"_a, "geometric_kernel"_a, "doppler_kernel"_a) .def(py::init([](double lambda_doppler) { return new TransformationEstimationForDopplerICP( @@ -656,8 +656,6 @@ must hold true for all edges.)"); .def_readwrite( "num_iterations", &RegistrationResult::num_iterations_, "int: Number of iterations the algorithm took to converge.") - .def_readwrite("errors", &RegistrationResult::errors_, - "error vector") .def("__repr__", [](const RegistrationResult &rr) { return fmt::format( "RegistrationResult with " diff --git a/examples/python/pipelines/doppler_icp.py b/examples/python/pipelines/doppler_icp.py new file mode 100644 index 00000000000..95aea309e24 --- /dev/null +++ b/examples/python/pipelines/doppler_icp.py @@ -0,0 +1,91 @@ +# ---------------------------------------------------------------------------- +# - 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. +# ---------------------------------------------------------------------------- +"""Doppler ICP (Iterative Closest Point) registration algorithm""" + +import argparse +import copy + +import open3d as o3d +import numpy as np + + +def draw_registration_result(source, target, transformation): + source_temp = copy.deepcopy(source) + target_temp = copy.deepcopy(target) + source_temp.paint_uniform_color([1, 0.706, 0]) + target_temp.paint_uniform_color([0, 0.651, 0.929]) + source_temp.transform(transformation) + o3d.visualization.draw([source_temp, target_temp]) + + +if __name__ == '__main__': + parser = argparse.ArgumentParser() + parser.add_argument('source', type=str, + help='Path to source point cloud in XYZD format') + parser.add_argument('target', type=str, + help='Path to target point cloud in XYZD format') + + args = parser.parse_args() + + source = o3d.io.read_point_cloud(args.source) + target = o3d.io.read_point_cloud(args.target) + + # Compute direction vectors for the source point cloud. + points = np.asarray(source.points) + directions = points / np.linalg.norm(points, axis=1)[:, np.newaxis] + source_directions = o3d.utility.Vector3dVector(directions) + + # Compute normal vectors for the target point cloud. + target.estimate_normals( + o3d.geometry.KDTreeSearchParamHybrid(radius=10.0, max_nn=30)) + + # This is the implementation of the following paper: + # B. Hexsel, H. Vhavle, Y. Chen, + # DICP: Doppler Iterative Closest Point Algorithm, RSS 2022. + print('Apply point-to-plane Doppler ICP') + max_corr_distance = 0.3 # meters + init_transform = np.eye(4) + result = o3d.pipelines.registration.registration_doppler_icp( + source, target, source_directions, max_corr_distance, init_transform, + o3d.pipelines.registration.TransformationEstimationForDopplerICP( + lambda_doppler=0.01, + prune_correspondences=False, + doppler_outlier_threshold=2.0, # m/s + geometric_robust_loss_min_iteration=0, + doppler_robust_loss_min_iteration=2, + geometric_kernel=o3d.pipelines.registration.TukeyLoss(k=0.5), + doppler_kernel=o3d.pipelines.registration.TukeyLoss(k=0.5)), + o3d.pipelines.registration.ICPConvergenceCriteria( + relative_fitness=1e-6, + relative_rmse=1e-6, + max_iteration=50), + period=0.1, # seconds + T_V_to_S=np.eye(4), # vehicle-to-sensor extrinsic calibration + ) + print(result) + print('Transformation is:') + print(result.transformation, "\n") + draw_registration_result(source, target, result.transformation) From 6f4c1c35df827c0b9d5eacbf0277379fda8e5f17 Mon Sep 17 00:00:00 2001 From: Heethesh Vhavle Date: Thu, 2 Jun 2022 17:24:01 -0700 Subject: [PATCH 3/7] Rename outlier rejection parameters and add DPOR min iterations (#3) --- .../pipelines/registration/DopplerICP.cpp | 9 ++-- .../pipelines/registration/DopplerICP.h | 10 +++-- .../pipelines/registration/registration.cpp | 43 +++++++++++-------- examples/cpp/RegistrationDopplerICP.cpp | 7 +-- examples/python/pipelines/doppler_icp.py | 3 +- 5 files changed, 43 insertions(+), 29 deletions(-) diff --git a/cpp/open3d/pipelines/registration/DopplerICP.cpp b/cpp/open3d/pipelines/registration/DopplerICP.cpp index 6953e36b49c..b5c34672185 100644 --- a/cpp/open3d/pipelines/registration/DopplerICP.cpp +++ b/cpp/open3d/pipelines/registration/DopplerICP.cpp @@ -115,14 +115,16 @@ Eigen::Matrix4d TransformationEstimationForDopplerICP::ComputeTransformation( // Dynamic point outlier pruning of correspondences. bool optimize{true}; - if (prune_correspondences_ && + if (reject_dynamic_outliers_ && + iteration >= outlier_rejection_min_iteration_ && std::abs(doppler_error) > doppler_outlier_threshold_) { optimize = false; } if (optimize) { // Compute geometric point-to-plane error and Jacobian. - const double geometric_error = (ps_in_V - pt_in_V).dot(nt_in_V); + const double geometric_error = + (ps_in_V - pt_in_V).dot(nt_in_V); r[0] = sqrt_lambda_geometric * geometric_error; w[0] = (iteration >= geometric_robust_loss_min_iteration_) ? geometric_kernel_->Weight(r[0]) @@ -138,7 +140,8 @@ Eigen::Matrix4d TransformationEstimationForDopplerICP::ComputeTransformation( : default_kernel_->Weight(r[1]); J_r[1].block<3, 1>(0, 0) = sqrt_lambda_doppler_by_dt * ds_in_V.cross(r_v_to_s_in_V); - J_r[1].block<3, 1>(3, 0) = sqrt_lambda_doppler_by_dt * -ds_in_V; + J_r[1].block<3, 1>(3, 0) = + sqrt_lambda_doppler_by_dt * -ds_in_V; } else { r[0] = 0.F; w[0] = 0.F; diff --git a/cpp/open3d/pipelines/registration/DopplerICP.h b/cpp/open3d/pipelines/registration/DopplerICP.h index 8ff07f60826..a5ccce599df 100644 --- a/cpp/open3d/pipelines/registration/DopplerICP.h +++ b/cpp/open3d/pipelines/registration/DopplerICP.h @@ -56,8 +56,9 @@ class TransformationEstimationForDopplerICP : public TransformationEstimation { /// outlier rejection. explicit TransformationEstimationForDopplerICP( double lambda_doppler = 0.01, - bool prune_correspondences = false, + 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, std::shared_ptr geometric_kernel = @@ -65,8 +66,9 @@ class TransformationEstimationForDopplerICP : public TransformationEstimation { std::shared_ptr doppler_kernel = std::make_shared()) : lambda_doppler_(lambda_doppler), - prune_correspondences_(prune_correspondences), + 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), @@ -103,10 +105,12 @@ class TransformationEstimationForDopplerICP : public TransformationEstimation { /// 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 prune_correspondences_{false}; + 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}; diff --git a/cpp/pybind/pipelines/registration/registration.cpp b/cpp/pybind/pipelines/registration/registration.cpp index 0af7bedf4c2..d16dab3b79d 100644 --- a/cpp/pybind/pipelines/registration/registration.cpp +++ b/cpp/pybind/pipelines/registration/registration.cpp @@ -305,23 +305,25 @@ Sets :math:`c = 1` if ``with_scaling`` is ``False``. te_dop); py::detail::bind_copy_functions( te_dop); - te_dop.def(py::init([](double lambda_doppler, bool prune_correspondences, + 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, std::shared_ptr geometric_kernel, std::shared_ptr doppler_kernel) { return new TransformationEstimationForDopplerICP( - lambda_doppler, - prune_correspondences, + lambda_doppler, reject_dynamic_outliers, doppler_outlier_threshold, + outlier_rejection_min_iteration, geometric_robust_loss_min_iteration, doppler_robust_loss_min_iteration, std::move(geometric_kernel), std::move(doppler_kernel)); }), - "lambda_doppler"_a, "prune_correspondences"_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, "geometric_kernel"_a, "doppler_kernel"_a) @@ -353,27 +355,30 @@ Sets :math:`c = 1` if ``with_scaling`` is ``False``. "lambda_doppler", &TransformationEstimationForDopplerICP::lambda_doppler_, "lambda_doppler") - .def_readwrite("prune_correspondences", + .def_readwrite("reject_dynamic_outliers", &TransformationEstimationForDopplerICP:: - prune_correspondences_, + reject_dynamic_outliers_, "Performs dynamic point outlier rejection of " "correspondences") .def_readwrite("doppler_outlier_threshold", &TransformationEstimationForDopplerICP:: doppler_outlier_threshold_, "doppler_outlier_threshold") - .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("outlier_rejection_min_iteration", + &TransformationEstimationForDopplerICP:: + outlier_rejection_min_iteration_, + "Minimum iterations after which the dynamic point " + "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_, @@ -708,7 +713,7 @@ static const std::unordered_map {"period", "Time period (in seconds) between the source and the target " "point clouds."}, - {"prune_correspondences", + {"reject_dynamic_outliers", "Prune dynamic point outlier correspondences."}, {"ransac_n", "Fit ransac with ``ransac_n`` correspondences"}, {"seed", "Random seed."}, diff --git a/examples/cpp/RegistrationDopplerICP.cpp b/examples/cpp/RegistrationDopplerICP.cpp index de11451543b..7d60e5760f8 100644 --- a/examples/cpp/RegistrationDopplerICP.cpp +++ b/examples/cpp/RegistrationDopplerICP.cpp @@ -102,8 +102,9 @@ int main(int argc, char *argv[]) { // Configure DICP parameters. const double max_neighbor_distance = {0.3}; const double lambda_doppler = {0.01}; - const bool prune_correspondences = {false}; + const bool reject_dynamic_outliers = {false}; const double doppler_outlier_threshold = {2.0}; + const size_t outlier_rejection_min_iteration = {2}; const size_t geometric_robust_loss_min_iteration = {0}; const size_t doppler_robust_loss_min_iteration = {2}; const double period = {0.1}; // seconds @@ -123,8 +124,8 @@ int main(int argc, char *argv[]) { *source, *target, source_directions, max_neighbor_distance, transform, TransformationEstimationForDopplerICP( - lambda_doppler, prune_correspondences, - doppler_outlier_threshold, + 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), diff --git a/examples/python/pipelines/doppler_icp.py b/examples/python/pipelines/doppler_icp.py index 95aea309e24..a429d7ed03d 100644 --- a/examples/python/pipelines/doppler_icp.py +++ b/examples/python/pipelines/doppler_icp.py @@ -72,8 +72,9 @@ def draw_registration_result(source, target, transformation): source, target, source_directions, max_corr_distance, init_transform, o3d.pipelines.registration.TransformationEstimationForDopplerICP( lambda_doppler=0.01, - prune_correspondences=False, + reject_dynamic_outliers=False, doppler_outlier_threshold=2.0, # m/s + outlier_rejection_min_iteration=2, geometric_robust_loss_min_iteration=0, doppler_robust_loss_min_iteration=2, geometric_kernel=o3d.pipelines.registration.TukeyLoss(k=0.5), From 6732968cfca2a0772c93694989d1d6c72c59324d Mon Sep 17 00:00:00 2001 From: Heethesh Vhavle Date: Tue, 14 Jun 2022 17:05:56 -0700 Subject: [PATCH 4/7] Update CHANGELOG.md --- CHANGELOG.md | 1 + 1 file changed, 1 insertion(+) diff --git a/CHANGELOG.md b/CHANGELOG.md index eb5c7e7ee51..3b3ad9f2930 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -4,6 +4,7 @@ * Fix undefined names: docstr and VisibleDeprecationWarning (PR #3844) * Corrected documentation for Tensor based PointClound, LineSet, TriangleMesh (PR #4685) * Corrected documentation for KDTree (typo in Notebook) (PR #4744) +* Added Doppler channel in point cloud and Doppler ICP algorithm in registration pipeline (PR #1) ## 0.13 From f0bb0cbe6f5404d406438a5f73aa7a69929382a4 Mon Sep 17 00:00:00 2001 From: Heethesh Vhavle Date: Sun, 19 Jun 2022 19:10:19 -0700 Subject: [PATCH 5/7] update PR# in changelog --- CHANGELOG.md | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/CHANGELOG.md b/CHANGELOG.md index 7020af37bdb..da1d856e143 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -6,7 +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) -* Added Doppler channel in point cloud and Doppler ICP algorithm in registration pipeline (PR #1) +* Added Doppler channel in point cloud and Doppler ICP algorithm in registration pipeline (PR #5220) ## 0.13 From c02dd86be8c3eec8739b7cc51d51be4488768c17 Mon Sep 17 00:00:00 2001 From: Heethesh Vhavle Date: Mon, 20 Jun 2022 21:42:51 -0700 Subject: [PATCH 6/7] style check --- cpp/pybind/geometry/pointcloud.cpp | 3 ++- examples/python/pipelines/doppler_icp.py | 21 +++++++++++++-------- 2 files changed, 15 insertions(+), 9 deletions(-) diff --git a/cpp/pybind/geometry/pointcloud.cpp b/cpp/pybind/geometry/pointcloud.cpp index 83b1eb58958..d4bd0b6ddde 100644 --- a/cpp/pybind/geometry/pointcloud.cpp +++ b/cpp/pybind/geometry/pointcloud.cpp @@ -64,7 +64,8 @@ void pybind_pointcloud(py::module &m) { .def("has_colors", &PointCloud::HasColors, "Returns ``True`` if the point cloud contains point colors.") .def("has_dopplers", &PointCloud::HasDopplers, - "Returns ``True`` if the point cloud contains Doppler velocities.") + "Returns ``True`` if the point cloud contains Doppler " + "velocities.") .def("has_covariances", &PointCloud::HasCovariances, "Returns ``True`` if the point cloud contains covariances.") .def("normalize_normals", &PointCloud::NormalizeNormals, diff --git a/examples/python/pipelines/doppler_icp.py b/examples/python/pipelines/doppler_icp.py index a429d7ed03d..ba275ac6140 100644 --- a/examples/python/pipelines/doppler_icp.py +++ b/examples/python/pipelines/doppler_icp.py @@ -43,9 +43,11 @@ def draw_registration_result(source, target, transformation): if __name__ == '__main__': parser = argparse.ArgumentParser() - parser.add_argument('source', type=str, + parser.add_argument('source', + type=str, help='Path to source point cloud in XYZD format') - parser.add_argument('target', type=str, + parser.add_argument('target', + type=str, help='Path to target point cloud in XYZD format') args = parser.parse_args() @@ -69,7 +71,11 @@ def draw_registration_result(source, target, transformation): max_corr_distance = 0.3 # meters init_transform = np.eye(4) result = o3d.pipelines.registration.registration_doppler_icp( - source, target, source_directions, max_corr_distance, init_transform, + source, + target, + source_directions, + max_corr_distance, + init_transform, o3d.pipelines.registration.TransformationEstimationForDopplerICP( lambda_doppler=0.01, reject_dynamic_outliers=False, @@ -79,13 +85,12 @@ def draw_registration_result(source, target, transformation): doppler_robust_loss_min_iteration=2, geometric_kernel=o3d.pipelines.registration.TukeyLoss(k=0.5), doppler_kernel=o3d.pipelines.registration.TukeyLoss(k=0.5)), - o3d.pipelines.registration.ICPConvergenceCriteria( - relative_fitness=1e-6, - relative_rmse=1e-6, - max_iteration=50), + o3d.pipelines.registration.ICPConvergenceCriteria(relative_fitness=1e-6, + relative_rmse=1e-6, + max_iteration=50), period=0.1, # seconds T_V_to_S=np.eye(4), # vehicle-to-sensor extrinsic calibration - ) + ) print(result) print('Transformation is:') print(result.transformation, "\n") From dca238dab5f13484c3383b16ad8fa95c867f3c4a Mon Sep 17 00:00:00 2001 From: Heethesh Vhavle Date: Tue, 21 Jun 2022 00:25:13 -0700 Subject: [PATCH 7/7] reduce scope for r --- cpp/open3d/pipelines/registration/DopplerICP.cpp | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/cpp/open3d/pipelines/registration/DopplerICP.cpp b/cpp/open3d/pipelines/registration/DopplerICP.cpp index b5c34672185..e4cf1908ed3 100644 --- a/cpp/open3d/pipelines/registration/DopplerICP.cpp +++ b/cpp/open3d/pipelines/registration/DopplerICP.cpp @@ -175,10 +175,10 @@ double TransformationEstimationForDopplerICP::ComputeRMSE( const geometry::PointCloud &target, const CorrespondenceSet &corres) const { if (corres.empty() || !target.HasNormals()) return 0.0; - double err = 0.0, r; + double err = 0.0; for (const auto &c : corres) { - r = (source.points_[c[0]] - target.points_[c[1]]) - .dot(target.normals_[c[1]]); + const double r = (source.points_[c[0]] - target.points_[c[1]]) + .dot(target.normals_[c[1]]); err += r * r; } return std::sqrt(err / (double)corres.size());