commit depthsensing

depthsensing.py

@@ -0,0 +1,529 @@
+#!python3
+"""
+Python 3 wrapper for identifying objects in images
+
+Requires DLL compilation
+
+Original *nix 2.7: https://github.com/pjreddie/darknet/blob/0f110834f4e18b30d5f101bf8f1724c34b7b83db/python/darknet.py
+Windows Python 2.7 version: https://github.com/AlexeyAB/darknet/blob/fc496d52bf22a0bb257300d3c79be9cd80e722cb/build/darknet/x64/darknet.py
+
+@author: Philip Kahn, Aymeric Dujardin
+@date: 20180911
+"""
+# pylint: disable=R, W0401, W0614, W0703
+import cv2
+import pyzed.sl as sl
+from ctypes import *
+import math
+import random
+import os
+import numpy as np
+import statistics
+import sys
+import getopt
+from random import randint
+import time
+import pandas as pd
+from pandas import  DataFrame as df
+import  subprocess
+
+
+#NOTE at 2020-12-22: if svo's resulotion changed, then you have to edit WIDTH, HEIGHT and DEPTH_MAX
+WIDTH = 1920
+HEIGHT = 1080
+DEPTH_MAX = 8
+
+
+def sample(probs):
+    s = sum(probs)
+    probs = [a/s for a in probs]
+    r = random.uniform(0, 1)
+    for i in range(len(probs)):
+        r = r - probs[i]
+        if r <= 0:
+            return i
+    return len(probs)-1
+
+
+def c_array(ctype, values):
+    arr = (ctype*len(values))()
+    arr[:] = values
+    return arr
+
+
+class BOX(Structure):
+    _fields_ = [("x", c_float),
+                ("y", c_float),
+                ("w", c_float),
+                ("h", c_float)]
+
+
+class DETECTION(Structure):
+    _fields_ = [("bbox", BOX),
+                ("classes", c_int),
+                ("prob", POINTER(c_float)),
+                ("mask", POINTER(c_float)),
+                ("objectness", c_float),
+                ("sort_class", c_int)]
+
+
+class IMAGE(Structure):
+    _fields_ = [("w", c_int),
+                ("h", c_int),
+                ("c", c_int),
+                ("data", POINTER(c_float))]
+
+
+class METADATA(Structure):
+    _fields_ = [("classes", c_int),
+                ("names", POINTER(c_char_p))]
+
+
+#lib = CDLL("/home/pjreddie/documents/darknet/libdarknet.so", RTLD_GLOBAL)
+#lib = CDLL("darknet.so", RTLD_GLOBAL)
+hasGPU = True
+if os.name == "nt":
+    cwd = os.path.dirname(__file__)
+    os.environ['PATH'] = cwd + ';' + os.environ['PATH']
+    winGPUdll = os.path.join(cwd, "yolo_cpp_dll.dll")
+    #print(winGPUdll)
+    winNoGPUdll = os.path.join(cwd, "yolo_cpp_dll_nogpu.dll")
+    envKeys = list()
+    for k, v in os.environ.items():
+        envKeys.append(k)
+    try:
+        try:
+            tmp = os.environ["FORCE_CPU"].lower()
+            if tmp in ["1", "true", "yes", "on"]:
+                raise ValueError("ForceCPU")
+            else:
+                pass#print("Flag value '"+tmp+"' not forcing CPU mode")
+        except KeyError:
+            # We never set the flag
+            if 'CUDA_VISIBLE_DEVICES' in envKeys:
+                if int(os.environ['CUDA_VISIBLE_DEVICES']) < 0:
+                    raise ValueError("ForceCPU")
+            try:
+                global DARKNET_FORCE_CPU
+                if DARKNET_FORCE_CPU:
+                    raise ValueError("ForceCPU")
+            except NameError:
+                pass
+            # #print(os.environ.keys())
+            # #print("FORCE_CPU flag undefined, proceeding with GPU")
+        if not os.path.exists(winGPUdll):
+            raise ValueError("NoDLL")
+        lib = CDLL(winGPUdll, RTLD_GLOBAL)
+    except (KeyError, ValueError):
+        hasGPU = False
+        if os.path.exists(winNoGPUdll):
+            lib = CDLL(winNoGPUdll, RTLD_GLOBAL)
+            #print("Notice: CPU-only mode")
+        else:
+            # Try the other way, in case no_gpu was
+            # compile but not renamed
+            lib = CDLL(winGPUdll, RTLD_GLOBAL)
+            #print("Environment variables indicated a CPU run, but we didn't find `" +
+            #      winNoGPUdll+"`. Trying a GPU run anyway.")
+else:
+    lib = CDLL("/root/darknet/libdarknet.so", RTLD_GLOBAL)
+lib.network_width.argtypes = [c_void_p]
+lib.network_width.restype = c_int
+lib.network_height.argtypes = [c_void_p]
+lib.network_height.restype = c_int
+
+predict = lib.network_predict
+predict.argtypes = [c_void_p, POINTER(c_float)]
+predict.restype = POINTER(c_float)
+
+if hasGPU:
+    set_gpu = lib.cuda_set_device
+    set_gpu.argtypes = [c_int]
+
+make_image = lib.make_image
+make_image.argtypes = [c_int, c_int, c_int]
+make_image.restype = IMAGE
+
+get_network_boxes = lib.get_network_boxes
+get_network_boxes.argtypes = [c_void_p, c_int, c_int, c_float, c_float, POINTER(
+    c_int), c_int, POINTER(c_int), c_int]
+get_network_boxes.restype = POINTER(DETECTION)
+
+make_network_boxes = lib.make_network_boxes
+make_network_boxes.argtypes = [c_void_p]
+make_network_boxes.restype = POINTER(DETECTION)
+
+free_detections = lib.free_detections
+free_detections.argtypes = [POINTER(DETECTION), c_int]
+
+free_ptrs = lib.free_ptrs
+free_ptrs.argtypes = [POINTER(c_void_p), c_int]
+
+network_predict = lib.network_predict
+network_predict.argtypes = [c_void_p, POINTER(c_float)]
+
+reset_rnn = lib.reset_rnn
+reset_rnn.argtypes = [c_void_p]
+
+load_net = lib.load_network
+load_net.argtypes = [c_char_p, c_char_p, c_int]
+load_net.restype = c_void_p
+
+load_net_custom = lib.load_network_custom
+load_net_custom.argtypes = [c_char_p, c_char_p, c_int, c_int]
+load_net_custom.restype = c_void_p
+
+do_nms_obj = lib.do_nms_obj
+do_nms_obj.argtypes = [POINTER(DETECTION), c_int, c_int, c_float]
+
+do_nms_sort = lib.do_nms_sort
+do_nms_sort.argtypes = [POINTER(DETECTION), c_int, c_int, c_float]
+
+free_image = lib.free_image
+free_image.argtypes = [IMAGE]
+
+letterbox_image = lib.letterbox_image
+letterbox_image.argtypes = [IMAGE, c_int, c_int]
+letterbox_image.restype = IMAGE
+
+load_meta = lib.get_metadata
+lib.get_metadata.argtypes = [c_char_p]
+lib.get_metadata.restype = METADATA
+
+load_image = lib.load_image_color
+load_image.argtypes = [c_char_p, c_int, c_int]
+load_image.restype = IMAGE
+
+rgbgr_image = lib.rgbgr_image
+rgbgr_image.argtypes = [IMAGE]
+
+predict_image = lib.network_predict_image
+predict_image.argtypes = [c_void_p, IMAGE]
+predict_image.restype = POINTER(c_float)
+
+
+def array_to_image(arr):
+    import numpy as np
+    # need to return old values to avoid python freeing memory
+    arr = arr.transpose(2, 0, 1)
+    c = arr.shape[0]
+    h = arr.shape[1]
+    w = arr.shape[2]
+    arr = np.ascontiguousarray(arr.flat, dtype=np.float32) / 255.0
+    data = arr.ctypes.data_as(POINTER(c_float))
+    im = IMAGE(w, h, c, data)
+    return im, arr
+
+
+def classify(net, meta, im):
+    out = predict_image(net, im)
+    res = []
+    for i in range(meta.classes):
+        if altNames is None:
+            nameTag = meta.names[i]
+        else:
+            nameTag = altNames[i]
+        res.append((nameTag, out[i]))
+    res = sorted(res, key=lambda x: -x[1])
+    return res
+
+
+def detect(net, meta, image, thresh=.5, hier_thresh=.5, nms=.45, debug=False):
+    """
+    Performs the detection
+    """
+    custom_image = cv2.cvtColor(image, cv2.COLOR_BGR2RGB)
+    custom_image = cv2.resize(custom_image, (lib.network_width(
+        net), lib.network_height(net)), interpolation=cv2.INTER_LINEAR)
+    im, arr = array_to_image(custom_image)
+    num = c_int(0)
+    pnum = pointer(num)
+    predict_image(net, im)
+    dets = get_network_boxes(
+        net, image.shape[1], image.shape[0], thresh, hier_thresh, None, 0, pnum, 0)
+    num = pnum[0]
+    if nms:
+        do_nms_sort(dets, num, meta.classes, nms)
+    res = []
+    if debug:
+        pass
+        #print("about to range")
+    for j in range(num):
+        for i in range(meta.classes):
+            try:
+                if dets[j].prob[i] > 0:
+                    b = dets[j].bbox
+                    if altNames is None:
+                        nameTag = meta.names[i]
+                    else:
+                        nameTag = altNames[i]
+                    res.append((nameTag, dets[j].prob[i], (b.x, b.y, b.w, b.h), i))
+            except ValueError as e:
+                #print(e)
+                pass
+    res = sorted(res, key=lambda x: -x[1])
+    free_detections(dets, num)
+    return res
+
+
+netMain = None
+metaMain = None
+altNames = None
+
+
+def getObjectDepth(depth, bounds):
+    area_div = 2
+
+    x_vect = []
+    y_vect = []
+    z_vect = []
+
+    for j in range(int(bounds[0] - area_div), int(bounds[0] + area_div)):
+        for i in range(int(bounds[1] - area_div), int(bounds[1] + area_div)):
+            z = depth[i, j, 2]
+            if not np.isnan(z) and not np.isinf(z):
+                x_vect.append(depth[i, j, 0])
+                y_vect.append(depth[i, j, 1])
+                z_vect.append(z)
+    try:
+        x = statistics.median(x_vect)
+        y = statistics.median(y_vect)
+        z = statistics.median(z_vect)
+    except Exception:
+        x = -1
+        y = -1
+        z = -1
+        pass
+
+    return x, y, z
+
+
+def generateColor(metaPath):
+    random.seed(42)
+    f = open(metaPath, 'r')
+    content = f.readlines()
+    class_num = int(content[0].split("=")[1])
+    color_array = []
+    for x in range(0, class_num):
+        color_array.append((randint(0, 255), randint(0, 255), randint(0, 255)))
+    return color_array
+
+
+def main(argv):
+
+    thresh = 0.25
+    configPath = "/sources/cfg.cfg"
+    weightPath = "/sources/mailsys.weight"
+    metaPath = "/sources/data.data"
+    svoPath = None
+
+    help_str = 'darknet_zed.py -c <config> -w <weight> -m <meta> -t <threshold> -s <svo_file> -o <output_path>'
+    try:
+        opts, args = getopt.getopt(
+            argv, "hc:w:m:t:s:o:", ["config=", "weight=", "meta=", "threshold=", "svo_file=", "output_path="])
+    except getopt.GetoptError:
+        print (help_str)
+        sys.exit(2)
+    for opt, arg in opts:
+        if opt == '-h':
+            print (help_str)
+            sys.exit()
+        elif opt in ("-c", "--config"):
+            configPath = arg
+        elif opt in ("-w", "--weight"):
+            weightPath = arg
+        elif opt in ("-m", "--meta"):
+            metaPath = arg
+        elif opt in ("-t", "--threshold"):
+            thresh = float(arg)
+        elif opt in ("-s", "--svo_file"):
+            svoPath = arg
+        elif opt in ("-o", "--output_path"):
+            output_path = arg
+
+    init = sl.InitParameters()
+    init.coordinate_units = sl.UNIT.METER
+
+
+
+    if svoPath is not None:
+        init.set_from_svo_file(svoPath)
+
+    dirname = os.path.splitext(svoPath)[0]
+    dir, filename = os.path.split(dirname)
+    #print(output_path)
+    #print(filename)
+
+    #try:
+    #    if not (os.path.isdir(output_path)):
+    #        os.makedirs(os.path.join(output_path))
+    #except OSError as e:
+    #    #print(e)
+
+
+    cam = sl.Camera()
+    if not cam.is_opened():
+        #print("Opening ZED Camera...")
+        pass
+    status = cam.open(init)
+    if status != sl.ERROR_CODE.SUCCESS:
+        #print(repr(status))
+        exit()
+
+    runtime = sl.RuntimeParameters()
+    # Use STANDARD sensing mode
+    runtime.sensing_mode = sl.SENSING_MODE.STANDARD
+    mat = sl.Mat()
+    point_cloud_mat = sl.Mat()
+
+    # Import the global variables. This lets us instance Darknet once, then just call performDetect() again without instancing again
+    global metaMain, netMain, altNames  # pylint: disable=W0603
+    assert 0 < thresh < 1, "Threshold should be a float between zero and one (non-inclusive)"
+    if not os.path.exists(configPath):
+        raise ValueError("Invalid config path `" +
+                         os.path.abspath(configPath)+"`")
+    if not os.path.exists(weightPath):
+        raise ValueError("Invalid weight path `" +
+                         os.path.abspath(weightPath)+"`")
+    if not os.path.exists(metaPath):
+        raise ValueError("Invalid data file path `" +
+                         os.path.abspath(metaPath)+"`")
+    if netMain is None:
+        netMain = load_net_custom(configPath.encode(
+            "ascii"), weightPath.encode("ascii"), 0, 1)  # batch size = 1
+    if metaMain is None:
+        metaMain = load_meta(metaPath.encode("ascii"))
+    if altNames is None:
+        # In thon 3, the metafile default access craps out on Windows (but not Linux)
+        # Read the names file and create a list to feed to detect
+        try:
+            with open(metaPath) as metaFH:
+                metaContents = metaFH.read()
+                import re
+                match = re.search("names *= *(.*)$", metaContents,
+                                  re.IGNORECASE | re.MULTILINE)
+                if match:
+                    result = match.group(1)
+                else:
+                    result = None
+                try:
+                    if os.path.exists(result):
+                        with open(result) as namesFH:
+                            namesList = namesFH.read().strip().split("\n")
+                            altNames = [x.strip() for x in namesList]
+                except TypeError:
+                    pass
+        except Exception:
+            pass
+
+    color_array = generateColor(metaPath)
+
+    #print("Running...")
+    #df1 = df(data={'frame': [], 'label': [], 'x': [], 'y': [], 'depth': []})
+    df1 = df(data={'x': [], 'y': [], 'z': [], 'frame': []})
+    #print(df1)
+    start = time.time()
+    key = ''
+    count = 0
+    frame = 1
+
+    #image_size = cam.get_resolution()
+    #width = image_size.width
+    #height = image_size.height
+    #width_sbs = width * 2
+
+    # Prepare side by side image container equivalent to CV_8UC4
+    #svo_image_sbs_rgba = np.zeros((height, width_sbs, 4), dtype=np.uint8)
+    fourcc = cv2.VideoWriter_fourcc('M', '4', 'S', '2')
+
+    # 컬러 영상 저장시
+    ##print(cam.get_camera_fps(), "!!!!!!!!!!!")
+    #video_path = os.path.join(output_path, filename)
+    #writer = cv2.VideoWriter(str(video_path)+"_output.avi", fourcc, cam.get_camera_fps(), (width, height))
+    ##print(video_path)
+    ##print(writer)
+    while key != 113:  # for 'q' key
+        err = cam.grab(runtime)
+        if err == sl.ERROR_CODE.SUCCESS:
+            cam.retrieve_image(mat, sl.VIEW.LEFT)
+            image = mat.get_data()
+
+            cam.retrieve_measure(
+                point_cloud_mat, sl.MEASURE.XYZRGBA)
+            depth = point_cloud_mat.get_data()
+
+            # Do the detection
+            detections = detect(netMain, metaMain, image, thresh)
+
+            #print(chr(27) + "[2J"+"**** " +
+                #  str(len(detections)) + " Results ****")
+            frame += 1
+            for detection in detections:
+                label = detection[0]
+                confidence = detection[1]
+                pstring = label+": "+str(np.rint(100 * confidence))+"%"
+                #print(pstring)
+                bounds = detection[2]
+                yExtent = int(bounds[3])
+                xEntent = int(bounds[2])
+                # Coordinates are around the center
+                xCoord = int(bounds[0] - bounds[2]/2)
+                yCoord = int(bounds[1] - bounds[3]/2)
+                boundingBox = [[xCoord, yCoord], [xCoord, yCoord + yExtent], [xCoord + xEntent, yCoord + yExtent], [xCoord + xEntent, yCoord] ]
+                thickness = 1
+                x, y, z = getObjectDepth(depth, bounds)
+                distance_xyz = math.sqrt(x * x + y * y + z * z)
+
+                distance = "{:.4f}".format(distance_xyz)
+
+                #print(label, distance, xCoord, yCoord)
+                cv2.rectangle(image, (xCoord-thickness, yCoord-thickness), (xCoord + xEntent+thickness, yCoord+(18 +thickness*4)), color_array[detection[3]], -1)
+                cv2.putText(image, label + " " +  (str(distance) + " m"), (xCoord+(thickness*4), yCoord+(10 +thickness*4)), cv2.FONT_HERSHEY_SIMPLEX, 0.5, (255,255,255), 2)
+                cv2.rectangle(image, (xCoord-thickness, yCoord-thickness), (xCoord + xEntent+thickness, yCoord + yExtent+thickness), color_array[detection[3]], int(thickness*2))
+                x = xCoord-thickness
+                x1 = xCoord + xEntent+thickness
+                y = yCoord-thickness
+                y1 = yCoord + yExtent+thickness
+                
+
+                #If need normalize, erase # and change df1.loc[count]
+                #norm_x = (x + x1) / 2 / WIDTH
+                #norm_y = (y + y1) / 2 / HEIGHT
+                #norm_z = distance_xyz / DEPTH_MAX
+
+
+                
+                df1.loc[count] = [int(xCoord), int(yCoord), distance_xyz, int(frame)]
+                #df1.columns = ['idx','frame','label','x','y','depth']
+                cv2.line(image, (int((x+x1)/2), int((y+y1)/2)), (int((x+x1)/2), int((y+y1)/2)), (0, 0, 255), 10)
+
+                count += 1
+                image = cv2.cvtColor(image, cv2.COLOR_RGBA2RGB)
+            #print(frame)
+            image = cv2.cvtColor(image, cv2.COLOR_RGBA2RGB)
+            #writer.write(image)
+            # cv2.imshow("ZED", image)
+           # key = cv2.waitKey(5)
+            # #print(cam.get_svo_number_of_frames())
+            if frame == cam.get_svo_number_of_frames():
+                break
+        else:
+            #print('something happend')
+            #key = cv2.waitKey(5)
+            pass
+    #print("time :", time.time() - start)
+
+    cam.close()
+    #writer.release()
+    #cv2.destroyAllWindows()
+    #print("\nFINISH")
+    ##print(df1)
+    
+    df1.to_csv(output_path, index=False)
+    #print("csv save")
+
+
+if __name__ == "__main__":
+
+    #print(sys.argv)
+    main(sys.argv[1:])