isl-org / isl-org/Open3D

Implement unproject (depth to cloud) with "inv(K) @ pixels"?

Open
#4,890 1 comment 0 reactions 1 assignee Claimed by @reyanshsolis View on GitHub
question
Dominant language
C++
Stars
14k
Forks
2.6k
Avg merge
5d 18h
Merged PRs (30d)
6

Description

### Checklist

- [X] I have searched for [similar issues](https://github.com/isl-org/Open3D/issues).
- [X] For Python issues, I have tested with the [latest development wheel](http://www.open3d.org/docs/latest/getting_started.html#development-version-pip).
- [X] I have checked the [release documentation](http://www.open3d.org/docs/release/) and the [latest documentation](http://www.open3d.org/docs/latest/) (for `master` branch).

### My Question

## Issue
Currently, "unprojection" of depth to 3D points is implemented individually for each point. This implementation does not make use of matrix operation, and the code is complicated.

#### Legacy
https://github.com/isl-org/Open3D/blob/8868e7b41ffab68331c2fb46395583684048a548/cpp/open3d/geometry/PointCloudFactory.cpp#L121-L130

#### Tensor

https://github.com/isl-org/Open3D/blob/8868e7b41ffab68331c2fb46395583684048a548/cpp/open3d/t/geometry/kernel/PointCloudImpl.h#L117-L126 https://github.com/isl-org/Open3D/blob/8868e7b41ffab68331c2fb46395583684048a548/cpp/open3d/t/geometry/kernel/GeometryIndexer.h#L130-L139

## New approach

In the code we are essentially doing the following for each pixel `(r, c)` (`r` is row, `c` is coloumn):
```python
z = im_depth[r, c]
x = (c - cx) * z / fx
y = (r - cy) * z / fy
point = (pose @ np.array([x, y, z, 1]))[:3]
```

We know that
```python
K = [
[fx, 0, cx],
[0, fy, cy],
[0, 0, 1],
]

K_inv = [
[1 / fx, 0, -cx / fx],
[0, 1 / fy, -cy / fy],
[0, 0, 1],
]
```

It can be shown that the transformation is equivalent to:
```python
xyz = im_depth[r, c] * K_inv @ np.array([c, r, 1])
point = (pose @ xyz)[:3]
```

The above form can be further vectorized, such that we don't need to iterate point-by-point, see the sample code below.

## Sample code

Sample code with marix-based projection vs. open3d's `create_from_rgbd_image()` projection.

```python
import open3d as o3d
import numpy as np

class CameraPose:

def __init__(self, meta, mat):
self.metadata = meta
self.pose = mat

def __str__(self):
return 'Metadata : ' + ' '.join(map(str, self.metadata)) + '\n' + \
"Pose : " + "\n" + np.array_str(self.pose)

def read_trajectory(filename):
traj = []
with open(filename, 'r') as f:
metastr = f.readline()
while metastr:
metadata = list(map(int, metastr.split()))
mat = np.zeros(shape=(4, 4))
for i in range(4):
matstr = f.readline()
mat[i, :] = np.fromstring(matstr, dtype=float, sep=' \t')
traj.append(CameraPose(metadata, mat))
metastr = f.readline()
return traj

def main():
dataset = o3d.data.SampleRedwoodRGBDImages()
im_color = o3d.io.read_image(dataset.color_paths[0])
im_depth = o3d.io.read_image(dataset.depth_paths[0])
im_rgbd = o3d.geometry.RGBDImage.create_from_color_and_depth(
im_color, im_depth)

o3d_intrinsics = o3d.camera.PinholeCameraIntrinsic(
o3d.camera.PinholeCameraIntrinsicParameters.PrimeSenseDefault)
camera_poses = read_trajectory(dataset.odometry_log_path)
pose = camera_poses[0].pose
intrinsics = o3d_intrinsics.intrinsic_matrix
extrinsics = np.linalg.inv(pose)

# Open3D's projection
pcd = o3d.geometry.PointCloud.create_from_rgbd_image(
im_rgbd, o3d_intrinsics, extrinsics)

# Manual projection
im_color_arr = np.asarray(im_color)
im_depth_arr = np.asarray(im_depth)
height, width = im_depth_arr.shape
valid_depth_mask = im_depth_arr.flatten() > 0
depth_scale = 1000.0

# All pixel indices (height, width, 2)
# pixels.shape == (height, width, 2)
# pixels[r, c] == [c, r] # Since x-axis goes from top-left to top-right.
pixels = np.transpose(np.indices((width, height)), (2, 1, 0))
# (height * width, 2)
pixels = pixels.reshape((-1, 2))
# (num_points, 2)
pixels = pixels[valid_depth_mask]
# (num_points, 3)
pixels = np.hstack((pixels, np.ones((pixels.shape[0], 1))))
# (num_points, )
depths = im_depth_arr.flatten()[valid_depth_mask] / depth_scale
# C(num_points, 3)
points = depths.reshape((-1, 1)) * (np.linalg.inv(intrinsics) @ pixels.T).T
# (num_points, 4)
points = np.hstack((points, np.ones((points.shape[0], 1))))
# (num_points, 4)
points = (pose @ points.T).T
# (num_points, 3)
points = points[:, :3]

print(np.asarray(pcd.points))
print(np.asarray(points))
np.testing.assert_allclose(np.asarray(pcd.points),
np.asarray(points),
atol=1e-5,
rtol=1e-5)

# Visualize
pcd_manual = o3d.geometry.PointCloud()
pcd_manual.points = o3d.utility.Vector3dVector(points)
pcd_manual.colors = o3d.utility.Vector3dVector(
im_color_arr.reshape((-1, 3))[valid_depth_mask] / 255.0)
o3d.visualization.draw_geometries([pcd, pcd_manual])

if __name__ == '__main__':
main()
```

Contributor guide

No contributing guide indexed for this repository

Assessment

This issue has not been assessed yet.

Get new issues in your inbox

A short digest of beginner-friendly GitHub issues.