diff --git a/cpp/open3d/geometry/VoxelGrid.cpp b/cpp/open3d/geometry/VoxelGrid.cpp index aa028b22ac9..c09055fe93e 100644 --- a/cpp/open3d/geometry/VoxelGrid.cpp +++ b/cpp/open3d/geometry/VoxelGrid.cpp @@ -24,6 +24,7 @@ VoxelGrid::VoxelGrid(const VoxelGrid &src_voxel_grid) : Geometry3D(Geometry::GeometryType::VoxelGrid), voxel_size_(src_voxel_grid.voxel_size_), origin_(src_voxel_grid.origin_), + origin_rotation_(src_voxel_grid.origin_rotation_), voxels_(src_voxel_grid.voxels_) {} VoxelGrid &VoxelGrid::Clear() { @@ -38,28 +39,56 @@ bool VoxelGrid::IsEmpty() const { return !HasVoxels(); } Eigen::Vector3d VoxelGrid::GetMinBound() const { if (!HasVoxels()) { return origin_; - } else { - Eigen::Array3i min_grid_index = voxels_.begin()->first; - for (const auto &it : voxels_) { - const geometry::Voxel &voxel = it.second; - min_grid_index = min_grid_index.min(voxel.grid_index_.array()); - } - return min_grid_index.cast() * voxel_size_ + origin_.array(); } + + const auto corners = GetAllVoxelCorners(); + + Eigen::Vector3d min_bound = corners.front(); + for (const auto &p : corners) { + min_bound = min_bound.cwiseMin(p); + } + + return min_bound; } Eigen::Vector3d VoxelGrid::GetMaxBound() const { if (!HasVoxels()) { return origin_; - } else { - Eigen::Array3i max_grid_index = voxels_.begin()->first; - for (const auto &it : voxels_) { - const geometry::Voxel &voxel = it.second; - max_grid_index = max_grid_index.max(voxel.grid_index_.array()); + } + + const auto corners = GetAllVoxelCorners(); + + Eigen::Vector3d max_bound = corners.front(); + for (const auto &p : corners) { + max_bound = max_bound.cwiseMax(p); + } + + return max_bound; +} + +std::vector VoxelGrid::GetAllVoxelCorners() const { + std::vector voxel_corners; + voxel_corners.reserve(voxels_.size() * 8); + + // Precompute voxel corner offsets in local grid space + static const Eigen::Vector3d offsets[8] = {{0, 0, 0}, {1, 0, 0}, {0, 1, 0}, + {1, 1, 0}, {0, 0, 1}, {1, 0, 1}, + {0, 1, 1}, {1, 1, 1}}; + + for (const auto &it : voxels_) { + const Eigen::Vector3i &grid_index = it.second.grid_index_; + Eigen::Vector3d base = + origin_ + + origin_rotation_ * (grid_index.cast() * voxel_size_); + + // Add all 8 corners directly without constructing temporary vectors + for (const auto &off : offsets) { + voxel_corners.push_back(base + + origin_rotation_ * (off * voxel_size_)); } - return (max_grid_index.cast() + 1) * voxel_size_ + - origin_.array(); } + + return voxel_corners; } Eigen::Vector3d VoxelGrid::GetCenter() const { @@ -71,11 +100,11 @@ Eigen::Vector3d VoxelGrid::GetCenter() const { 0.5 * voxel_size_); for (const auto &it : voxels_) { const geometry::Voxel &voxel = it.second; - center += voxel.grid_index_.cast() * voxel_size_ + origin_ + + center += voxel.grid_index_.cast() * voxel_size_ + half_voxel_size; } center /= double(voxels_.size()); - return center; + return origin_ + origin_rotation_ * center; } AxisAlignedBoundingBox VoxelGrid::GetAxisAlignedBoundingBox() const { @@ -85,35 +114,41 @@ AxisAlignedBoundingBox VoxelGrid::GetAxisAlignedBoundingBox() const { return box; } -OrientedBoundingBox VoxelGrid::GetOrientedBoundingBox(bool) const { - return OrientedBoundingBox::CreateFromAxisAlignedBoundingBox( - GetAxisAlignedBoundingBox()); +OrientedBoundingBox VoxelGrid::GetOrientedBoundingBox(bool robust) const { + return OrientedBoundingBox::CreateFromPoints(GetAllVoxelCorners(), robust); } -OrientedBoundingBox VoxelGrid::GetMinimalOrientedBoundingBox(bool) const { - return OrientedBoundingBox::CreateFromAxisAlignedBoundingBox( - GetAxisAlignedBoundingBox()); +OrientedBoundingBox VoxelGrid::GetMinimalOrientedBoundingBox( + bool robust) const { + return OrientedBoundingBox::CreateFromPointsMinimal(GetAllVoxelCorners(), + robust); } VoxelGrid &VoxelGrid::Transform(const Eigen::Matrix4d &transformation) { - utility::LogError("VoxelGrid::Transform is not supported"); + // Only update origin_ and origin_rotation_ (lazy transform) + origin_ = (transformation.block<3, 3>(0, 0) * origin_) + + transformation.block<3, 1>(0, 3); + origin_rotation_ = transformation.block<3, 3>(0, 0) * origin_rotation_; return *this; } VoxelGrid &VoxelGrid::Translate(const Eigen::Vector3d &translation, bool relative) { - utility::LogError("Not implemented"); + origin_ += (relative ? translation : translation - GetCenter()); return *this; } VoxelGrid &VoxelGrid::Scale(const double scale, const Eigen::Vector3d ¢er) { - utility::LogError("Not implemented"); + voxel_size_ *= scale; + origin_ = (origin_ - center) * scale + center; return *this; } VoxelGrid &VoxelGrid::Rotate(const Eigen::Matrix3d &R, const Eigen::Vector3d ¢er) { - utility::LogError("Not implemented"); + // Rotate the origin and the orientation + origin_ = R * (origin_ - center) + center; + origin_rotation_ = R * origin_rotation_; return *this; } @@ -173,7 +208,9 @@ VoxelGrid VoxelGrid::operator+(const VoxelGrid &voxelgrid) const { } Eigen::Vector3i VoxelGrid::GetVoxel(const Eigen::Vector3d &point) const { - Eigen::Vector3d voxel_f = (point - origin_) / voxel_size_; + // Convert world point to local grid frame + Eigen::Vector3d local = origin_rotation_.transpose() * (point - origin_); + Eigen::Vector3d voxel_f = local / voxel_size_; return (Eigen::floor(voxel_f.array())).cast(); } @@ -182,14 +219,14 @@ std::vector VoxelGrid::GetVoxelBoundingPoints( double r = voxel_size_ / 2.0; auto x = GetVoxelCenterCoordinate(index); std::vector points; - points.push_back(x + Eigen::Vector3d(-r, -r, -r)); - points.push_back(x + Eigen::Vector3d(-r, -r, r)); - points.push_back(x + Eigen::Vector3d(r, -r, -r)); - points.push_back(x + Eigen::Vector3d(r, -r, r)); - points.push_back(x + Eigen::Vector3d(-r, r, -r)); - points.push_back(x + Eigen::Vector3d(-r, r, r)); - points.push_back(x + Eigen::Vector3d(r, r, -r)); - points.push_back(x + Eigen::Vector3d(r, r, r)); + points.push_back(x + origin_rotation_ * Eigen::Vector3d(-r, -r, -r)); + points.push_back(x + origin_rotation_ * Eigen::Vector3d(-r, -r, r)); + points.push_back(x + origin_rotation_ * Eigen::Vector3d(r, -r, -r)); + points.push_back(x + origin_rotation_ * Eigen::Vector3d(r, -r, r)); + points.push_back(x + origin_rotation_ * Eigen::Vector3d(-r, r, -r)); + points.push_back(x + origin_rotation_ * Eigen::Vector3d(-r, r, r)); + points.push_back(x + origin_rotation_ * Eigen::Vector3d(r, r, -r)); + points.push_back(x + origin_rotation_ * Eigen::Vector3d(r, r, r)); return points; } diff --git a/cpp/open3d/geometry/VoxelGrid.h b/cpp/open3d/geometry/VoxelGrid.h index 258e975a12d..603a45ea9a6 100644 --- a/cpp/open3d/geometry/VoxelGrid.h +++ b/cpp/open3d/geometry/VoxelGrid.h @@ -72,6 +72,7 @@ class VoxelGrid : public Geometry3D { Eigen::Vector3d GetMaxBound() const override; Eigen::Vector3d GetCenter() const override; + std::vector GetAllVoxelCorners() const; /// Creates the axis-aligned bounding box around the object. /// Further details in AxisAlignedBoundingBox::AxisAlignedBoundingBox() AxisAlignedBoundingBox GetAxisAlignedBoundingBox() const override; @@ -111,10 +112,10 @@ class VoxelGrid : public Geometry3D { auto it = voxels_.find(idx); if (it != voxels_.end()) { auto voxel = it->second; - return ((voxel.grid_index_.cast() + - Eigen::Vector3d(0.5, 0.5, 0.5)) * - voxel_size_) + - origin_; + Eigen::Vector3d local = (voxel.grid_index_.cast() + + Eigen::Vector3d(0.5, 0.5, 0.5)) * + voxel_size_; + return origin_ + origin_rotation_ * local; } else { return Eigen::Vector3d::Zero(); } @@ -259,6 +260,9 @@ class VoxelGrid : public Geometry3D { double voxel_size_ = 0.0; /// Coordinate of the origin point. Eigen::Vector3d origin_ = Eigen::Vector3d::Zero(); + /// Orientation (rotation) of the voxel grid (world = origin_ + + /// origin_rotation_ * local) + Eigen::Matrix3d origin_rotation_ = Eigen::Matrix3d::Identity(); /// Voxels contained in voxel grid std::unordered_map *voxelgrid_ptr; Eigen::Vector3d origin; + Eigen::Matrix3d origin_rotation; double voxel_size; long voxel_index; long voxel_num; @@ -284,6 +285,19 @@ int ReadOriginCallback(p_ply_argument argument) { return 1; } +int ReadOriginRotationCallback(p_ply_argument argument) { + PLYReaderState *state_ptr; + long index; + ply_get_argument_user_data(argument, reinterpret_cast(&state_ptr), + &index); + + double value = ply_get_argument_value(argument); + int row = index / 3; + int col = index % 3; + state_ptr->origin_rotation(row, col) = value; + return 1; +} + int ReadScaleCallback(p_ply_argument argument) { PLYReaderState *state_ptr; long index; @@ -874,6 +888,24 @@ bool ReadVoxelGridFromPLY(const std::string &filename, ply_set_read_cb(ply_file, "origin", "x", ReadOriginCallback, &state, 0); ply_set_read_cb(ply_file, "origin", "y", ReadOriginCallback, &state, 1); ply_set_read_cb(ply_file, "origin", "z", ReadOriginCallback, &state, 2); + ply_set_read_cb(ply_file, "origin_rotation", "r00", + ReadOriginRotationCallback, &state, 0); + ply_set_read_cb(ply_file, "origin_rotation", "r01", + ReadOriginRotationCallback, &state, 1); + ply_set_read_cb(ply_file, "origin_rotation", "r02", + ReadOriginRotationCallback, &state, 2); + ply_set_read_cb(ply_file, "origin_rotation", "r10", + ReadOriginRotationCallback, &state, 3); + ply_set_read_cb(ply_file, "origin_rotation", "r11", + ReadOriginRotationCallback, &state, 4); + ply_set_read_cb(ply_file, "origin_rotation", "r12", + ReadOriginRotationCallback, &state, 5); + ply_set_read_cb(ply_file, "origin_rotation", "r20", + ReadOriginRotationCallback, &state, 6); + ply_set_read_cb(ply_file, "origin_rotation", "r21", + ReadOriginRotationCallback, &state, 7); + ply_set_read_cb(ply_file, "origin_rotation", "r22", + ReadOriginRotationCallback, &state, 8); ply_set_read_cb(ply_file, "voxel_size", "val", ReadScaleCallback, &state, 0); @@ -901,6 +933,7 @@ bool ReadVoxelGridFromPLY(const std::string &filename, voxelgrid.AddVoxel(geometry::Voxel(it.grid_index_)); } voxelgrid.origin_ = state.origin; + voxelgrid.origin_rotation_ = state.origin_rotation; voxelgrid.voxel_size_ = state.voxel_size; ply_close(ply_file); @@ -930,6 +963,16 @@ bool WriteVoxelGridToPLY(const std::string &filename, ply_add_property(ply_file, "x", PLY_DOUBLE, PLY_DOUBLE, PLY_DOUBLE); ply_add_property(ply_file, "y", PLY_DOUBLE, PLY_DOUBLE, PLY_DOUBLE); ply_add_property(ply_file, "z", PLY_DOUBLE, PLY_DOUBLE, PLY_DOUBLE); + ply_add_element(ply_file, "origin_rotation", 1); + ply_add_property(ply_file, "r00", PLY_DOUBLE, PLY_DOUBLE, PLY_DOUBLE); + ply_add_property(ply_file, "r01", PLY_DOUBLE, PLY_DOUBLE, PLY_DOUBLE); + ply_add_property(ply_file, "r02", PLY_DOUBLE, PLY_DOUBLE, PLY_DOUBLE); + ply_add_property(ply_file, "r10", PLY_DOUBLE, PLY_DOUBLE, PLY_DOUBLE); + ply_add_property(ply_file, "r11", PLY_DOUBLE, PLY_DOUBLE, PLY_DOUBLE); + ply_add_property(ply_file, "r12", PLY_DOUBLE, PLY_DOUBLE, PLY_DOUBLE); + ply_add_property(ply_file, "r20", PLY_DOUBLE, PLY_DOUBLE, PLY_DOUBLE); + ply_add_property(ply_file, "r21", PLY_DOUBLE, PLY_DOUBLE, PLY_DOUBLE); + ply_add_property(ply_file, "r22", PLY_DOUBLE, PLY_DOUBLE, PLY_DOUBLE); ply_add_element(ply_file, "voxel_size", 1); ply_add_property(ply_file, "val", PLY_DOUBLE, PLY_DOUBLE, PLY_DOUBLE); @@ -960,6 +1003,17 @@ bool WriteVoxelGridToPLY(const std::string &filename, ply_write(ply_file, origin(0)); ply_write(ply_file, origin(1)); ply_write(ply_file, origin(2)); + const Eigen::Matrix3d &origin_rotation = voxelgrid.origin_rotation_; + ply_write(ply_file, origin_rotation(0, 0)); + ply_write(ply_file, origin_rotation(0, 1)); + ply_write(ply_file, origin_rotation(0, 2)); + ply_write(ply_file, origin_rotation(1, 0)); + ply_write(ply_file, origin_rotation(1, 1)); + ply_write(ply_file, origin_rotation(1, 2)); + ply_write(ply_file, origin_rotation(2, 0)); + ply_write(ply_file, origin_rotation(2, 1)); + ply_write(ply_file, origin_rotation(2, 2)); + ply_write(ply_file, voxelgrid.voxel_size_); for (auto &it : voxelgrid.voxels_) { diff --git a/cpp/open3d/visualization/rendering/filament/FilamentGeometryBuffersBuilder.cpp b/cpp/open3d/visualization/rendering/filament/FilamentGeometryBuffersBuilder.cpp index 1a09943c699..ad5f1172a75 100644 --- a/cpp/open3d/visualization/rendering/filament/FilamentGeometryBuffersBuilder.cpp +++ b/cpp/open3d/visualization/rendering/filament/FilamentGeometryBuffersBuilder.cpp @@ -98,12 +98,14 @@ static std::shared_ptr CreateTriangleMeshFromVoxelGrid( vertices.clear(); const geometry::Voxel& voxel = it.second; // 8 vertices in a voxel + const Eigen::Matrix3d scaled_rot = + voxel_grid.origin_rotation_ * voxel_grid.voxel_size_; Eigen::Vector3d base_vertex = voxel_grid.origin_ + - voxel.grid_index_.cast() * voxel_grid.voxel_size_; + scaled_rot * voxel.grid_index_.cast(); for (const Eigen::Vector3i& vertex_offset : kCuboidVertexOffsets) { - vertices.push_back(base_vertex + vertex_offset.cast() * - voxel_grid.voxel_size_); + vertices.push_back(base_vertex + + scaled_rot * vertex_offset.cast()); } // Voxel color (applied to all points) diff --git a/cpp/open3d/visualization/shader/SimpleShader.cpp b/cpp/open3d/visualization/shader/SimpleShader.cpp index c14db429639..f120061bbb2 100644 --- a/cpp/open3d/visualization/shader/SimpleShader.cpp +++ b/cpp/open3d/visualization/shader/SimpleShader.cpp @@ -537,13 +537,16 @@ bool SimpleShaderForVoxelGridLine::PrepareBinding( for (auto &it : voxel_grid.voxels_) { const geometry::Voxel &voxel = it.second; // 8 vertices in a voxel + const Eigen::Matrix3f scaled_rot = + voxel_grid.origin_rotation_.cast() * + voxel_grid.voxel_size_; Eigen::Vector3f base_vertex = voxel_grid.origin_.cast() + - voxel.grid_index_.cast() * voxel_grid.voxel_size_; + scaled_rot * voxel.grid_index_.cast(); std::vector vertices; for (const Eigen::Vector3i &vertex_offset : cuboid_vertex_offsets) { - vertices.push_back(base_vertex + vertex_offset.cast() * - voxel_grid.voxel_size_); + vertices.push_back(base_vertex + + scaled_rot * vertex_offset.cast()); } // Voxel color (applied to all points) @@ -628,13 +631,16 @@ bool SimpleShaderForVoxelGridFace::PrepareBinding( for (auto &it : voxel_grid.voxels_) { const geometry::Voxel &voxel = it.second; // 8 vertices in a voxel + const Eigen::Matrix3f scaled_rot = + voxel_grid.origin_rotation_.cast() * + voxel_grid.voxel_size_; Eigen::Vector3f base_vertex = voxel_grid.origin_.cast() + - voxel.grid_index_.cast() * voxel_grid.voxel_size_; + scaled_rot * voxel.grid_index_.cast(); std::vector vertices; for (const Eigen::Vector3i &vertex_offset : cuboid_vertex_offsets) { - vertices.push_back(base_vertex + vertex_offset.cast() * - voxel_grid.voxel_size_); + vertices.push_back(base_vertex + + scaled_rot * vertex_offset.cast()); } // Voxel color (applied to all points) diff --git a/cpp/pybind/geometry/voxelgrid.cpp b/cpp/pybind/geometry/voxelgrid.cpp index d0ce7ed7fea..84d5af5b871 100644 --- a/cpp/pybind/geometry/voxelgrid.cpp +++ b/cpp/pybind/geometry/voxelgrid.cpp @@ -176,6 +176,9 @@ void pybind_voxelgrid_definitions(py::module &m) { .def_readwrite("origin", &VoxelGrid::origin_, "``float64`` vector of length 3: Coordinate of the " "origin point.") + .def_readwrite("origin_rotation", &VoxelGrid::origin_rotation_, + "``float64`` 3x3 matrix: Rotation matrix of the " + "origin point.") .def_readwrite("voxel_size", &VoxelGrid::voxel_size_, "``float64`` Size of the voxel."); diff --git a/cpp/tests/geometry/VoxelGrid.cpp b/cpp/tests/geometry/VoxelGrid.cpp index 51e13b720a1..f4ea49ea18a 100644 --- a/cpp/tests/geometry/VoxelGrid.cpp +++ b/cpp/tests/geometry/VoxelGrid.cpp @@ -8,7 +8,9 @@ #include "open3d/geometry/VoxelGrid.h" #include "open3d/geometry/LineSet.h" +#include "open3d/geometry/PointCloud.h" #include "open3d/geometry/TriangleMesh.h" +#include "open3d/io/PointCloudIO.h" #include "open3d/visualization/utility/DrawGeometry.h" #include "tests/Tests.h" @@ -56,5 +58,159 @@ TEST(VoxelGrid, Visualization) { // visualization::DrawGeometries({voxel_grid}); } +TEST(VoxelGrid, Scale) { + open3d::geometry::PointCloud pcd; + open3d::data::PLYPointCloud pointcloud_ply; + open3d::io::ReadPointCloud(pointcloud_ply.GetPath(), pcd); + + double voxel_size = 0.01; + auto voxel_grid = std::make_shared(); + voxel_grid = geometry::VoxelGrid::CreateFromPointCloud( + pcd, voxel_size, geometry::VoxelGrid::VoxelPoolingMode::AVG); + + Eigen::Vector3d original_center = voxel_grid->GetCenter(); + + // Get original current voxels centers + std::vector original_centers; + for (const auto &it : voxel_grid->voxels_) { + original_centers.push_back( + voxel_grid->GetVoxelCenterCoordinate(it.second.grid_index_)); + } + + // Scale the voxel grid by 2x + double scale = 2.0; + voxel_grid->Scale(scale, voxel_grid->GetCenter()); + EXPECT_NEAR(voxel_grid->voxel_size_, voxel_size * scale, 1e-6); + // Get new current voxels centers + std::vector new_centers; + for (const auto &it : voxel_grid->voxels_) { + new_centers.push_back( + voxel_grid->GetVoxelCenterCoordinate(it.second.grid_index_)); + } + // Check that the new centers are scaled by 2x + EXPECT_EQ(original_centers.size(), new_centers.size()); + for (size_t i = 0; i < original_centers.size(); i++) { + Eigen::Vector3d vec = new_centers[i] - voxel_grid->GetCenter(); + Eigen::Vector3d vec_orig = original_centers[i] - original_center; + ExpectEQ(vec, (vec_orig * scale).eval()); + } +} + +TEST(VoxelGrid, Translate) { + open3d::geometry::PointCloud pcd; + open3d::data::PLYPointCloud pointcloud_ply; + open3d::io::ReadPointCloud(pointcloud_ply.GetPath(), pcd); + + double voxel_size = 0.01; + auto voxel_grid = std::make_shared(); + voxel_grid = geometry::VoxelGrid::CreateFromPointCloud( + pcd, voxel_size, geometry::VoxelGrid::VoxelPoolingMode::AVG); + + Eigen::Vector3d original_center = voxel_grid->GetCenter(); + + // Get original current voxels centers + std::vector original_centers; + for (const auto &it : voxel_grid->voxels_) { + original_centers.push_back( + voxel_grid->GetVoxelCenterCoordinate(it.second.grid_index_)); + } + + // Translate by (1, 2, 3) + Eigen::Vector3d translation(1, 2, 3); + voxel_grid->Translate(translation, true); + EXPECT_NEAR(voxel_grid->voxel_size_, voxel_size, 1e-6); + ExpectEQ(voxel_grid->GetCenter(), (original_center + translation).eval()); + // Get new current voxels centers + std::vector new_centers; + for (const auto &it : voxel_grid->voxels_) { + new_centers.push_back( + voxel_grid->GetVoxelCenterCoordinate(it.second.grid_index_)); + } + // Check that the new centers are translated by (1, 2, 3) + EXPECT_EQ(original_centers.size(), new_centers.size()); + for (size_t i = 0; i < original_centers.size(); i++) { + ExpectEQ(new_centers[i], (original_centers[i] + translation).eval()); + } +} + +TEST(VoxelGrid, Rotate) { + open3d::geometry::PointCloud pcd; + open3d::data::PLYPointCloud pointcloud_ply; + open3d::io::ReadPointCloud(pointcloud_ply.GetPath(), pcd); + + double voxel_size = 0.01; + auto voxel_grid = std::make_shared(); + voxel_grid = geometry::VoxelGrid::CreateFromPointCloud( + pcd, voxel_size, geometry::VoxelGrid::VoxelPoolingMode::AVG); + + Eigen::Vector3d original_center = voxel_grid->GetCenter(); + + // Get original current voxels centers + std::vector original_centers; + for (const auto &it : voxel_grid->voxels_) { + original_centers.push_back( + voxel_grid->GetVoxelCenterCoordinate(it.second.grid_index_)); + } + + // Rotate by 90 degrees around Z axis + Eigen::Matrix3d R; + R = Eigen::AngleAxisd(M_PI / 2, Eigen::Vector3d::UnitZ()); + voxel_grid->Rotate(R, original_center); + EXPECT_NEAR(voxel_grid->voxel_size_, voxel_size, 1e-6); + // Get new current voxels centers + std::vector new_centers; + for (const auto &it : voxel_grid->voxels_) { + new_centers.push_back( + voxel_grid->GetVoxelCenterCoordinate(it.second.grid_index_)); + } + // Check that the new centers are rotated by R + EXPECT_EQ(original_centers.size(), new_centers.size()); + for (size_t i = 0; i < original_centers.size(); i++) { + ExpectEQ(new_centers[i], + (R * (original_centers[i] - original_center) + original_center) + .eval()); + } +} + +TEST(VoxelGrid, Transform) { + open3d::geometry::PointCloud pcd; + open3d::data::PLYPointCloud pointcloud_ply; + open3d::io::ReadPointCloud(pointcloud_ply.GetPath(), pcd); + + double voxel_size = 0.01; + auto voxel_grid = std::make_shared(); + voxel_grid = geometry::VoxelGrid::CreateFromPointCloud( + pcd, voxel_size, geometry::VoxelGrid::VoxelPoolingMode::AVG); + + // Get original current voxels centers + std::vector original_centers; + for (const auto &it : voxel_grid->voxels_) { + original_centers.push_back( + voxel_grid->GetVoxelCenterCoordinate(it.second.grid_index_)); + } + + // Transform + Eigen::Matrix4d transformation; + transformation << 0.10, 0.20, 0.30, 0.40, 0.50, 0.60, 0.70, 0.80, 0.90, + 0.10, 0.11, 0.12, 0.13, 0.14, 0.15, 0.16; + voxel_grid->Transform(transformation); + EXPECT_NEAR(voxel_grid->voxel_size_, voxel_size, 1e-6); + // Get new current voxels centers + std::vector new_centers; + for (const auto &it : voxel_grid->voxels_) { + new_centers.push_back( + voxel_grid->GetVoxelCenterCoordinate(it.second.grid_index_)); + } + // Check that the new centers are transformed by transformation + EXPECT_EQ(original_centers.size(), new_centers.size()); + for (size_t i = 0; i < original_centers.size(); i++) { + Eigen::Vector4d vec; + vec << original_centers[i](0), original_centers[i](1), + original_centers[i](2), 1; + vec = transformation * vec; + ExpectEQ(new_centers[i], vec.head<3>().eval()); + } +} + } // namespace tests } // namespace open3d