CaptureAndConvertToOpen3DType

Capture the point cloud and convert it to Open3D format.

# Copyright (c) RVBUST, Inc - All rights reserved.
import open3d as o3d
import PyRVC as RVC
import os
import numpy as np
import cv2
from Utils.Tools import *


def App():

    # Initialize RVC X system.
    RVC.SystemInit()

    # Choose RVC X Camera type (USB, GigE or All)
    opt = RVC.SystemListDeviceTypeEnum.All

    # Scan all RVC X USB Camera devices.
    ret, devices = RVC.SystemListDevices(opt)
    print("RVC X USB Camera devices number:%d" % len(devices))

    # Find whether any RVC X Camera is connected or not.
    if len(devices) == 0:
        print("Can not find any RVC X USB Camera!")
        RVC.SystemShutdown()
        return 1
    print("devices size = %d" % len(devices))

    # Create a RVC X Camera and choose use left side camera.
    x = RVC.X1.Create(devices[0], RVC.CameraID_Left)

    # Test RVC X Camera is valid or not.
    if x.IsValid() == True:
        print("RVC X Camera is valid!")
    else:
        print("RVC X Camera is not valid!")
        RVC.X1.Destroy(x)
        RVC.SystemShutdown()
        return 1
   # PrintCaptureMode(devices[0])

    # Open RVC X Camera.
    ret1 = x.Open()

    # Test RVC X Camera is opened or not.
    if ret1 and x.IsOpen() == True:
        print("RVC X Camera is opened!")
    else:
        print("RVC X Camera is not opened!")
        RVC.X1.Destroy(x)
        RVC.SystemShutdown()
        return 1

    # Print ExposureTime Range
    _, exp_range_min, exp_range_max = x.GetExposureTimeRange()
    print("ExposureTime Range:[{}, {}]".format(exp_range_min, exp_range_max))

    # Capture a point map and a image.
    ret2 = x.Capture()

    if ret2 == True:
        print("RVC X Camera capture successed!")

        pm = x.GetPointMap()
        n_pts = pm.GetSize().rows * pm.GetSize().cols
        pm = np.array(pm, copy=False).reshape(-1, 3)
        pcd_o3d = o3d.geometry.PointCloud(o3d.utility.Vector3dVector(pm.reshape((-1, 3))))
        step = n_pts // 10000

        for i in range(0,n_pts,step):
            rvc_point = pm[i]
            o3d_point = pcd_o3d.points[i]
            print(f"index: {i} RVC::PointMap:{rvc_point} , open3d::PointCloud:{o3d_point}")

    else:
        print("RVC X Camera capture failed!")
        print(RVC.GetLastErrorMessage())
        x.Close()
        RVC.X1.Destroy(x)
        RVC.SystemShutdown()
        return 1

    # Close RVC X Camera.
    x.Close()

    # Destroy RVC X Camera.
    RVC.X1.Destroy(x)

    # Shutdown RVC X System.
    RVC.SystemShutdown()

    return 0


if __name__ == "__main__":
    App()