std vector interface

neka-nat · Sep 15, 2024 · 5e8c907 · 5e8c907
1 parent daadbf9
commit 5e8c907
Show file tree

Hide file tree

Showing 15 changed files with 103 additions and 62 deletions.
diff --git a/CMakeLists.txt b/CMakeLists.txt
@@ -160,6 +160,7 @@ elseif (UNIX)
 endif ()
 
 find_package(CUDA REQUIRED)
+enable_language(CUDA)
 if(NOT CMAKE_CUDA_ARCHITECTURES)
   set(CMAKE_CUDA_ARCHITECTURES 86)
 endif()

diff --git a/src/cupoch/geometry/lineset.cu b/src/cupoch/geometry/lineset.cu
@@ -68,6 +68,13 @@ LineSet<Dim>::LineSet(
       points_(points),
       lines_(lines) {}
 
+template <int Dim>
+LineSet<Dim>::LineSet(const std::vector<Eigen::Matrix<float, Dim, 1>> &points,
+                     const std::vector<Eigen::Vector2i> &lines)
+    : GeometryBaseXD<Dim>(Geometry::GeometryType::LineSet),
+      points_(points),
+      lines_(lines) {}
+
 template <int Dim>
 LineSet<Dim>::LineSet(const std::vector<Eigen::Matrix<float, Dim, 1>> &path)
     : GeometryBaseXD<Dim>(Geometry::GeometryType::LineSet) {
@@ -101,6 +108,13 @@ void LineSet<Dim>::SetPoints(
     points_ = points;
 }
 
+template <int Dim>
+void LineSet<Dim>::SetPoints(
+        const std::vector<Eigen::Matrix<float, Dim, 1>> &points) {
+    points_.resize(points.size());
+    copy_host_to_device(points, points_);
+}
+
 template <int Dim>
 thrust::host_vector<Eigen::Matrix<float, Dim, 1>> LineSet<Dim>::GetPoints()
         const {
@@ -113,6 +127,12 @@ void LineSet<Dim>::SetLines(const thrust::host_vector<Eigen::Vector2i> &lines) {
     lines_ = lines;
 }
 
+template <int Dim>
+void LineSet<Dim>::SetLines(const std::vector<Eigen::Vector2i> &lines) {
+    lines_.resize(lines.size());
+    copy_host_to_device(lines, lines_);
+}
+
 template <int Dim>
 thrust::host_vector<Eigen::Vector2i> LineSet<Dim>::GetLines() const {
     thrust::host_vector<Eigen::Vector2i> lines = lines_;
@@ -125,6 +145,12 @@ void LineSet<Dim>::SetColors(
     colors_ = colors;
 }
 
+template <int Dim>
+void LineSet<Dim>::SetColors(const std::vector<Eigen::Vector3f> &colors) {
+    colors_.resize(colors.size());
+    copy_host_to_device(colors, colors_);
+}
+
 template <int Dim>
 thrust::host_vector<Eigen::Vector3f> LineSet<Dim>::GetColors() const {
     thrust::host_vector<Eigen::Vector3f> colors = colors_;

diff --git a/src/cupoch/geometry/lineset.h b/src/cupoch/geometry/lineset.h
@@ -58,18 +58,24 @@ class LineSet : public GeometryBaseXD<Dim> {
             const utility::pinned_host_vector<Eigen::Vector2i> &lines);
     LineSet(const thrust::host_vector<Eigen::Matrix<float, Dim, 1>> &points,
             const thrust::host_vector<Eigen::Vector2i> &lines);
+    LineSet(const std::vector<Eigen::Matrix<float, Dim, 1>> &points,
+            const std::vector<Eigen::Vector2i> &lines);
     LineSet(const std::vector<Eigen::Matrix<float, Dim, 1>> &path);
     LineSet(const LineSet &other);
     ~LineSet();
 
     void SetPoints(
             const thrust::host_vector<Eigen::Matrix<float, Dim, 1>> &points);
+    void SetPoints(
+            const std::vector<Eigen::Matrix<float, Dim, 1>> &points);
     thrust::host_vector<Eigen::Matrix<float, Dim, 1>> GetPoints() const;
 
     void SetLines(const thrust::host_vector<Eigen::Vector2i> &lines);
+    void SetLines(const std::vector<Eigen::Vector2i> &lines);
     thrust::host_vector<Eigen::Vector2i> GetLines() const;
 
     void SetColors(const thrust::host_vector<Eigen::Vector3f> &colors);
+    void SetColors(const std::vector<Eigen::Vector3f> &colors);
     thrust::host_vector<Eigen::Vector3f> GetColors() const;
 
 public:

diff --git a/src/cupoch/geometry/pointcloud.cu b/src/cupoch/geometry/pointcloud.cu
@@ -168,6 +168,11 @@ void PointCloud::SetPoints(const thrust::host_vector<Eigen::Vector3f> &points) {
     points_ = points;
 }
 
+void PointCloud::SetPoints(const std::vector<Eigen::Vector3f> &points) {
+    points_.resize(points.size());
+    copy_host_to_device(points, points_);
+}
+
 thrust::host_vector<Eigen::Vector3f> PointCloud::GetPoints() const {
     thrust::host_vector<Eigen::Vector3f> points = points_;
     return points;
@@ -178,6 +183,11 @@ void PointCloud::SetNormals(
     normals_ = normals;
 }
 
+void PointCloud::SetNormals(const std::vector<Eigen::Vector3f> &normals) {
+    normals_.resize(normals.size());
+    copy_host_to_device(normals, normals_);
+}
+
 thrust::host_vector<Eigen::Vector3f> PointCloud::GetNormals() const {
     thrust::host_vector<Eigen::Vector3f> normals = normals_;
     return normals;
@@ -187,6 +197,11 @@ void PointCloud::SetColors(const thrust::host_vector<Eigen::Vector3f> &colors) {
     colors_ = colors;
 }
 
+void PointCloud::SetColors(const std::vector<Eigen::Vector3f> &colors) {
+    colors_.resize(colors.size());
+    copy_host_to_device(colors, colors_);
+}
+
 thrust::host_vector<Eigen::Vector3f> PointCloud::GetColors() const {
     thrust::host_vector<Eigen::Vector3f> colors = colors_;
     return colors;

diff --git a/src/cupoch/geometry/pointcloud.h b/src/cupoch/geometry/pointcloud.h
@@ -51,12 +51,15 @@ class PointCloud : public GeometryBase3D {
     PointCloud &operator=(const PointCloud &other);
 
     void SetPoints(const thrust::host_vector<Eigen::Vector3f> &points);
+    void SetPoints(const std::vector<Eigen::Vector3f> &points);
     thrust::host_vector<Eigen::Vector3f> GetPoints() const;
 
     void SetNormals(const thrust::host_vector<Eigen::Vector3f> &normals);
+    void SetNormals(const std::vector<Eigen::Vector3f> &normals);
     thrust::host_vector<Eigen::Vector3f> GetNormals() const;
 
     void SetColors(const thrust::host_vector<Eigen::Vector3f> &colors);
+    void SetColors(const std::vector<Eigen::Vector3f> &colors);
     thrust::host_vector<Eigen::Vector3f> GetColors() const;
 
     PointCloud &Clear() override;

diff --git a/src/cupoch/geometry/voxelgrid.cu b/src/cupoch/geometry/voxelgrid.cu
@@ -148,6 +148,14 @@ void VoxelGrid::SetVoxels(
     voxels_values_ = voxels_values;
 }
 
+void VoxelGrid::SetVoxels(const std::vector<Eigen::Vector3i> &voxels_keys,
+                          const std::vector<Voxel> &voxels_values) {
+    voxels_keys_.resize(voxels_keys.size());
+    voxels_values_.resize(voxels_values.size());
+    copy_host_to_device(voxels_keys, voxels_keys_);
+    copy_host_to_device(voxels_values, voxels_values_);
+}
+
 VoxelGrid &VoxelGrid::Clear() {
     voxel_size_ = 0.0;
     origin_ = Eigen::Vector3f::Zero();

diff --git a/src/cupoch/geometry/voxelgrid.h b/src/cupoch/geometry/voxelgrid.h
@@ -91,6 +91,8 @@ class VoxelGrid : public GeometryBase3D {
     GetVoxels() const;
     void SetVoxels(const thrust::host_vector<Eigen::Vector3i> &voxels_keys,
                    const thrust::host_vector<Voxel> &voxels_values);
+    void SetVoxels(const std::vector<Eigen::Vector3i> &voxels_keys,
+                   const std::vector<Voxel> &voxels_values);
 
     VoxelGrid &Clear() override;
     bool IsEmpty() const override;

diff --git a/src/cupoch/io/class_io/image_io.cu b/src/cupoch/io/class_io/image_io.cu
@@ -30,13 +30,13 @@ void HostImage::FromDevice(const geometry::Image& image) {
     data_.resize(image.data_.size());
     Prepare(image.width_, image.height_, image.num_of_channels_,
             image.bytes_per_channel_);
-    cudaSafeCall(cudaMemcpy(thrust::raw_pointer_cast(data_.data()), thrust::raw_pointer_cast(image.data_.data()), image.data_.size(), cudaMemcpyDeviceToHost));
+    copy_device_to_host(image.data_, data_);
 }
 
 void HostImage::ToDevice(geometry::Image& image) const {
     image.Prepare(width_, height_, num_of_channels_, bytes_per_channel_);
     image.data_.resize(data_.size());
-    cudaSafeCall(cudaMemcpy(thrust::raw_pointer_cast(image.data_.data()), thrust::raw_pointer_cast(data_.data()), image.data_.size(), cudaMemcpyHostToDevice));
+    copy_host_to_device(data_, image.data_);
 }
 
 void HostImage::Clear() {

diff --git a/src/cupoch/io/class_io/pointcloud_io.cu b/src/cupoch/io/class_io/pointcloud_io.cu
@@ -30,24 +30,18 @@ void HostPointCloud::FromDevice(const geometry::PointCloud& pointcloud) {
     points_.resize(pointcloud.points_.size());
     normals_.resize(pointcloud.normals_.size());
     colors_.resize(pointcloud.colors_.size());
-    cudaSafeCall(cudaMemcpy(thrust::raw_pointer_cast(points_.data()), thrust::raw_pointer_cast(pointcloud.points_.data()),
-                            points_.size() * sizeof(Eigen::Vector3f), cudaMemcpyDeviceToHost));
-    cudaSafeCall(cudaMemcpy(thrust::raw_pointer_cast(normals_.data()), thrust::raw_pointer_cast(pointcloud.normals_.data()),
-                            normals_.size() * sizeof(Eigen::Vector3f), cudaMemcpyDeviceToHost));
-    cudaSafeCall(cudaMemcpy(thrust::raw_pointer_cast(colors_.data()), thrust::raw_pointer_cast(pointcloud.colors_.data()),
-                            colors_.size() * sizeof(Eigen::Vector3f), cudaMemcpyDeviceToHost));
+    copy_device_to_host(pointcloud.points_, points_);
+    copy_device_to_host(pointcloud.normals_, normals_);
+    copy_device_to_host(pointcloud.colors_, colors_);
 }
 
 void HostPointCloud::ToDevice(geometry::PointCloud& pointcloud) const {
     pointcloud.points_.resize(points_.size());
     pointcloud.normals_.resize(normals_.size());
     pointcloud.colors_.resize(colors_.size());
-    cudaSafeCall(cudaMemcpy(thrust::raw_pointer_cast(pointcloud.points_.data()), thrust::raw_pointer_cast(points_.data()),
-                            points_.size() * sizeof(Eigen::Vector3f), cudaMemcpyHostToDevice));
-    cudaSafeCall(cudaMemcpy(thrust::raw_pointer_cast(pointcloud.normals_.data()), thrust::raw_pointer_cast(normals_.data()),
-                            normals_.size() * sizeof(Eigen::Vector3f), cudaMemcpyHostToDevice));
-    cudaSafeCall(cudaMemcpy(thrust::raw_pointer_cast(pointcloud.colors_.data()), thrust::raw_pointer_cast(colors_.data()),
-                            colors_.size() * sizeof(Eigen::Vector3f), cudaMemcpyHostToDevice));
+    copy_host_to_device(points_, pointcloud.points_);
+    copy_host_to_device(normals_, pointcloud.normals_);
+    copy_host_to_device(colors_, pointcloud.colors_);
 }
 
 void HostPointCloud::Clear() {

diff --git a/src/cupoch/io/class_io/trianglemesh_io.cu b/src/cupoch/io/class_io/trianglemesh_io.cu
@@ -32,18 +32,12 @@ void HostTriangleMesh::FromDevice(const geometry::TriangleMesh& trianglemesh) {
     triangles_.resize(trianglemesh.triangles_.size());
     triangle_normals_.resize(trianglemesh.triangle_normals_.size());
     triangle_uvs_.resize(trianglemesh.triangle_uvs_.size());
-    cudaSafeCall(cudaMemcpy(thrust::raw_pointer_cast(vertices_.data()), thrust::raw_pointer_cast(trianglemesh.vertices_.data()),
-                            vertices_.size() * sizeof(Eigen::Vector3f), cudaMemcpyDeviceToHost));
-    cudaSafeCall(cudaMemcpy(thrust::raw_pointer_cast(vertex_normals_.data()), thrust::raw_pointer_cast(trianglemesh.vertex_normals_.data()),
-                            vertex_normals_.size() * sizeof(Eigen::Vector3f), cudaMemcpyDeviceToHost));
-    cudaSafeCall(cudaMemcpy(thrust::raw_pointer_cast(vertex_colors_.data()), thrust::raw_pointer_cast(trianglemesh.vertex_colors_.data()),
-                            vertex_colors_.size() * sizeof(Eigen::Vector3f), cudaMemcpyDeviceToHost));
-    cudaSafeCall(cudaMemcpy(thrust::raw_pointer_cast(triangles_.data()), thrust::raw_pointer_cast(trianglemesh.triangles_.data()),
-                            triangles_.size() * sizeof(Eigen::Vector3i), cudaMemcpyDeviceToHost));
-    cudaSafeCall(cudaMemcpy(thrust::raw_pointer_cast(triangle_normals_.data()), thrust::raw_pointer_cast(trianglemesh.triangle_normals_.data()),
-                            triangle_normals_.size() * sizeof(Eigen::Vector3f), cudaMemcpyDeviceToHost));
-    cudaSafeCall(cudaMemcpy(thrust::raw_pointer_cast(triangle_uvs_.data()), thrust::raw_pointer_cast(trianglemesh.triangle_uvs_.data()),
-                            triangle_uvs_.size() * sizeof(Eigen::Vector2f), cudaMemcpyDeviceToHost));
+    copy_device_to_host(trianglemesh.vertices_, vertices_);
+    copy_device_to_host(trianglemesh.vertex_normals_, vertex_normals_);
+    copy_device_to_host(trianglemesh.vertex_colors_, vertex_colors_);
+    copy_device_to_host(trianglemesh.triangles_, triangles_);
+    copy_device_to_host(trianglemesh.triangle_normals_, triangle_normals_);
+    copy_device_to_host(trianglemesh.triangle_uvs_, triangle_uvs_);
     texture_.FromDevice(trianglemesh.texture_);
 }
 
@@ -54,18 +48,12 @@ void HostTriangleMesh::ToDevice(geometry::TriangleMesh& trianglemesh) const {
     trianglemesh.triangles_.resize(triangles_.size());
     trianglemesh.triangle_normals_.resize(triangle_normals_.size());
     trianglemesh.triangle_uvs_.resize(triangle_uvs_.size());
-    cudaSafeCall(cudaMemcpy(thrust::raw_pointer_cast(trianglemesh.vertices_.data()), thrust::raw_pointer_cast(vertices_.data()),
-                            vertices_.size() * sizeof(Eigen::Vector3f), cudaMemcpyHostToDevice));
-    cudaSafeCall(cudaMemcpy(thrust::raw_pointer_cast(trianglemesh.vertex_normals_.data()), thrust::raw_pointer_cast(vertex_normals_.data()),
-                            vertex_normals_.size() * sizeof(Eigen::Vector3f), cudaMemcpyHostToDevice));
-    cudaSafeCall(cudaMemcpy(thrust::raw_pointer_cast(trianglemesh.vertex_colors_.data()), thrust::raw_pointer_cast(vertex_colors_.data()),
-                            vertex_colors_.size() * sizeof(Eigen::Vector3f), cudaMemcpyHostToDevice));
-    cudaSafeCall(cudaMemcpy(thrust::raw_pointer_cast(trianglemesh.triangles_.data()), thrust::raw_pointer_cast(triangles_.data()),
-                            triangles_.size() * sizeof(Eigen::Vector3i), cudaMemcpyHostToDevice));
-    cudaSafeCall(cudaMemcpy(thrust::raw_pointer_cast(trianglemesh.triangle_normals_.data()), thrust::raw_pointer_cast(triangle_normals_.data()),
-                            triangle_normals_.size() * sizeof(Eigen::Vector3f), cudaMemcpyHostToDevice));
-    cudaSafeCall(cudaMemcpy(thrust::raw_pointer_cast(trianglemesh.triangle_uvs_.data()), thrust::raw_pointer_cast(triangle_uvs_.data()),
-                            triangle_uvs_.size() * sizeof(Eigen::Vector2f), cudaMemcpyHostToDevice));
+    copy_host_to_device(vertices_, trianglemesh.vertices_);
+    copy_host_to_device(vertex_normals_, trianglemesh.vertex_normals_);
+    copy_host_to_device(vertex_colors_, trianglemesh.vertex_colors_);
+    copy_host_to_device(triangles_, trianglemesh.triangles_);
+    copy_host_to_device(triangle_normals_, trianglemesh.triangle_normals_);
+    copy_host_to_device(triangle_uvs_, trianglemesh.triangle_uvs_);
     texture_.ToDevice(trianglemesh.texture_);
 }
 

diff --git a/src/cupoch/io/class_io/voxelgrid_io.cu b/src/cupoch/io/class_io/voxelgrid_io.cu
@@ -29,19 +29,15 @@ using namespace cupoch::io;
 void HostVoxelGrid::FromDevice(const geometry::VoxelGrid& voxelgrid) {
     voxels_keys_.resize(voxelgrid.voxels_keys_.size());
     voxels_values_.resize(voxelgrid.voxels_values_.size());
-    cudaSafeCall(cudaMemcpy(thrust::raw_pointer_cast(voxels_keys_.data()), thrust::raw_pointer_cast(voxelgrid.voxels_keys_.data()),
-                            voxels_keys_.size() * sizeof(Eigen::Vector3i), cudaMemcpyDeviceToHost));
-    cudaSafeCall(cudaMemcpy(thrust::raw_pointer_cast(voxels_values_.data()), thrust::raw_pointer_cast(voxelgrid.voxels_values_.data()),
-                            voxels_values_.size() * sizeof(geometry::Voxel), cudaMemcpyDeviceToHost));
+    copy_device_to_host(voxelgrid.voxels_keys_, voxels_keys_);
+    copy_device_to_host(voxelgrid.voxels_values_, voxels_values_);
 }
 
 void HostVoxelGrid::ToDevice(geometry::VoxelGrid& voxelgrid) const {
     voxelgrid.voxels_keys_.resize(voxels_keys_.size());
     voxelgrid.voxels_values_.resize(voxels_values_.size());
-    cudaSafeCall(cudaMemcpy(thrust::raw_pointer_cast(voxelgrid.voxels_keys_.data()), thrust::raw_pointer_cast(voxels_keys_.data()),
-                            voxels_keys_.size() * sizeof(Eigen::Vector3i), cudaMemcpyHostToDevice));
-    cudaSafeCall(cudaMemcpy(thrust::raw_pointer_cast(voxelgrid.voxels_values_.data()), thrust::raw_pointer_cast(voxels_values_.data()),
-                            voxels_values_.size() * sizeof(geometry::Voxel), cudaMemcpyHostToDevice));
+    copy_host_to_device(voxels_keys_, voxelgrid.voxels_keys_);
+    copy_host_to_device(voxels_values_, voxelgrid.voxels_values_);
 }
 
 void HostVoxelGrid::Clear() {

diff --git a/src/cupoch/utility/helper.h b/src/cupoch/utility/helper.h
@@ -438,25 +438,25 @@ __host__ __device__ inline thrust::tuple<int, int, int> KeyOf(size_t idx,
     return thrust::make_tuple(x, y, z);
 }
 
-template <typename T>
+template <typename T, typename ContainerType>
 inline void copy_device_to_host(const utility::device_vector<T> &src,
-                                utility::pinned_host_vector<T> &dist) {
+                                ContainerType &dist) {
     cudaSafeCall(cudaMemcpy(thrust::raw_pointer_cast(dist.data()),
                             thrust::raw_pointer_cast(src.data()),
                             src.size() * sizeof(T), cudaMemcpyDeviceToHost));
 }
 
-template <typename T>
-inline void copy_host_to_device(const utility::pinned_host_vector<T> &src,
+template <typename T, typename ContainerType>
+inline void copy_host_to_device(const ContainerType &src,
                                 utility::device_vector<T> &dist) {
     cudaSafeCall(cudaMemcpy(thrust::raw_pointer_cast(dist.data()),
                             thrust::raw_pointer_cast(src.data()),
                             src.size() * sizeof(T), cudaMemcpyHostToDevice));
 }
 
-template <typename T>
+template <typename T, typename ContainerType>
 inline void copy_device_to_device(const utility::device_vector<T> &src,
-                                  utility::device_vector<T> &dist) {
+                                  ContainerType &dist) {
     cudaSafeCall(cudaMemcpy(thrust::raw_pointer_cast(dist.data()),
                             thrust::raw_pointer_cast(src.data()),
                             src.size() * sizeof(T), cudaMemcpyDeviceToDevice));

diff --git a/src/python/pyproject.toml b/src/python/pyproject.toml
@@ -21,8 +21,8 @@ rospy-all = {version = "^0.0.1", optional = true}
 rosmaster = {version = "^1.15.11", optional = true}
 rosbag = {version = "^1.15.11", optional = true}
 transformations = {version = "^2022.9.26", optional = true}
-
 setuptools = "^72.2.0"
+
 [tool.poetry.dev-dependencies]
 twine = "^3.2.0"
 wheel = "^0.36.2"

diff --git a/src/tests/collision/collision.cpp b/src/tests/collision/collision.cpp
@@ -56,13 +56,15 @@ TEST(Collision, VoxelLineSet) {
     voxel.voxel_size_ = 1.0;
     voxel.AddVoxel(geometry::Voxel(Eigen::Vector3i(0, 0, 0)));
     voxel.AddVoxel(geometry::Voxel(Eigen::Vector3i(5, 0, 0)));
-    thrust::host_vector<Eigen::Vector3f> points;
-    points.push_back(Eigen::Vector3f(-0.1, -0.1, -0.1));
-    points.push_back(Eigen::Vector3f(-0.1, -0.1, 1.1));
-    points.push_back(Eigen::Vector3f(1.1, 1.1, 1.1));
-    thrust::host_vector<Eigen::Vector2i> lines;
-    lines.push_back(Eigen::Vector2i(0, 1));
-    lines.push_back(Eigen::Vector2i(0, 2));
+    std::vector<Eigen::Vector3f> points = {
+        Eigen::Vector3f(-0.1, -0.1, -0.1),
+        Eigen::Vector3f(-0.1, -0.1, 1.1),
+        Eigen::Vector3f(1.1, 1.1, 1.1)
+        };
+    std::vector<Eigen::Vector2i> lines = {
+        Eigen::Vector2i(0, 1),
+        Eigen::Vector2i(0, 2)
+    };
     lineset.SetPoints(points);
     lineset.SetLines(lines);
 

diff --git a/src/tests/geometry/distancetransform.cpp b/src/tests/geometry/distancetransform.cpp
@@ -32,10 +32,10 @@ using namespace unit_test;
 TEST(DistanceTransform, ComputeVoronoiDiagram) {
     geometry::VoxelGrid voxelgrid;
     voxelgrid.voxel_size_ = 1.0;
-    thrust::host_vector<Eigen::Vector3i> h_keys;
+    std::vector<Eigen::Vector3i> h_keys;
     Eigen::Vector3i ref(5, 5, 5);
     h_keys.push_back(ref);
-    voxelgrid.SetVoxels(h_keys, thrust::host_vector<geometry::Voxel>());
+    voxelgrid.SetVoxels(h_keys, std::vector<geometry::Voxel>());
     geometry::DistanceTransform dt(1.0, 512);
     dt.ComputeVoronoiDiagram(voxelgrid);
     auto v = dt.GetVoxel(Eigen::Vector3f(0.0, 0.0, 0.0));