[Depreciating RegistrationColoredICP function]. Legacy ColoredICP Refactoring.

cpp/benchmarks/pipelines/registration/Registration.cpp

@@ -48,6 +48,11 @@ static const std::string source_pointcloud_filename =
 static const std::string target_pointcloud_filename =
         utility::GetDataPathCommon("ICP/cloud_bin_1.pcd");
 
+static const std::string source_colored_pcd_filename =
+        utility::GetDataPathCommon("ColoredICP/frag_115.ply");
+static const std::string target_colored_pcd_filename =
+        utility::GetDataPathCommon("ColoredICP/frag_116.ply");
+
 static const double voxel_downsampling_factor = 0.02;
 
 // ICP ConvergenceCriteria.
@@ -61,7 +66,7 @@ static const double max_correspondence_distance = 0.05;
 static std::tuple<geometry::PointCloud, geometry::PointCloud> LoadPointCloud(
         const std::string& source_filename,
         const std::string& target_filename,
-        const double voxel_downsample_factor) {
+        const double voxel_downsample_factor = -1) {
     geometry::PointCloud source;
     geometry::PointCloud target;
 
@@ -122,6 +127,37 @@ static void BenchmarkICPLegacy(benchmark::State& state,
                       reg_result.inlier_rmse_);
 }
 
+static void BenchmarkColoredICPLegacy(benchmark::State& state,
+                                      double voxel_size,
+                                      double iterations) {
+    geometry::PointCloud source;
+    geometry::PointCloud target;
+
+    std::tie(source, target) =
+            LoadPointCloud(source_colored_pcd_filename,
+                           target_colored_pcd_filename, voxel_size);
+
+    // TODO (@rishabh) Add MultiScale ColoredICP to Benchmarks.
+    // std::vector<double> voxel_sizes = {0.05, 0.05 / 2, 0.05 / 4};
+    // std::vector<int> iterations = {50, 30, 14};
+    Eigen::Matrix4d trans = Eigen::Matrix4d::Identity();
+
+    source.EstimateNormals(
+            open3d::geometry::KDTreeSearchParamHybrid(voxel_size * 2.0, 30));
+
+    target.EstimateNormals(
+            open3d::geometry::KDTreeSearchParamHybrid(voxel_size * 2.0, 30));
+    for (auto _ : state) {
+        auto result = pipelines::registration::RegistrationICP(
+                source, target, 0.07, trans,
+                pipelines::registration::
+                        TransformationEstimationForColoredICP(),
+                pipelines::registration::ICPConvergenceCriteria(1e-6, 1e-6,
+                                                                iterations));
+        trans = result.transformation_;
+    }
+}
+
 BENCHMARK_CAPTURE(BenchmarkICPLegacy,
                   PointToPlane / CPU,
                   TransformationEstimationType::PointToPlane)
@@ -132,6 +168,9 @@ BENCHMARK_CAPTURE(BenchmarkICPLegacy,
                   TransformationEstimationType::PointToPoint)
         ->Unit(benchmark::kMillisecond);
 
+BENCHMARK_CAPTURE(BenchmarkColoredICPLegacy, ColoredICP / CPU, 0.0125, 20)
+        ->Unit(benchmark::kMillisecond);
+
 }  // namespace registration
 }  // namespace pipelines
 }  // namespace open3d

cpp/open3d/Open3D.h.in

@@ -77,7 +77,6 @@
 #include "open3d/pipelines/integration/TSDFVolume.h"
 #include "open3d/pipelines/integration/UniformTSDFVolume.h"
 #include "open3d/pipelines/odometry/Odometry.h"
-#include "open3d/pipelines/registration/ColoredICP.h"
 #include "open3d/pipelines/registration/Feature.h"
 #include "open3d/pipelines/registration/GeneralizedICP.h"
 #include "open3d/pipelines/registration/Registration.h"

cpp/open3d/geometry/PointCloud.cpp

@@ -47,6 +47,7 @@ PointCloud &PointCloud::Clear() {
     points_.clear();
     normals_.clear();
     colors_.clear();
+    color_gradients_.clear();
     covariances_.clear();
     return *this;
 }
