Quick script to visualize a depth image from an ASUS Primesense camera for course I gave on March 2022 at ENSTA Paris

camera3d.py

"""
camera3d.py
Victor Talpaert 2022
Quick script to visualize a depth image from an ASUS Primesense camera for course I gave on March 2022 at ENSTA Paris

======================
Instructions

# Install dependencies
pip3 install primesense numpy matplotlib opencv-python

# Driver
Download driver from https://structure.io/openni
Either system install or simply extract the folder anywhere, the directory containing libOpenNI2.so(.dll) will be your PATH_TO_LIB_FOLDER
See https://codeyarns.com/tech/2014-05-06-depthsense-error-some-dll-files-are-missing.html if you run in some "device not found error"
My fix on Ubuntu 20 was to run :
sudo ln -s /usr/lib/libOpenNI2.so.0 /usr/lib/libOpenNI2.so

# Run
python3 camera3d.py

"""

from itertools import count
from time import sleep
from primesense import openni2
import numpy as np
from matplotlib import pyplot as plt
import cv2


PATH_TO_LIB_FOLDER = "/usr/lib/"  # See instructions, can also accept the path of the OpenNI redistribution

DEPTH_IM_HEIGHT = 480
DEPTH_IM_WIDTH = 640
CENTER_X = float(DEPTH_IM_WIDTH - 1) / 2.0
CENTER_Y = float(DEPTH_IM_HEIGHT - 1) / 2.0
FOCAL_X = 525.0
FOCAL_Y = 525.0
FPS = 30
MM_TO_METERS = 0.001

flip_images = False
show_images = True

try:
    openni2.initialize(PATH_TO_LIB_FOLDER)
    dev = openni2.Device.open_any()
    print(dev.get_device_info())

    depth_stream = dev.create_depth_stream()
    depth_stream.configure_mode(
        DEPTH_IM_WIDTH,
        DEPTH_IM_HEIGHT,
        FPS,
        openni2.PIXEL_FORMAT_DEPTH_1_MM,
    )
    depth_stream.start()

    for frame_number in count():
        frame = depth_stream.read_frame()
        raw_buf = frame.get_buffer_as_uint16()
        buf_array = np.array(
            [
                raw_buf[i]
                for i in range(
                    DEPTH_IM_WIDTH
                    * DEPTH_IM_HEIGHT
                )
            ]
        )
        # convert to image in meters
        depth_image = buf_array.reshape(
            DEPTH_IM_HEIGHT, DEPTH_IM_WIDTH
        )
        depth_image = depth_image * MM_TO_METERS  # convert to meters
        if flip_images:
            depth_image = np.flipud(depth_image)
        else:
            depth_image = np.fliplr(depth_image)

        if show_images:
            # SLOW VERSION
            # plt.imshow(depth_image, cmap='gray')
            # plt.show(block=False)
            # plt.pause(1./FPS)  # use second one
            # plt.pause(0.03)

            # FASTER
            cv2.imshow('Frame', depth_image)
            # Press Q on keyboard to  exit
            if cv2.waitKey(25) & 0xFF == ord('q'):
                break
        else:
            print("frame number:", frame_number)
            sleep(0.01)

except:
    depth_stream.stop()
    openni2.unload()
    cv2.destroyAllWindows()