Add Doppler ICP algorithm in registration pipeline and dopplers field in PointCloud

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)
+* Added Doppler channel in point cloud and Doppler ICP algorithm in registration pipeline (PR #5220)
 
 ## 0.13
 

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/FastGlobalRegistration.h"
 #include "open3d/pipelines/registration/Feature.h"
 #include "open3d/pipelines/registration/GeneralizedICP.h"

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,13 +185,15 @@ 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++;
         }
     }
     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> PointCloud::SelectByIndex(
     auto output = std::make_shared<PointCloud>();
     bool has_normals = HasNormals();
     bool has_colors = HasColors();
+    bool has_doppler = HasDopplers();
     bool has_covariance = HasCovariances();
 
     std::vector<bool> mask = std::vector<bool>(points_.size(), invert);
@@ -207,6 +219,7 @@ std::shared_ptr<PointCloud> 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> 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> 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());

cpp/open3d/geometry/PointCloud.h

@@ -61,6 +61,15 @@ class PointCloud : public Geometry3D {
     /// \param points Points coordinates.
     PointCloud(const std::vector<Eigen::Vector3d> &points)
         : Geometry3D(Geometry::GeometryType::PointCloud), points_(points) {}
+    /// \brief Parameterized Constructor.
+    ///
+    /// \param points Points coordinates.
+    /// \param dopplers Doppler velocities.
+    PointCloud(const std::vector<Eigen::Vector3d> &points,
+               const std::vector<double> &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();
@@ -433,6 +447,8 @@ class PointCloud : public Geometry3D {
     std::vector<Eigen::Vector3d> normals_;
     /// RGB colors of points.
     std::vector<Eigen::Vector3d> colors_;
+    /// Doppler velocity of points.
+    std::vector<double> dopplers_;
     /// Covariance Matrix for each point
     std::vector<Eigen::Matrix3d> covariances_;
 };

cpp/open3d/io/CMakeLists.txt

@@ -32,6 +32,7 @@ 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/FileXYZRGB.cpp
 )

cpp/open3d/io/FileFormatIO.cpp

@@ -44,6 +44,7 @@ static std::map<std::string, FileGeometry (*)(const std::string&)> gExt2Func = {
         {"pts", ReadFileGeometryTypePTS},
         {"stl", ReadFileGeometryTypeSTL},
         {"xyz", ReadFileGeometryTypeXYZ},
+        {"xyzd", ReadFileGeometryTypeXYZD},
         {"xyzn", ReadFileGeometryTypeXYZN},
         {"xyzrgb", ReadFileGeometryTypeXYZRGB},
 };

cpp/open3d/io/FileFormatIO.h

@@ -52,6 +52,7 @@ 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 ReadFileGeometryTypeXYZRGB(const std::string& path);
 

cpp/open3d/io/PointCloudIO.cpp

@@ -44,6 +44,7 @@ static const std::unordered_map<
                            const ReadPointCloudOption &)>>
         file_extension_to_pointcloud_read_function{
                 {"xyz", ReadPointCloudFromXYZ},
+                {"xyzd", ReadPointCloudFromXYZD},
                 {"xyzn", ReadPointCloudFromXYZN},
                 {"xyzrgb", ReadPointCloudFromXYZRGB},
                 {"ply", ReadPointCloudFromPLY},
@@ -58,6 +59,7 @@ static const std::unordered_map<
                            const WritePointCloudOption &)>>
         file_extension_to_pointcloud_write_function{
                 {"xyz", WritePointCloudToXYZ},
+                {"xyzd", WritePointCloudToXYZD},
                 {"xyzn", WritePointCloudToXYZN},
                 {"xyzrgb", WritePointCloudToXYZRGB},
                 {"ply", WritePointCloudToPLY},

cpp/open3d/io/PointCloudIO.h

@@ -147,6 +147,14 @@ bool WritePointCloudToXYZ(const std::string &filename,
                           const geometry::PointCloud &pointcloud,
                           const WritePointCloudOption &params);
 
+bool ReadPointCloudFromXYZD(const std::string &filename,
+                            geometry::PointCloud &pointcloud,
+                            const ReadPointCloudOption &params);
+
+bool WritePointCloudToXYZD(const std::string &filename,
+                           const geometry::PointCloud &pointcloud,
+                           const WritePointCloudOption &params);
+
 bool ReadPointCloudFromXYZN(const std::string &filename,
                             geometry::PointCloud &pointcloud,
                             const ReadPointCloudOption &params);

cpp/open3d/io/file_format/FileXYZD.cpp

@@ -0,0 +1,118 @@
+// ----------------------------------------------------------------------------
+// -                        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 <cstdio>
+
+#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 ReadFileGeometryTypeXYZD(const std::string &path) {
+    return CONTAINS_POINTS;
+}
+
+bool ReadPointCloudFromXYZD(const std::string &filename,
+                            geometry::PointCloud &pointcloud,
+                            const ReadPointCloudOption &params) {
+    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());
+
+        pointcloud.Clear();
+        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) {
+                pointcloud.points_.push_back(Eigen::Vector3d(x, y, z));
+                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 XYZD failed with exception: {}", e.what());
+        return false;
+    }
+}
+
+bool WritePointCloudToXYZD(const std::string &filename,
+                           const geometry::PointCloud &pointcloud,
+                           const WritePointCloudOption &params) {
+    if (!pointcloud.HasDopplers()) {
+        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);
+        reporter.SetTotal(pointcloud.points_.size());
+
+        for (size_t i = 0; i < pointcloud.points_.size(); i++) {
+            const Eigen::Vector3d &point = pointcloud.points_[i];
+            const double &doppler = pointcloud.dopplers_[i];
+            if (fprintf(file.GetFILE(), "%.10f %.10f %.10f %.10f\n", point(0),
+                        point(1), point(2), doppler) < 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 open3d

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