revoscan_5_parse_cache.py

import numpy as np
import json

# load the project JSON to get the depth scale
with open('/path/to/project/data/subfolder/property.rvproj','rt') as f:
    project = json.load(f)
depth_scale = project['scan_param']['depth_scale']
print(depth_scale)

# load projection matrix from 2D to 3D
Q = np.fromfile('/path/to/project/data/subfolder/param/Q.bin', count=4*4, dtype=np.float32).reshape((4,4,))
print('Q', Q)

def load_frame(filename):
    # load depth map, convert to float, scale
    frame = np.fromfile(filename, dtype=np.uint16).reshape((600,800,)).astype(np.float32) * depth_scale

    # make [x,y,z,1] vectors
    xs = np.arange(0,800).astype(np.float32)
    ys = np.arange(0,600).astype(np.float32)
    xs = np.tile(xs.reshape((1,800)),(600,1,))
    ys = np.tile(ys.reshape((600,1)),(1,800,))
    data = np.stack([xs,ys,frame,np.ones_like(frame),],-1)

    # flatten 2D image to 1D pixel collection and drop pixels without depth
    data = data.reshape((-1,4))
    data = data[data[:,2] > 0]

    # apply projection matrix Q to get ray direction, then scale by z
    zs = data[:,2:3]
    data = np.matmul(Q.reshape((1,4,4,))[:,:3,:4], data.reshape((-1,4,1,)))
    data = data[:,0:3,0] * zs / data[:,2:3,0]

    # RevoScan appears to work with inverted Y and Z axis
    data = np.stack([data[:,0],-data[:,1],-data[:,2]], -1)

    # check that the object size is reasonable
    print('extends', np.max(data,0)-np.min(data,0))

    if True:
        # load the transformation matrix for this frame
        T = np.fromfile(filename.replace('.dph','.inf'), offset=0x10, count=4*4, dtype=np.float64).reshape((4, 4,))
        print('T', T)
        # convert point cloud to [x,y,z,1] and multipla and reduce to [x/w,y/w,z/w]
        data = np.concatenate([data,np.ones_like(data[:,0:1])],-1)
        data = np.matmul(T.reshape((1,4,4,)), data.reshape((-1,4,1,)))
        data = data[:,0:3,0] / data[:,3:4,0]

    return data

# load demo frames and merge them into one point cloud
data = np.concatenate([
    load_frame('/path/to/project/data/subfolder/cache/frame_000_0000.dph'),
    load_frame('/path/to/project/data/subfolder/cache/frame_000_0001.dph'),
    load_frame('/path/to/project/data/subfolder/cache/frame_001_0000.dph'),
    load_frame('/path/to/project/data/subfolder/cache/frame_001_0001.dph'),
    load_frame('/path/to/project/data/subfolder/cache/frame_001_0002.dph'),
], 0)

# display result
import open3d as o3d
pcd = o3d.geometry.PointCloud()
pcd.points = o3d.utility.Vector3dVector(data)
o3d.visualization.draw_geometries([pcd])