fix related coverience

neka-nat · Jun 4, 2024 · 31deb2d · 31deb2d
1 parent 6b8d6f7
commit 31deb2d
Show file tree

Hide file tree

Showing 4 changed files with 60 additions and 0 deletions.
diff --git a/docker-compose.yml b/docker-compose.yml
@@ -11,6 +11,7 @@ services:
       - "NVIDIA_DRIVER_CAPABILITIES=all"
     volumes:
       - "/tmp/.X11-unix:/tmp/.X11-unix:rw"
+      - .:/work/cupoch
     tty: true
     privileged: true
     deploy:

diff --git a/src/cupoch/geometry/geometry_utils.cu b/src/cupoch/geometry/geometry_utils.cu
@@ -135,6 +135,18 @@ void TransformNormals(cudaStream_t stream,
                      normals.end(), func);
 }
 
+void TransformCovariances(const Eigen::Matrix4f &transformation,
+                          utility::device_vector<Eigen::Matrix3f> &covariances) {
+    TransformCovariances(0, transformation, covariances);
+}
+
+void TransformCovariances(cudaStream_t stream,
+                         const Eigen::Matrix4f& transformation,
+                         utility::device_vector<Eigen::Matrix3f>& covariances) {
+    RotateCovariances(transformation.block<3, 3>(0, 0), covariances);
+}
+
+
 template <int Dim>
 void TranslatePoints(
         const Eigen::Matrix<float, Dim, 1> &translation,
@@ -237,6 +249,21 @@ void RotateNormals(cudaStream_t stream,
                      });
 }
 
+void RotateCovariances(const Eigen::Matrix3f &R,
+                       utility::device_vector<Eigen::Matrix3f> &covariances) {
+    RotateCovariances(0, R, covariances);
+}
+
+void RotateCovariances(cudaStream_t stream,
+                       const Eigen::Matrix3f& R,
+                       utility::device_vector<Eigen::Matrix3f>& covariances) {
+    thrust::for_each(utility::exec_policy(stream),
+                     covariances.begin(), covariances.end(),
+                     [=] __device__(Eigen::Matrix3f& covariance) {
+                         covariance = R * covariance * R.transpose();
+                     });
+}
+
 Eigen::Matrix3f GetRotationMatrixFromXYZ(const Eigen::Vector3f &rotation) {
     return cupoch::utility::RotationMatrixX(rotation(0)) *
            cupoch::utility::RotationMatrixY(rotation(1)) *

diff --git a/src/cupoch/geometry/geometry_utils.h b/src/cupoch/geometry/geometry_utils.h
@@ -72,6 +72,17 @@ void TransformNormals(const Eigen::Matrix4f &transformation,
 void TransformNormals(cudaStream_t stream,
                       const Eigen::Matrix4f &transformation,
                       utility::device_vector<Eigen::Vector3f> &normals);
+
+/// \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(cudaStream_t stream,
+                          const Eigen::Matrix4f& transformation,
+                          utility::device_vector<Eigen::Matrix3f>& covariances);
+void TransformCovariances(const Eigen::Matrix4f& transformation,
+                          utility::device_vector<Eigen::Matrix3f>& covariances);
+
 /// \brief Apply translation to the geometry coordinates.
 ///
 /// \param translation A 3D vector to transform the geometry.
@@ -126,5 +137,15 @@ void RotateNormals(cudaStream_t stream,
                    const Eigen::Matrix3f &R,
                    utility::device_vector<Eigen::Vector3f> &normals);
 
+/// \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::Matrix3f& R,
+                       utility::device_vector<Eigen::Matrix3f>& covariances);
+void RotateCovariances(cudaStream_t stream,
+                       const Eigen::Matrix3f& R,
+                       utility::device_vector<Eigen::Matrix3f>& covariances);
+
 }  // namespace geometry
 }  // namespace cupoch
diff --git a/src/cupoch/geometry/pointcloud.cu b/src/cupoch/geometry/pointcloud.cu
@@ -140,6 +140,7 @@ PointCloud &PointCloud::operator=(const PointCloud &other) {
     points_ = other.points_;
     normals_ = other.normals_;
     colors_ = other.colors_;
+    covariances_ = other.covariances_;
     return *this;
 }
 
@@ -175,6 +176,7 @@ PointCloud &PointCloud::Clear() {
     points_.clear();
     normals_.clear();
     colors_.clear();
+    covariances_.clear();
     return *this;
 }
 
@@ -214,6 +216,7 @@ PointCloud &PointCloud::Scale(const float scale, bool center) {
 PointCloud &PointCloud::Rotate(const Eigen::Matrix3f &R, bool center) {
     RotatePoints<3>(utility::GetStream(0), R, points_, center);
     RotateNormals(utility::GetStream(1), R, normals_);
+    RotateCovariances(utility::GetStream(2), R, covariances_);
     cudaSafeCall(cudaDeviceSynchronize());
     return *this;
 }
@@ -239,6 +242,13 @@ PointCloud &PointCloud::operator+=(const PointCloud &cloud) {
     } else {
         colors_.clear();
     }
+    if ((!HasPoints() || HasCovariances()) && cloud.HasCovariances()) {
+        covariances_.resize(new_vert_num);
+        thrust::copy(cloud.covariances_.begin(), cloud.covariances_.end(),
+                     covariances_.begin() + old_vert_num);
+    } else {
+        covariances_.clear();
+    }
     points_.resize(new_vert_num);
     thrust::copy(cloud.points_.begin(), cloud.points_.end(),
                  points_.begin() + old_vert_num);
@@ -263,6 +273,7 @@ PointCloud &PointCloud::PaintUniformColor(const Eigen::Vector3f &color) {
 PointCloud &PointCloud::Transform(const Eigen::Matrix4f &transformation) {
     TransformPoints<3>(utility::GetStream(0), transformation, points_);
     TransformNormals(utility::GetStream(1), transformation, normals_);
+    TransformCovariances(utility::GetStream(2), transformation, covariances_);
     cudaSafeCall(cudaDeviceSynchronize());
     return *this;
 }