Skip to content

Feat: VoxelGrid transform, rotate, translate and scale #7339

New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Open
wants to merge 4 commits into
base: main
Choose a base branch
from
Open

Feat: VoxelGrid transform, rotate, translate and scale #7339

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
4 commits
Select commit Hold shift + click to select a range
8d5e712
feat: VoxelGrid scale, translate, rotate and transform
lmuffang Sep 26, 2025
9ce6bb4
dev: rotate VoxelGrid visualization accordingly
lmuffang Sep 29, 2025
8360407
test: VoxelGrid scale, translate, rotate, transform
lmuffang Sep 30, 2025
e4e21ff
fix: update boundung boxes creation
lmuffang Oct 8, 2025
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
107 changes: 72 additions & 35 deletions cpp/open3d/geometry/VoxelGrid.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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() {
Expand All @@ -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<double>() * 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<Eigen::Vector3d> VoxelGrid::GetAllVoxelCorners() const {
std::vector<Eigen::Vector3d> 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<double>() * 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<double>() + 1) * voxel_size_ +
origin_.array();
}

return voxel_corners;
}

Eigen::Vector3d VoxelGrid::GetCenter() const {
Expand All @@ -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<double>() * voxel_size_ + origin_ +
center += voxel.grid_index_.cast<double>() * voxel_size_ +
half_voxel_size;
}
center /= double(voxels_.size());
return center;
return origin_ + origin_rotation_ * center;
}

AxisAlignedBoundingBox VoxelGrid::GetAxisAlignedBoundingBox() const {
Expand All @@ -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 &center) {
utility::LogError("Not implemented");
voxel_size_ *= scale;
origin_ = (origin_ - center) * scale + center;
return *this;
}

VoxelGrid &VoxelGrid::Rotate(const Eigen::Matrix3d &R,
const Eigen::Vector3d &center) {
utility::LogError("Not implemented");
// Rotate the origin and the orientation
origin_ = R * (origin_ - center) + center;
origin_rotation_ = R * origin_rotation_;
return *this;
}

Expand Down Expand Up @@ -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<int>();
}

Expand All @@ -182,14 +219,14 @@ std::vector<Eigen::Vector3d> VoxelGrid::GetVoxelBoundingPoints(
double r = voxel_size_ / 2.0;
auto x = GetVoxelCenterCoordinate(index);
std::vector<Eigen::Vector3d> 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;
}

Expand Down
12 changes: 8 additions & 4 deletions cpp/open3d/geometry/VoxelGrid.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -72,6 +72,7 @@ class VoxelGrid : public Geometry3D {
Eigen::Vector3d GetMaxBound() const override;
Eigen::Vector3d GetCenter() const override;

std::vector<Eigen::Vector3d> GetAllVoxelCorners() const;
/// Creates the axis-aligned bounding box around the object.
/// Further details in AxisAlignedBoundingBox::AxisAlignedBoundingBox()
AxisAlignedBoundingBox GetAxisAlignedBoundingBox() const override;
Expand Down Expand Up @@ -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<double>() +
Eigen::Vector3d(0.5, 0.5, 0.5)) *
voxel_size_) +
origin_;
Eigen::Vector3d local = (voxel.grid_index_.cast<double>() +
Eigen::Vector3d(0.5, 0.5, 0.5)) *
voxel_size_;
return origin_ + origin_rotation_ * local;
} else {
return Eigen::Vector3d::Zero();
}
Expand Down Expand Up @@ -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<Eigen::Vector3i,
Voxel,
Expand Down
54 changes: 54 additions & 0 deletions cpp/open3d/io/file_format/FilePLY.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -266,6 +266,7 @@ struct PLYReaderState {
utility::ProgressBar *progress_bar;
std::vector<geometry::Voxel> *voxelgrid_ptr;
Eigen::Vector3d origin;
Eigen::Matrix3d origin_rotation;
double voxel_size;
long voxel_index;
long voxel_num;
Expand All @@ -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<void **>(&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;
Expand Down Expand Up @@ -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);

Expand Down Expand Up @@ -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);
Expand Down Expand Up @@ -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);

Expand Down Expand Up @@ -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_) {
Expand Down
8 changes: 5 additions & 3 deletions cpp/open3d/visualization/rendering/filament/FilamentGeometryBuffersBuilder.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -98,12 +98,14 @@ static std::shared_ptr<geometry::TriangleMesh> 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<double>() * voxel_grid.voxel_size_;
scaled_rot * voxel.grid_index_.cast<double>();
for (const Eigen::Vector3i& vertex_offset : kCuboidVertexOffsets) {
vertices.push_back(base_vertex + vertex_offset.cast<double>() *
voxel_grid.voxel_size_);
vertices.push_back(base_vertex +
scaled_rot * vertex_offset.cast<double>());
}

// Voxel color (applied to all points)
Expand Down
18 changes: 12 additions & 6 deletions cpp/open3d/visualization/shader/SimpleShader.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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<float>() *
voxel_grid.voxel_size_;
Eigen::Vector3f base_vertex =
voxel_grid.origin_.cast<float>() +
voxel.grid_index_.cast<float>() * voxel_grid.voxel_size_;
scaled_rot * voxel.grid_index_.cast<float>();
std::vector<Eigen::Vector3f> vertices;
for (const Eigen::Vector3i &vertex_offset : cuboid_vertex_offsets) {
vertices.push_back(base_vertex + vertex_offset.cast<float>() *
voxel_grid.voxel_size_);
vertices.push_back(base_vertex +
scaled_rot * vertex_offset.cast<float>());
}

// Voxel color (applied to all points)
Expand Down Expand Up @@ -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<float>() *
voxel_grid.voxel_size_;
Eigen::Vector3f base_vertex =
voxel_grid.origin_.cast<float>() +
voxel.grid_index_.cast<float>() * voxel_grid.voxel_size_;
scaled_rot * voxel.grid_index_.cast<float>();
std::vector<Eigen::Vector3f> vertices;
for (const Eigen::Vector3i &vertex_offset : cuboid_vertex_offsets) {
vertices.push_back(base_vertex + vertex_offset.cast<float>() *
voxel_grid.voxel_size_);
vertices.push_back(base_vertex +
scaled_rot * vertex_offset.cast<float>());
}

// Voxel color (applied to all points)
Expand Down
3 changes: 3 additions & 0 deletions cpp/pybind/geometry/voxelgrid.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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.");

Expand Down
Loading