-
-
Notifications
You must be signed in to change notification settings - Fork 107
/
rgbd_integration.py
26 lines (23 loc) · 1.09 KB
/
rgbd_integration.py
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
import cupoch as cph
from trajectory_io import *
import numpy as np
if __name__ == "__main__":
camera_poses = read_trajectory("../../testdata/rgbd/odometry.log")
volume = cph.integration.ScalableTSDFVolume(
voxel_length=8.0 / 512.0, sdf_trunc=0.04, color_type=cph.integration.TSDFVolumeColorType.RGB8
)
for i in range(len(camera_poses)):
print("Integrate {:d}-th image into the volume.".format(i))
color = cph.io.read_image("../../testdata/rgbd/color/{:05d}.jpg".format(i))
depth = cph.io.read_image("../../testdata/rgbd/depth/{:05d}.png".format(i))
rgbd = cph.geometry.RGBDImage.create_from_color_and_depth(
color, depth, depth_trunc=4.0, convert_rgb_to_intensity=False
)
volume.integrate(
rgbd,
cph.camera.PinholeCameraIntrinsic(cph.camera.PinholeCameraIntrinsicParameters.PrimeSenseDefault),
np.linalg.inv(camera_poses[i].pose),
)
print("Extract a point cloud from the volume and visualize it.")
pcd = volume.extract_point_cloud()
cph.visualization.draw_geometries([pcd])