diff --git a/sandbox/threedpointfinder.py b/sandbox/threedpointfinder.py new file mode 100644 index 0000000..5264b2d --- /dev/null +++ b/sandbox/threedpointfinder.py @@ -0,0 +1,113 @@ +import sys +import struct +from typing import Dict, Any +import numpy as np +import cwipc +try: + import cwipc.realsense2 + _ = cwipc.realsense2.cwipc_realsense2_dll_load() +except: + pass +try: + import cwipc.kinect + _ = cwipc.kinect.cwipc_kinect_dll_load() +except: + pass + +def _parse_aux_description(description : str) -> Dict[str, Any]: + rv = {} + fields = description.split(',') + for f in fields: + k, v = f.split('=') + try: + v = int(v) + except ValueError: + pass + rv[k] = v + return rv + +# Check that we can convert image (x, y) positions to 3D (x, y, z) positions +def main(): + configfile = sys.argv[1] + x = int(sys.argv[2]) + y = int(sys.argv[3]) + grabber = cwipc.cwipc_capturer(configfile) + grabber.request_auxiliary_data("rgb") + grabber.request_auxiliary_data("depth") + while True: + while not grabber.available(True): + print("Waiting for grab") + pc = grabber.get() + if not pc: + print("Skip empty pc") + continue + auxdata = pc.access_auxiliary_data() + assert auxdata + if auxdata.count() > 0: + break + rgb_images = {} + depth_images = {} + print(f"auxdata has {auxdata.count()} items") + for i in range(auxdata.count()): + # print(f"auxdata {i}: name={auxdata.name(i)}, description={auxdata.description(i)}") + name = auxdata.name(i) + if name.startswith("rgb."): + serial = name[4:] + descrstr = auxdata.description(i) + descr = _parse_aux_description(descrstr) + width = descr["width"] + height = descr["height"] + stride = descr["stride"] + bpp = descr["bpp"] + image_data = auxdata.data(i) + np_image_data_bytes = np.array(image_data) + np_image_data = np.reshape(np_image_data_bytes, (height, width, bpp)) + # Select B, G, R channels + # np_image_data = np_image_data[:,:,[2,1,0]] + rgb_images[serial] = np_image_data + elif name.startswith("depth."): + serial = name[6:] + descrstr = auxdata.description(i) + descr = _parse_aux_description(descrstr) + width = descr["width"] + height = descr["height"] + stride = descr["stride"] + bpp = descr["bpp"] + image_data = auxdata.data(i) + np_image_data_bytes = np.array(image_data) + np_image_data = np.reshape(np_image_data_bytes, (height, width, bpp)) + depth_images[serial] = np_image_data + serials = rgb_images.keys() + serial_to_tilenum = {} + for t in range(grabber.maxtile()): + tileinfo = grabber.get_tileinfo_dict(t) + serial = tileinfo["cameraName"] + if serial: + serial = serial.decode('utf8') + print(f"serial {serial}: tileMask {tileinfo['cameraMask']}") + serial_to_tilenum[serial] = tileinfo["cameraMask"] + for serial in serials: + rgb_image = rgb_images[serial] + depth_image = depth_images[serial] + rgb = rgb_image[y, x] + depth_bytes = depth_image[y, x] + depth = float(depth_bytes[0] + depth_bytes[1]*256) + print(f"serial {serial}: point={(x, y)}, distance={depth}, color={rgb}") + # Pack the arguments + arg_tilenum = float(serial_to_tilenum[serial]) + arg_x = float(x) + arg_y = float(y) + arg_depth = float(depth) + inargs = struct.pack("ffff", arg_tilenum, arg_x, arg_y, arg_depth) + outargs = bytearray(12) + ok = grabber.auxiliary_operation("map2d3d", inargs, outargs) + if not ok: + print(f"serial {serial}: map2d3d failed") + continue + _, rv_x, rv_y, rv_z = struct.unpack("ffff", outargs) + print(f"serial {serial}: x={rv_x}, y={rv_y}, z={rv_z}") + + + +if __name__ == '__main__': + main()