@@ -102,33 +103,42 @@ PointCloud &PointCloud::operator+=(const PointCloud &cloud) {
     // We do not use std::vector::insert to combine std::vector because it will
     // crash if the pointcloud is added to itself.
     if (cloud.IsEmpty()) return (*this);
-    size_t old_vert_num = points_.size();
-    size_t add_vert_num = cloud.points_.size();
-    size_t new_vert_num = old_vert_num + add_vert_num;
+    int old_vert_num = static_cast<int>(points_.size());
+    int add_vert_num = static_cast<int>(cloud.points_.size());
+    int new_vert_num = old_vert_num + add_vert_num;
     if ((!HasPoints() || HasNormals()) && cloud.HasNormals()) {
         normals_.resize(new_vert_num);
-        for (size_t i = 0; i < add_vert_num; i++)
+#pragma omp parallel for schedule(static)
+        for (int i = 0; i < add_vert_num; i++) {
             normals_[old_vert_num + i] = cloud.normals_[i];
+        }
     } else {
         normals_.clear();
     }
     if ((!HasPoints() || HasColors()) && cloud.HasColors()) {
         colors_.resize(new_vert_num);
-        for (size_t i = 0; i < add_vert_num; i++)
+#pragma omp parallel for schedule(static)
+        for (int i = 0; i < add_vert_num; i++) {
             colors_[old_vert_num + i] = cloud.colors_[i];
+        }
     } else {
         colors_.clear();
     }
-    if ((!HasPoints() || HasCovariances()) && cloud.HasCovariances()) {
-        covariances_.resize(new_vert_num);
-        for (size_t i = 0; i < add_vert_num; i++)
-            covariances_[old_vert_num + i] = cloud.covariances_[i];
+    if ((!HasPoints() || HasColorGradients()) && cloud.HasColorGradients()) {
+        colors_.resize(new_vert_num);
+#pragma omp parallel for schedule(static)
+        for (int i = 0; i < add_vert_num; i++) {
+            color_gradients_[old_vert_num + i] = cloud.color_gradients_[i];
+        }
     } else {
-        covariances_.clear();
+        color_gradients_.clear();
     }
     points_.resize(new_vert_num);
-    for (size_t i = 0; i < add_vert_num; i++)
+#pragma omp parallel for schedule(static)
+    for (int i = 0; i < add_vert_num; i++) {
         points_[old_vert_num + i] = cloud.points_[i];
+    }
+
     return (*this);
 }
 
@@ -160,9 +170,10 @@ std::vector<double> PointCloud::ComputePointCloudDistance(
 
 PointCloud &PointCloud::RemoveNonFinitePoints(bool remove_nan,
                                               bool remove_infinite) {
-    bool has_normal = HasNormals();
-    bool has_color = HasColors();
-    bool has_covariance = HasCovariances();
+    const bool has_normal = HasNormals();
+    const bool has_color = HasColors();
+    const bool has_color_gradient = HasColorGradients();
+    const bool has_covariance = HasCovariances();
     size_t old_point_num = points_.size();
     size_t k = 0;                                 // new index
     for (size_t i = 0; i < old_point_num; i++) {  // old index
@@ -176,6 +187,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_color_gradient) color_gradients_[k] = color_gradients_[i];
             if (has_covariance) covariances_[k] = covariances_[i];
             k++;
         }
@@ -193,9 +205,10 @@ PointCloud &PointCloud::RemoveNonFinitePoints(bool remove_nan,
 std::shared_ptr<PointCloud> PointCloud::SelectByIndex(
         const std::vector<size_t> &indices, bool invert /* = false */) const {
     auto output = std::make_shared<PointCloud>();
-    bool has_normals = HasNormals();
-    bool has_colors = HasColors();
-    bool has_covariance = HasCovariances();
+    const bool has_normals = HasNormals();
+    const bool has_colors = HasColors();
+    const bool has_color_gradient = HasColorGradients();
+    const bool has_covariance = HasCovariances();
 
     std::vector<bool> mask = std::vector<bool>(points_.size(), invert);
     for (size_t i : indices) {
@@ -207,6 +220,8 @@ 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_color_gradient)
+                output->color_gradients_.push_back(color_gradients_[i]);
             if (has_covariance) output->covariances_.push_back(covariances_[i]);
         }
     }
@@ -232,6 +247,10 @@ class AccumulatedPoint {
         if (cloud.HasColors()) {
             color_ += cloud.colors_[index];
         }
+        if (cloud.HasColorGradients()) {
+            color_gradient_ += cloud.color_gradients_[index];
+        }
+
         if (cloud.HasCovariances()) {
             covariance_ += cloud.covariances_[index];
         }
@@ -251,6 +270,10 @@ class AccumulatedPoint {
         return color_ / double(num_of_points_);
     }
 
+    Eigen::Vector3d GetAverageColorGradient() const {
+        return color_gradient_ / double(num_of_points_);
+    }
+
     Eigen::Matrix3d GetAverageCovariance() const {
         return covariance_ / double(num_of_points_);
     }
@@ -260,6 +283,7 @@ class AccumulatedPoint {
     Eigen::Vector3d point_ = Eigen::Vector3d::Zero();
     Eigen::Vector3d normal_ = Eigen::Vector3d::Zero();
     Eigen::Vector3d color_ = Eigen::Vector3d::Zero();
+    Eigen::Vector3d color_gradient_ = Eigen::Vector3d::Zero();
     Eigen::Matrix3d covariance_ = Eigen::Matrix3d::Zero();
 };
 
@@ -351,9 +375,10 @@ std::shared_ptr<PointCloud> PointCloud::VoxelDownSample(
                 int(floor(ref_coord(2)));
         voxelindex_to_accpoint[voxel_index].AddPoint(*this, i);
     }
-    bool has_normals = HasNormals();
-    bool has_colors = HasColors();
-    bool has_covariances = HasCovariances();
+    const bool has_normals = HasNormals();
+    const bool has_colors = HasColors();
+    const bool has_color_gradients = HasColorGradients();
+    const bool has_covariances = HasCovariances();
     for (auto accpoint : voxelindex_to_accpoint) {
         output->points_.push_back(accpoint.second.GetAveragePoint());
         if (has_normals) {
@@ -362,6 +387,10 @@ std::shared_ptr<PointCloud> PointCloud::VoxelDownSample(
         if (has_colors) {
             output->colors_.push_back(accpoint.second.GetAverageColor());
         }
+        if (has_color_gradients) {
+            output->color_gradients_.emplace_back(
+                    accpoint.second.GetAverageColorGradient());
+        }
         if (has_covariances) {
             output->covariances_.emplace_back(
                     accpoint.second.GetAverageCovariance());
@@ -397,7 +426,8 @@ PointCloud::VoxelDownSampleAndTrace(double voxel_size,
                        utility::hash_eigen<Eigen::Vector3i>>
             voxelindex_to_accpoint;
     int cid_temp[3] = {1, 2, 4};
-    for (size_t i = 0; i < points_.size(); i++) {
+#pragma omp parallel for schedule(static)
+    for (int i = 0; i < static_cast<int>(points_.size()); i++) {
         auto ref_coord = (points_[i] - voxel_min_bound) / voxel_size;
         auto voxel_index = Eigen::Vector3i(int(floor(ref_coord(0))),
                                            int(floor(ref_coord(1))),
@@ -408,12 +438,13 @@ PointCloud::VoxelDownSampleAndTrace(double voxel_size,
                 cid += cid_temp[c];
             }
         }
-        voxelindex_to_accpoint[voxel_index].AddPoint(*this, i, cid,
-                                                     approximate_class);
+        voxelindex_to_accpoint[voxel_index].AddPoint(
+                *this, static_cast<size_t>(i), cid, approximate_class);
     }
-    bool has_normals = HasNormals();
-    bool has_colors = HasColors();
-    bool has_covariances = HasCovariances();
+    const bool has_normals = HasNormals();
+    const bool has_colors = HasColors();
+    const bool has_color_gradients = HasColorGradients();
+    const bool has_covariances = HasCovariances();
     int cnt = 0;
     cubic_id.resize(voxelindex_to_accpoint.size(), 8);
     cubic_id.setConstant(-1);
@@ -431,6 +462,10 @@ PointCloud::VoxelDownSampleAndTrace(double voxel_size,
                 output->colors_.push_back(accpoint.second.GetAverageColor());
             }
         }
+        if (has_color_gradients) {
+            output->color_gradients_.emplace_back(
+                    accpoint.second.GetAverageColorGradient());
+        }
         if (has_covariances) {
             output->covariances_.emplace_back(
                     accpoint.second.GetAverageCovariance());
@@ -591,6 +626,74 @@ PointCloud::RemoveStatisticalOutliers(size_t nb_neighbors,
     return std::make_tuple(SelectByIndex(indices), indices);
 }
 
+// For ColoredICP.
+void PointCloud::EstimateColorGradients(
+        const KDTreeSearchParamHybrid
+                &search_param /*= KDTreeSearchParamHybrid(0.5, 30)*/) {
+    utility::LogDebug("Estimating Color Gradients.");
+
+    if (!this->HasColors() || !this->HasNormals()) {
+        utility::LogError(
+                "PointCloud must have colors and normals attribute "
+                "to compute color gradients.");
+    }
+
+    geometry::KDTreeFlann tree;
+    tree.SetGeometry(*this);
+
+    size_t n_points = this->points_.size();
+    this->color_gradients_.resize(n_points, Eigen::Vector3d::Zero());
+
+#pragma omp parallel for schedule(static)
+    for (int k = 0; k < static_cast<int>(n_points); k++) {
+        const Eigen::Vector3d &vt = this->points_[k];
+        const Eigen::Vector3d &nt = this->normals_[k];
+        double it = (this->colors_[k](0) + this->colors_[k](1) +
+                     this->colors_[k](2)) /
+                    3.0;
+
+        std::vector<int> point_idx;
+        std::vector<double> point_squared_distance;
+
+        if (tree.SearchHybrid(vt, search_param.radius_, search_param.max_nn_,
+                              point_idx, point_squared_distance) >= 4) {
+            // approximate image gradient of vt's tangential plane
+            size_t nn = point_idx.size();
+            Eigen::MatrixXd A(nn, 3);
+            Eigen::MatrixXd b(nn, 1);
+            A.setZero();
+            b.setZero();
+
+            for (size_t i = 1; i < nn; i++) {
+                int P_adj_idx = point_idx[i];
+                Eigen::Vector3d vt_adj = this->points_[P_adj_idx];
+                Eigen::Vector3d vt_proj = vt_adj - (vt_adj - vt).dot(nt) * nt;
+                double it_adj = (this->colors_[P_adj_idx](0) +
+                                 this->colors_[P_adj_idx](1) +
+                                 this->colors_[P_adj_idx](2)) /
+                                3.0;
+                A(i - 1, 0) = (vt_proj(0) - vt(0));
+                A(i - 1, 1) = (vt_proj(1) - vt(1));
+                A(i - 1, 2) = (vt_proj(2) - vt(2));
+                b(i - 1, 0) = (it_adj - it);
+            }
+            // adds orthogonal constraint
+            A(nn - 1, 0) = (nn - 1) * nt(0);
+            A(nn - 1, 1) = (nn - 1) * nt(1);
+            A(nn - 1, 2) = (nn - 1) * nt(2);
+            b(nn - 1, 0) = 0;
+            // solving linear equation
+            bool is_success = false;
+            Eigen::MatrixXd x;
+            std::tie(is_success, x) = utility::SolveLinearSystemPSD(
+                    A.transpose() * A, A.transpose() * b);
+            if (is_success) {
+                this->color_gradients_[k] = x;
+            }
+        }
+    }
+}
+
 std::vector<Eigen::Matrix3d> PointCloud::EstimatePerPointCovariances(
         const PointCloud &input,
         const KDTreeSearchParam &search_param /* = KDTreeSearchParamKNN()*/) {
@@ -617,6 +720,7 @@ std::vector<Eigen::Matrix3d> PointCloud::EstimatePerPointCovariances(
     }
     return covariances;
 }
+
 void PointCloud::EstimateCovariances(
         const KDTreeSearchParam &search_param /* = KDTreeSearchParamKNN()*/) {
     this->covariances_ = EstimatePerPointCovariances(*this, search_param);

cpp/open3d/geometry/PointCloud.h

@@ -96,6 +96,11 @@ class PointCloud : public Geometry3D {
         return points_.size() > 0 && colors_.size() == points_.size();
     }
 
+    /// Returns `true` if the point cloud contains point colors.
+    bool HasColorGradients() const {
+        return !points_.empty() && color_gradients_.size() == points_.size();
+    }
+
     /// Returns 'true' if the point cloud contains per-point covariance matrix.
     bool HasCovariances() const {
         return !points_.empty() && covariances_.size() == points_.size();
@@ -218,6 +223,14 @@ class PointCloud : public Geometry3D {
                               double std_ratio,
                               bool print_progress = false) const;
 
+    /// \brief Function to compute the color_gradients of a point cloud.
+    /// Ref: This function is used in ColoredICP.
+    ///
+    /// \param search_param The KDTree search parameters for `hybrid`
+    /// neighborhood search.
+    void EstimateColorGradients(const KDTreeSearchParamHybrid &search_param =
+                                        KDTreeSearchParamHybrid(0.5, 30));
+
     /// \brief Function to compute the normals of a point cloud.
     ///
     /// Normals are oriented with respect to the input point cloud if normals
@@ -420,6 +433,8 @@ class PointCloud : public Geometry3D {
     std::vector<Eigen::Vector3d> normals_;
     /// RGB colors of points.
     std::vector<Eigen::Vector3d> colors_;
+    /// Color gradients of points, required for Colored Registration.
+    std::vector<Eigen::Vector3d> color_gradients_;
     /// Covariance Matrix for each point
     std::vector<Eigen::Matrix3d> covariances_;
 };