Nacho/generalized icp

CHANGELOG.md

@@ -32,6 +32,8 @@
 * Updated Open3D.h.in to add certain missing header files
 * Add Open3D-ML to Open3D wheel
 * Fix a bug in PointCloud file format, use `float` instead of `float_t`
+* Add per-point covariance member for geometry::PointCloud class.
+* Add Generalized ICP implementation.
 
 ## 0.9.0
 

cpp/open3d/geometry/EstimateNormals.cpp

@@ -135,13 +135,14 @@ Eigen::Vector3d ComputeEigenvector1(const Eigen::Matrix3d &A,
     }
 }
 
-Eigen::Vector3d FastEigen3x3(Eigen::Matrix3d &A) {
+Eigen::Vector3d FastEigen3x3(const Eigen::Matrix3d &covariance) {
     // Previous version based on:
     // https://en.wikipedia.org/wiki/Eigenvalue_algorithm#3.C3.973_matrices
     // Current version based on
     // https://www.geometrictools.com/Documentation/RobustEigenSymmetric3x3.pdf
     // which handles edge cases like points on a plane
 
+    Eigen::Matrix3d A = covariance;
     double max_coeff = A.maxCoeff();
     if (max_coeff == 0) {
         return Eigen::Vector3d::Zero();
@@ -221,22 +222,14 @@ Eigen::Vector3d FastEigen3x3(Eigen::Matrix3d &A) {
     }
 }
 
-Eigen::Vector3d ComputeNormal(const PointCloud &cloud,
-                              const std::vector<int> &indices,
+Eigen::Vector3d ComputeNormal(const Eigen::Matrix3d &covariance,
                               bool fast_normal_computation) {
-    if (indices.size() == 0) {
-        return Eigen::Vector3d::Zero();
-    }
-    Eigen::Matrix3d covariance =
-            utility::ComputeCovariance(cloud.points_, indices);
-
     if (fast_normal_computation) {
         return FastEigen3x3(covariance);
-    } else {
-        Eigen::SelfAdjointEigenSolver<Eigen::Matrix3d> solver;
-        solver.compute(covariance, Eigen::ComputeEigenvectors);
-        return solver.eigenvectors().col(0);
     }
+    Eigen::SelfAdjointEigenSolver<Eigen::Matrix3d> solver;
+    solver.compute(covariance, Eigen::ComputeEigenvectors);
+    return solver.eigenvectors().col(0);
 }
 
 // Disjoint set data structure to find cycles in graphs
@@ -317,29 +310,26 @@ void PointCloud::EstimateNormals(
     if (!has_normal) {
         normals_.resize(points_.size());
     }
-    KDTreeFlann kdtree;
-    kdtree.SetGeometry(*this);
+    std::vector<Eigen::Matrix3d> covariances;
+    if (!HasCovariances()) {
+        covariances = EstimatePerPointCovariances(*this, search_param);
+    } else {
+        covariances = covariances_;
+    }
 #pragma omp parallel for schedule(static)
-    for (int i = 0; i < (int)points_.size(); i++) {
-        std::vector<int> indices;
-        std::vector<double> distance2;
-        Eigen::Vector3d normal;
-        if (kdtree.Search(points_[i], search_param, indices, distance2) >= 3) {
-            normal = ComputeNormal(*this, indices, fast_normal_computation);
-            if (normal.norm() == 0.0) {
-                if (has_normal) {
-                    normal = normals_[i];
-                } else {
-                    normal = Eigen::Vector3d(0.0, 0.0, 1.0);
-                }
-            }
-            if (has_normal && normal.dot(normals_[i]) < 0.0) {
-                normal *= -1.0;
+    for (int i = 0; i < (int)covariances.size(); i++) {
+        auto normal = ComputeNormal(covariances[i], fast_normal_computation);
+        if (normal.norm() == 0.0) {
+            if (has_normal) {
+                normal = normals_[i];
+            } else {
+                normal = Eigen::Vector3d(0.0, 0.0, 1.0);
             }
-            normals_[i] = normal;
-        } else {
-            normals_[i] = Eigen::Vector3d(0.0, 0.0, 1.0);
         }
+        if (has_normal && normal.dot(normals_[i]) < 0.0) {
+            normal *= -1.0;
+        }
+        normals_[i] = normal;
     }
 }
 

cpp/open3d/geometry/Geometry3D.cpp

@@ -113,6 +113,12 @@ void Geometry3D::TransformNormals(const Eigen::Matrix4d& transformation,
     }
 }
 
+void Geometry3D::TransformCovariances(
+        const Eigen::Matrix4d& transformation,
+        std::vector<Eigen::Matrix3d>& covariances) const {
+    RotateCovariances(transformation.block<3, 3>(0, 0), covariances);
+}
+
 void Geometry3D::TranslatePoints(const Eigen::Vector3d& translation,
                                  std::vector<Eigen::Vector3d>& points,
                                  bool relative) const {
@@ -148,6 +154,17 @@ void Geometry3D::RotateNormals(const Eigen::Matrix3d& R,
     }
 }
 
+/// The only part that affects the covariance is the rotation part. For more
+/// information on variance propagation please visit:
+/// https://en.wikipedia.org/wiki/Propagation_of_uncertainty
+void Geometry3D::RotateCovariances(
+        const Eigen::Matrix3d& R,
+        std::vector<Eigen::Matrix3d>& covariances) const {
+    for (auto& covariance : covariances) {
+        covariance = R * covariance * R.transpose();
+    }
+}
+
 Eigen::Matrix3d Geometry3D::GetRotationMatrixFromXYZ(
         const Eigen::Vector3d& rotation) {
     return open3d::utility::RotationMatrixX(rotation(0)) *

cpp/open3d/geometry/Geometry3D.h

@@ -28,6 +28,7 @@
 
 #include <Eigen/Core>
 #include <Eigen/Geometry>
+#include <vector>
 
 #include "open3d/geometry/Geometry.h"
 #include "open3d/utility/Eigen.h"
@@ -155,6 +156,14 @@ class Geometry3D : public Geometry {
     /// \param normals A list of normals to be transformed.
     void TransformNormals(const Eigen::Matrix4d& transformation,
                           std::vector<Eigen::Vector3d>& normals) const;
+
+    /// \brief Transforms all covariance matrices with the transformation.
+    ///
+    /// \param transformation 4x4 matrix for transformation.
+    /// \param covariances A list of covariance matrices to be transformed.
+    void TransformCovariances(const Eigen::Matrix4d& transformation,
+                              std::vector<Eigen::Matrix3d>& covariances) const;
+
     /// \brief Apply translation to the geometry coordinates.
     ///
     /// \param translation A 3D vector to transform the geometry.
@@ -192,6 +201,13 @@ class Geometry3D : public Geometry {
     /// \param normals A list of normals to be transformed.
     void RotateNormals(const Eigen::Matrix3d& R,
                        std::vector<Eigen::Vector3d>& normals) const;
+
+    /// \brief Rotate all covariance matrices with the rotation matrix \p R.
+    ///
+    /// \param R A 3x3 rotation matrix
+    /// \param covariances A list of covariance matrices to be transformed.
+    void RotateCovariances(const Eigen::Matrix3d& R,
+                           std::vector<Eigen::Matrix3d>& covariances) const;
 };
 
 }  // namespace geometry

cpp/open3d/geometry/PointCloud.cpp

@@ -45,6 +45,7 @@ PointCloud &PointCloud::Clear() {
     points_.clear();
     normals_.clear();
     colors_.clear();
+    covariances_.clear();
     return *this;
 }
 
@@ -71,6 +72,7 @@ OrientedBoundingBox PointCloud::GetOrientedBoundingBox() const {
 PointCloud &PointCloud::Transform(const Eigen::Matrix4d &transformation) {
     TransformPoints(transformation, points_);
     TransformNormals(transformation, normals_);
+    TransformCovariances(transformation, covariances_);
     return *this;
 }
 
@@ -90,6 +92,7 @@ PointCloud &PointCloud::Rotate(const Eigen::Matrix3d &R,
                                const Eigen::Vector3d &center) {
     RotatePoints(R, points_, center);
     RotateNormals(R, normals_);
+    RotateCovariances(R, covariances_);
     return *this;
 }
 
@@ -114,6 +117,13 @@ PointCloud &PointCloud::operator+=(const PointCloud &cloud) {
     } 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];
+    } else {
+        covariances_.clear();
+    }
     points_.resize(new_vert_num);
     for (size_t i = 0; i < add_vert_num; i++)
         points_[old_vert_num + i] = cloud.points_[i];
@@ -149,6 +159,7 @@ PointCloud &PointCloud::RemoveNonFinitePoints(bool remove_nan,
                                               bool remove_infinite) {
     bool has_normal = HasNormals();
     bool has_color = HasColors();
+    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
@@ -162,12 +173,14 @@ 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_covariance) covariances_[k] = covariances_[i];
             k++;
         }
     }
     points_.resize(k);
     if (has_normal) normals_.resize(k);
     if (has_color) colors_.resize(k);
+    if (has_covariance) covariances_.resize(k);
     utility::LogDebug(
             "[RemoveNonFinitePoints] {:d} nan points have been removed.",
             (int)(old_point_num - k));
@@ -179,6 +192,7 @@ std::shared_ptr<PointCloud> PointCloud::SelectByIndex(
     auto output = std::make_shared<PointCloud>();
     bool has_normals = HasNormals();
     bool has_colors = HasColors();
+    bool has_covariance = HasCovariances();
 
     std::vector<bool> mask = std::vector<bool>(points_.size(), invert);
     for (size_t i : indices) {
@@ -190,6 +204,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_covariance) output->covariances_.push_back(covariances_[i]);
         }
     }
     utility::LogDebug(
@@ -201,13 +216,6 @@ std::shared_ptr<PointCloud> PointCloud::SelectByIndex(
 // helper classes for VoxelDownSample and VoxelDownSampleAndTrace
 namespace {
 class AccumulatedPoint {
-public:
-    AccumulatedPoint()
-        : num_of_points_(0),
-          point_(0.0, 0.0, 0.0),
-          normal_(0.0, 0.0, 0.0),
-          color_(0.0, 0.0, 0.0) {}
-
 public:
     void AddPoint(const PointCloud &cloud, int index) {
         point_ += cloud.points_[index];
@@ -221,6 +229,9 @@ class AccumulatedPoint {
         if (cloud.HasColors()) {
             color_ += cloud.colors_[index];
         }
+        if (cloud.HasCovariances()) {
+            covariance_ += cloud.covariances_[index];
+        }
         num_of_points_++;
     }
 
@@ -237,11 +248,16 @@ class AccumulatedPoint {
         return color_ / double(num_of_points_);
     }
 
+    Eigen::Matrix3d GetAverageCovariance() const {
+        return covariance_ / double(num_of_points_);
+    }
+
 public:
-    int num_of_points_;
-    Eigen::Vector3d point_;
-    Eigen::Vector3d normal_;
-    Eigen::Vector3d color_;
+    int num_of_points_ = 0;
+    Eigen::Vector3d point_ = Eigen::Vector3d::Zero();
+    Eigen::Vector3d normal_ = Eigen::Vector3d::Zero();
+    Eigen::Vector3d color_ = Eigen::Vector3d::Zero();
+    Eigen::Matrix3d covariance_ = Eigen::Matrix3d::Zero();
 };
 
 class point_cubic_id {
@@ -275,6 +291,9 @@ class AccumulatedPointForTrace : public AccumulatedPoint {
                 color_ += cloud.colors_[index];
             }
         }
+        if (cloud.HasCovariances()) {
+            covariance_ += cloud.covariances_[index];
+        }
         point_cubic_id new_id;
         new_id.point_id = index;
         new_id.cubic_id = cubic_index;
@@ -331,6 +350,7 @@ std::shared_ptr<PointCloud> PointCloud::VoxelDownSample(
     }
     bool has_normals = HasNormals();
     bool has_colors = HasColors();
+    bool has_covariances = HasCovariances();
     for (auto accpoint : voxelindex_to_accpoint) {
         output->points_.push_back(accpoint.second.GetAveragePoint());
         if (has_normals) {
@@ -339,6 +359,10 @@ std::shared_ptr<PointCloud> PointCloud::VoxelDownSample(
         if (has_colors) {
             output->colors_.push_back(accpoint.second.GetAverageColor());
         }
+        if (has_covariances) {
+            output->covariances_.emplace_back(
+                    accpoint.second.GetAverageCovariance());
+        }
     }
     utility::LogDebug(
             "Pointcloud down sampled from {:d} points to {:d} points.",
@@ -386,6 +410,7 @@ PointCloud::VoxelDownSampleAndTrace(double voxel_size,
     }
     bool has_normals = HasNormals();
     bool has_colors = HasColors();
+    bool has_covariances = HasCovariances();
     int cnt = 0;
     cubic_id.resize(voxelindex_to_accpoint.size(), 8);
     cubic_id.setConstant(-1);
@@ -403,6 +428,10 @@ PointCloud::VoxelDownSampleAndTrace(double voxel_size,
                 output->colors_.push_back(accpoint.second.GetAverageColor());
             }
         }
+        if (has_covariances) {
+            output->covariances_.emplace_back(
+                    accpoint.second.GetAverageCovariance());
+        }
         auto original_id = accpoint.second.GetOriginalID();
         for (int i = 0; i < (int)original_id.size(); i++) {
             size_t pid = original_id[i].point_id;
@@ -549,6 +578,37 @@ PointCloud::RemoveStatisticalOutliers(size_t nb_neighbors,
     return std::make_tuple(SelectByIndex(indices), indices);
 }
 
+std::vector<Eigen::Matrix3d> PointCloud::EstimatePerPointCovariances(
+        const PointCloud &input,
+        const KDTreeSearchParam &search_param /* = KDTreeSearchParamKNN()*/) {
+    const auto &points = input.points_;
+    std::vector<Eigen::Matrix3d> covariances;
+    covariances.resize(points.size());
+
+    KDTreeFlann kdtree;
+    kdtree.SetGeometry(input);
+#pragma omp parallel for schedule(static)
+    for (int i = 0; i < (int)points.size(); i++) {
+        std::vector<int> indices;
+        std::vector<double> distance2;
+        if (kdtree.Search(points[i], search_param, indices, distance2) >= 3) {
+            auto covariance = utility::ComputeCovariance(points, indices);
+            if (input.HasCovariances() && covariance.isIdentity(1e-4)) {
+                covariances[i] = input.covariances_[i];
+            } else {
+                covariances[i] = covariance;
+            }
+        } else {
+            covariances[i] = Eigen::Matrix3d::Identity();
+        }
+    }
+    return covariances;
+}
+void PointCloud::EstimateCovariances(
+        const KDTreeSearchParam &search_param /* = KDTreeSearchParamKNN()*/) {
+    this->covariances_ = EstimatePerPointCovariances(*this, search_param);
+}
+
 std::tuple<Eigen::Vector3d, Eigen::Matrix3d>
 PointCloud::ComputeMeanAndCovariance() const {
     if (IsEmpty()) {