Initial commit

.gitignore

+*.jpg
+*.png
+*.zip
+*.gz
+*.asc
+*.whl
+
+
+
+
+sandbox_conf_orig.yaml
+
+.idea/**
+.venv/**
+test_data/**
+plot_depth.py
+**/__pycache__/**

demo.py

+# from ar_sandbox import Sandbox
+# from ar_sandbox.wrapper import sandbox_wrapper as sw
+import sys
+import cv2
+import numpy as np
+import numpy.ma as npma
+from esrimap.esrimap import EsriAscii
+from PIL import Image
+import requests
+import time
+import math
+
+
+NORMALIZATION_BETWEEN = (0.8, 1.2)
+LO_NORM = NORMALIZATION_BETWEEN[0]
+HI_NORM = NORMALIZATION_BETWEEN[1]
+
+SHOW_LINES = True
+USE_HARD_COLORS = False
+LINE_COLORS = [255, 255, 255]
+
+
+COLORS_FULL_SPECTRUM_SOFT = [
+    (0.2, (0, 0, 255)),
+    (.25, (0, 255, 255)),
+    (.3, (0, 255, 0)),
+    (.35, (255, 255, 0)),
+    (.45, (255, 128, 0)),
+    (.5, (255, 0, 0)),
+    (.75, (255, 0, 128)),
+    (.9, (255, 0, 255)),
+    (1, (255, 255, 255))
+]
+
+COLORS_FULL_SPECTRUM_HARD = [
+    (0.2, (0, 0, 255)),
+    (0.24999999, (0, 0, 255)),
+    (.25, (0, 255, 255)),
+    (.29999999, (0, 255, 255)),
+    (.3, (0, 255, 0)),
+    (.34999999, (0, 255, 0)),
+    (.35, (255, 255, 0)),
+    (.44999999, (255, 255, 0)),
+    (.45, (255, 128, 0)),
+    (.49999999, (255, 128, 0)),
+    (.5, (255, 0, 0)),
+    (.74999999, (255, 0, 0)),
+    (.75, (255, 0, 128)),
+    (.89999999, (255, 0, 128)),
+    (.9, (255, 0, 255)),
+    (.9999999, (255, 0, 255)),
+    (1, (255, 255, 255)),
+]
+COLORS = COLORS_FULL_SPECTRUM_SOFT
+#
+POINTS = np.array([c[0] for c in COLORS_FULL_SPECTRUM_SOFT])
+COLOR_R = [c[1][0] for c in COLORS_FULL_SPECTRUM_SOFT]
+COLOR_G = [c[1][1] for c in COLORS_FULL_SPECTRUM_SOFT]
+COLOR_B = [c[1][2] for c in COLORS_FULL_SPECTRUM_SOFT]
+#
+POINTS_HARD = np.array([c[0] for c in COLORS_FULL_SPECTRUM_HARD])
+COLOR_R_HARD = [c[1][0] for c in COLORS_FULL_SPECTRUM_HARD]
+COLOR_G_HARD = [c[1][1] for c in COLORS_FULL_SPECTRUM_HARD]
+COLOR_B_HARD = [c[1][2] for c in COLORS_FULL_SPECTRUM_HARD]
+
+COLORMAP = None
+COLORMAP_HARD = None
+
+DEFAULT_URL = "http://localhost:8000"
+
+
+def try_and_ignore(ignore=Exception, default_value=0.0):
+    """
+    Silently ignore specific exception from a decorated function
+    Return a default value if exception occur.
+    """
+    def decorate(func):
+        def _decorated(*args, **kwargs):
+            try:
+                return func(*args, **kwargs)
+            except ignore:
+                return default_value
+        return _decorated
+    return decorate
+
+
+parse_int = try_and_ignore(ValueError, 0)(int)
+parse_float = try_and_ignore(ValueError, 0.0)(float)
+
+
+class FakeSandbox:
+    """Placeholder for testing purposes"""
+    def __init__(self, refresh=250):
+        self.on_frame = None
+        self.refresh = refresh
+
+    def init(self):
+        pass
+
+    def start(self, *args):
+        cap = cv2.VideoCapture(0)
+        while cap.isOpened():
+            # im = Image.open("test_aru.png")
+            ret, fr = cap.read()
+            frame = cv2.resize(fr, (588, 432))
+            mat = np.loadtxt("depth")
+
+            # mat = cv2.cvtColor(fr, cv2.COLOR_BGR2GRAY)
+            mat2 = frame
+            if self.on_frame is not None:
+                frame = self.on_frame(mat, mat2)
+
+            cv2.imshow("frame", frame)
+
+            # time.sleep(self.refresh // 1000)
+            k = cv2.waitKey(self.refresh)
+            if k == 27:
+                break
+        cv2.destroyAllWindows()
+        cap.release()
+
+
+def get_color(depth, hard_colors=False) -> np.ndarray:
+    """
+    Given a depth matrix between 0-1, return a color from the colormap
+
+    Arguments:
+        depth : Normalized 1 channel numpy matrix
+        hard_colors: Whether to use hard colormap or not
+    Return:
+        A 3 channel BGR matrix of the same size as depth_matrix.
+    """
+    depth_matrix = depth.copy()
+    # if hard_colors:
+    #     pts = POINTS_HARD
+    #     r = COLOR_R_HARD
+    #     g = COLOR_G_HARD
+    #     b = COLOR_B_HARD
+    # else:
+    #     pts = POINTS
+    #     r = COLOR_R
+    #     g = COLOR_G
+    #     b = COLOR_B
+    if hard_colors:
+        pts = COLORMAP_HARD[0]
+        r = COLORMAP_HARD[1]
+        g = COLORMAP_HARD[2]
+        b = COLORMAP_HARD[3]
+    else:
+        pts = COLORMAP[0]
+        r = COLORMAP[1]
+        g = COLORMAP[2]
+        b = COLORMAP[3]
+
+    r_val = np.interp(depth_matrix, pts, r)
+    # print(r_val)
+    g_val = np.interp(depth_matrix, pts, g)
+    # print(g_val)
+    b_val = np.interp(depth_matrix, pts, b)
+    # print(b_val)
+    res = np.dstack((b_val, g_val, r_val)).astype(np.uint8)
+    return res
+
+
+def display_level(frame):
+    out_frame = frame.copy()
+    # out_frame = normalize_array_between(out_frame, -4, 1)
+    # out_frame = np.clip(out_frame, 0, 1)
+    print(out_frame, out_frame.min(), out_frame.max())
+    out_frame = get_color(out_frame)
+    return out_frame
+
+
+def send_request(body, url="http://localhost:8000/map"):
+    try:
+        requests.post(url, json=body)
+    except Exception as e:
+        print(e)
+
+
+def build_req_body(map, tag_list):
+    arucos = []
+    for t in tag_list:
+        print(t)
+        (x, y), id, r = t
+        (x, y) = (parse_int(x), parse_int(y))
+        id = parse_int(id)
+        r = parse_float(r)
+        arucos.append({
+            "tag_id": id,
+            "position": {
+                "x": x,
+                "y": y
+            },
+            "rotation": r
+        })
+    body = {
+        "esri": map.to_ascii(),
+        "arucos": arucos
+    }
+    return body
+
+
+def normalize_array(array):
+    return (array - array.min())/(array.max() - array.min())
+
+
+def fix_norm(array, min, max):
+    return (array - min) / (max - min)
+
+
+def normalize_array_between(array, lower_bound, higher_bound):
+    norm = normalize_array(array)
+    diff = higher_bound - lower_bound
+    return (norm * diff) + lower_bound
+
+
+def make_esri_and_send(depth, color, *args):
+    if COLORMAP is None or COLORMAP_HARD is None:
+        gen_global_default_cmaps()
+    # Normalization step
+    depth = np.clip(depth, LO_NORM, HI_NORM)  # Clip aberrants values
+    depth = np.interp(depth, [LO_NORM, HI_NORM], [0, 1])  # Normalize array
+    depth = 1 - depth  # Invert result, making 1 the highest, 0 the lowest.
+
+    # Create level frame, based on depth values
+    if USE_HARD_COLORS:
+        levels = get_color(depth, hard_colors=True)
+    else:
+        levels = get_color(depth, hard_colors=False)
+
+    if SHOW_LINES:  # Enable lines between region
+        if USE_HARD_COLORS:
+            lvl_image = levels
+        else:
+            lvl_image = get_color(depth, hard_colors=True)
+        canny = cv2.Canny(lvl_image, 100, 200)
+        can = normalize_array(canny)
+        mask = npma.masked_where(can == 1, can)
+        levels[mask.mask] = LINE_COLORS
+
+    # Aruco detection
+    grayscale_frame = cv2.cvtColor(color, cv2.COLOR_BGR2GRAY)
+    _, (corn, ids, rej), centers, rots = detect_aruco(grayscale_frame)
+
+    # Create map and tag dictionary
+    esri_map = EsriAscii.from_ndarray(depth)
+    detected = build_tag_list(ids, centers, rots)
+
+    # Send it toward api
+    send_request(build_req_body(esri_map, detected), url=DEFAULT_URL)
+    # May locally save the esri_map
+    # esri_map.to_file("depth_map_v2.asc")
+    # Return value will be displayed on the beamer
+    return levels
+
+
+def build_tag_list(tags_ids, tags_centers, tags_rotation):
+    detected = []
+    for i, c in enumerate(tags_centers):
+        detected.append((c, tags_ids[i][0], tags_rotation[i]))
+    return detected
+
+
+def get_center(corner):
+    c0, _, c2, _ = corner[0]
+    return int(c0[0] + (c2[0]-c0[0]) // 2), int(c0[1] + (c2[1]-c0[1]) // 2)
+
+
+def detect_aruco(frame,
+                 aruco_dict=cv2.aruco.Dictionary_get(cv2.aruco.DICT_4X4_50),
+                 aruco_params=cv2.aruco.DetectorParameters_create()):
+    output = frame.copy()
+    (corners, ids, rejected) = cv2.aruco.detectMarkers(frame, aruco_dict, parameters=aruco_params)
+    centers = []
+    rotations = []
+    for c in range(len(corners)):
+        try:
+            ctr = get_center(corners[c])
+            top_left = corners[c][0][0]
+            centers.append(get_center(corners[c]))
+            # print("Corner is", corners[c], "Center is", get_center(corners[c]))
+            (w1, w2) = [-1, -1]
+            (v1, v2) = top_left - ctr
+            rotation_in_degree = math.degrees(math.atan2(w2 * v1 - w1 * v2, w1 * v1 + w2 * v2))
+            rotations.append(round(rotation_in_degree, 2))
+        except Exception as e:
+            print(e)
+    return output, (corners, ids, rejected), centers, rotations
+
+
+def save_matrices(depth, frame, *args):
+    np.savetxt("depth", depth)
+    for i in range(frame.shape[2]):
+        np.savetxt("frame" + "RGB"[i], frame[:, :, i].astype(np.uint8))
+    Image.fromarray(frame).save("frame.png")
+    return frame
+
+
+def create_hard_colors(color_array):
+    hard_colors = []
+    epsilon = 1e-8
+    for i, c in enumerate(color_array[:-1]):
+        hard_colors.append(c)
+        col = ((color_array[i+1][0] - epsilon), c[1])
+        hard_colors.append(col)
+    hard_colors.append(color_array[-1])
+    return hard_colors
+
+
+def create_colormap(color_array):
+    pts = np.array([c[0] for c in color_array])
+    r_chan = [c[1][0] for c in color_array]
+    g_chan = [c[1][1] for c in color_array]
+    b_chan = [c[1][2] for c in color_array]
+    return pts, r_chan, g_chan, b_chan
+
+
+def gen_global_default_cmaps():
+    global COLORMAP, COLORMAP_HARD
+    print("Generating default colormaps")
+    COLORMAP = create_colormap(color_array=COLORS_FULL_SPECTRUM_SOFT)
+    COLORMAP_HARD = create_colormap(color_array=create_hard_colors(color_array=COLORS_FULL_SPECTRUM_SOFT))
+
+
+def hex_to_rgb(value):
+    value = value.lstrip('#')
+    lv = len(value)
+    return tuple(int(value[i:i + lv // 3], 16) for i in range(0, lv, lv // 3))
+
+
+def rgb_to_hex(rgb):
+    r, g, b = [int(i) for i in rgb.split(",")]
+    rgb = (r, g, b)
+    return '#%02x%02x%02x' % rgb
+
+
+if __name__ == "__main__":
+    import argparse
+    parser = argparse.ArgumentParser()
+    parser.add_argument("-c", "--colors", default="")
+    parser.add_argument("-u", "--url", default=DEFAULT_URL)
+    parser.add_argument("-l", "--lines", action="store_true")
+    parser.add_argument("-H", "--hard", action="store_true")
+    arguments = parser.parse_args()
+
+    print(arguments)
+    DEFAULT_URL = arguments.url if arguments.url.startswith("http://") else "http://" + arguments.url
+    arg_colors = arguments.colors
+    if arg_colors != "":
+        colours = []
+        cols = arg_colors.split(",")
+        for c in cols:
+            colours.append(hex_to_rgb(c))
+        print(colours)
+        colpoints = np.arange(0, 1, 1/(len(colours) - 1))
+        colpoints = np.append(colpoints, [1.0])
+        print(colpoints)
+        cmap = list(zip(colpoints, colours))
+        COLORMAP = create_colormap(cmap)
+        COLORMAP_HARD = create_colormap(create_hard_colors(cmap))
+
+    SHOW_LINES = False
+    if arguments.lines:
+        SHOW_LINES = True
+
+    USE_HARD_COLORS = False
+    if arguments.hard:
+        USE_HARD_COLORS = True
+
+    # colours = [(.2, (0,0,128)), (.25, (68, 87, 227)), (.3, (236, 255, 89)),(.4, (67, 138, 65)),(.6, (128,128,128)), (.8, (255, 255, 255))]
+    # colours = [(.2, (128,0,0)),(.3, (255,0,0)),(.4, (255,255,255)), (.5, (0,0,255)), (.6,(0, 0, 128))]
+    # COLORMAP = create_colormap(color_array=colours)
+    # COLORMAP_HARD = create_colormap(create_hard_colors(colours))
+
+    # USE_HARD_COLORS=True
+
+    LINE_COLORS = [0,0,0]
+    box = FakeSandbox(refresh=1000)
+    box.init()
+    box.on_frame = make_esri_and_send
+    box.start(box)

env.sh

+export LD_LIBRARY_PATH=/home/alexis/sandbox/sandbox_docker-builder/build/lib/

esrimap/esrimap.py

+import numpy as np
+from enum import Enum
+
+
+class EsriCoordinateType(Enum):
+    center = 0
+    corner = 1
+
+
+class EsriAscii:
+    def __init__(self,
+                 rows,
+                 cols,
+                 data_array = None,
+                 cellsize = 1,
+                 nodata_value = -9999,
+                 coord_type = EsriCoordinateType.corner):
+        if cellsize < 0:
+            raise ValueError("cellsize must be greater than 0")
+        if type(rows) is not int:
+            raise TypeError("rows must be integer")
+        if rows < 0:
+            raise ValueError("rows must be greater than 0")
+        if type(cols) is not int:
+            raise TypeError("cols must be integer")
+        if cols < 0:
+            raise ValueError("cols must be greater than 0")
+        if data_array is not None:
+            assert((rows, cols) == data_array.shape[0:2])
+        self.rows = rows
+        self.type = coord_type
+        self.cols = cols
+        self.xll = 0
+        self.yll = 0
+        self.cellsize = cellsize
+        self.nodata_value = nodata_value
+        self.data = data_array if data_array is not None else np.empty((rows, cols))
+
+    def __str__(self):
+        return self.to_ascii()
+
+    @property
+    def header(self):
+        return self.get_header()
+
+    @property
+    def sdata(self):
+        return self.data_as_string()
+
+    @classmethod
+    def from_ndarray(cls,
+                     arr,
+                     cellsize = 1,
+                     nodata_value = -9999,
+                     coord_type = EsriCoordinateType.corner):
+        """
+        Create an Esri raster map from a numpy array
+
+        :param arr: The numpy array
+        :param cellsize: Optional Cell Size
+        :param nodata_value: Value for no data
+        :param coord_type: Should the metadata use corner or center for xll and yll
+        :return: The Esri raster map
+        """
+        if len(arr.shape) == 4:
+            arr = arr[..., 0:2]
+        if len(arr.shape) == 3:
+            arr = np.dot(arr[..., :3], [0.299, 0.587, 0.114])
+        ro, co = arr.shape[0:2]
+        return cls(ro, co, arr, cellsize=cellsize, nodata_value=nodata_value, coord_type=coord_type)
+
+    def data_as_string(self):
+        """Return the data as a string
+
+        :return: A string representing the data
+        """
+        # return str(self.data)
+        ro, co = self.data.shape[0:2]
+        out = []
+        for r in range(ro):
+            # out.append(str(self.data[r]).replace(".", "")[1:-1])
+            out.append(' '.join(map(str, self.data[r])))
+        return "\n".join(out)
+
+    def get_header(self):
+        """Get the Ascii raster header
+
+        :return: The header, as a string
+        """
+        header = ""
+        header += "ncols " + str(self.cols) + "\n"
+        header += "nrows " + str(self.rows) + "\n"
+        header += "xllcenter " + str(self.xll) + "\n"
+        header += "yllcenter " +str(self.yll) + "\n"
+        header += "cellsize " +str(self.cellsize) + "\n"
+        header += "nodata_value " + str(self.nodata_value) + "\n"
+        if self.type == EsriCoordinateType.corner:
+            header = header.replace("xllcenter", "xllcorner").replace("yllcenter", "yllcorner")
+        return header
+
+    @classmethod
+    def from_ascii(cls, ascii_text):
+        """Create an EsriAscii object from an ascii text. Data length must match rows and columns
+        (length of data = rows * columns)
+
+        :param ascii_text: The text (Should be an Esri Ascii raster formatted text)
+        :return: an EsriAscii map
+        """
+        lines = ascii_text.splitlines()
+        assert len(lines) > 6
+        _, s_ncols = lines[0].split(" ")
+        ncols = int(s_ncols)
+        _, s_nrows = lines[1].split(" ")
+        nrows = int(s_nrows)
+        alignment, s_xll = lines[2].split(" ")
+        xll = int(s_xll)
+        if alignment[3:] == "center":
+            coord = EsriCoordinateType.center
+        else:
+            coord = EsriCoordinateType.corner
+        _, s_yll = lines[3].split(" ")
+        yll = int(s_yll)
+        _, s_cellsize = lines[4].split(" ")
+        cellsize = int(s_cellsize)
+        _, s_nodata = lines[5].split(" ")
+        nodata = int(s_nodata)
+        s_data = " ".join(lines[6:])
+        data = np.array(s_data.split(" "), dtype=np.int32).reshape((nrows, ncols))
+        ascii_map = cls(rows=nrows,
+                        cols=ncols,
+                        cellsize=cellsize,
+                        data_array=data,
+                        nodata_value=nodata,
+                        coord_type=coord,
+                        )
+        ascii_map.xll = int(xll)
+        ascii_map.yll = int(yll)
+        return ascii_map
+
+    def to_ascii(self):
+        """Return the map as text
+
+        :return: The Esri raster as a text
+        """
+        return self.header + self.sdata
+
+    @classmethod
+    def from_file(cls, path):
+        with open(path) as fp:
+            content = fp.read()
+        return cls.from_ascii(content)
+
+    def to_file(self, path):
+        """Write the ascii raster to a file.
+
+        :param path: The file to output
+        :return: nothing
+        """
+        with open(path, 'w') as fp:
+            fp.write(self.to_ascii())
+
+    def to_array(self):
+        """Get the data as a numpy array
+
+        :return: The map data
+        """
+        return self.data

readme.md

+# How to run the thing 
+
+Open a terminal here, then
+
+```bash
+source env.sh
+source .venv/bin/activate
+python test.py
+```
+
+Everything should run fine. Remember to duplicate the screen to the beamer, with a resolution of 1024x768
+

sandbox_conf.yaml

+AdjustingMatrix:
+  angle: 1.4119340386036046
+  width: 3
+  height: 2
+  matrix: [0.999696374, 0.0246404037, 0, -0.0246404037, 0.999696374, 0]
+DistanceTopSandbox:
+  distance: 1
+CroppingMask:
+  x: 52
+  y: 19
+  width: 588 # 568
+  height: 432
+BeamerResolution:
+  width: 1400
+  height: 1050
+BeamerPosition:
+  x: 0.0536931753
+  y: 0.260815978
+  z: -0.325273067
+FrameProcessProfil:
+  contrast: 1.0900000000000001
+  brightness: 14
+  minDistance: 15
+  cannyEdgeThreshold: 184
+  houghAccThreshold: 35
+  minRadius: 0
+  maxRadius: 0