utils.py 6.43 KB
Newer Older
1 2 3 4 5
from xml.dom import minidom
import logging
import numpy as np

SUCCESS = 0
Mohcine Chraibi's avatar
Mohcine Chraibi committed
6
FAILURE = -1
7

Mohcine Chraibi's avatar
Mohcine Chraibi committed
8

Mohcine Chraibi's avatar
Mohcine Chraibi committed
9 10 11 12 13 14 15 16 17 18 19 20 21 22 23 24 25 26 27 28 29 30 31 32 33 34 35 36 37 38 39 40 41 42 43 44 45 46 47 48 49 50 51 52 53 54 55 56 57 58 59 60 61 62 63
try:
    import xml.etree.cElementTree as ET
except ImportError:
    import xml.etree.ElementTree as ET


def get_polygon(poly):
    X = []
    Y = []
    # for poly in node.getchildren():
    #     # print "polygon tag: ", poly.tag
    #     # if poly.tag == "obstacle":
    #     #     # print poly.getchildren()
    #     #     # for pobst in poly.getchildren():
    #     #     #     # print pobst.tag
    #     #     #     for q in pobst.getchildren(): # vertex
    #     #     #         X.append( q.attrib['px'] )
    #     #     #         Y.append( q.attrib['py'] )
    #     #     pass
    #     # else:
    for p in poly.getchildren(): # vertex
        X.append(p.attrib['px'])
        Y.append(p.attrib['py'])

    return X, Y

def plot_geometry(filename):
    tree = ET.parse(geometry)
    root = tree.getroot()
    for node in root.iter():
        tag = node.tag
        # print "subroom tag", tag
        if tag == "polygon":
            X, Y = get_polygon(node)
            plt.plot(X, Y, "k", lw=2)
        elif tag == "obstacle":
            # print "obstacle tag",tag
            for n in node.getchildren():
                # print "N: ", n
                X, Y = get_polygon(n)
                # print "obstacle", X, Y
                plt.plot(X, Y, "g", lw=2)
        elif tag == "crossing":
            X, Y = get_polygon(node)
            plt.plot(X, Y, "--b", lw=0.9, alpha=0.2)
        elif tag == "HLine":
            X, Y = get_polygon(node)
            plt.plot(X, Y, "--b", lw=0.9, alpha=0.2)
        elif tag == "transition":
            X, Y = get_polygon(node)
            plt.plot(X, Y, "--r", lw=1.6, alpha=0.2)




Mohcine Chraibi's avatar
Mohcine Chraibi committed
64 65 66 67 68 69 70 71 72 73 74 75 76 77
def is_inside(trajectories, left, right, down, up):
    """
    up --> . _____.
           |      |
  down --> . -----.
           ^      ^
          left   right
    """
    condition = (trajectories[0, 2] > left) & \
                (trajectories[0, 2] < right) & \
                (trajectories[0, 3] > down) & \
                (trajectories[0, 3] < up)
    return condition.all()

78 79 80 81 82 83 84 85 86 87 88 89 90 91 92 93 94 95 96
def PassedLine(p, e):
    """
    check if pedestrian (given by matrix p) passed the line [x1, x2, y1, y2] with x1<x2 and  y1<y2
    """
    assert (isinstance(e, list) or isinstance(e, np.ndarray)) and len(e) == 4,\
        "exit should be a list with len=4"

    x1 = e[0]
    y1 = e[1]
    x2 = e[2]
    y2 = e[3]
    A = np.array([x1, y1])
    B = np.array([x2, y2])

    is_left_and_right = (any(np.cross(B-A, p[:, 2:]-A)) < 0) & (any(np.cross(B-A, p[:, 2:]-A)) > 0)
    is_between = any(np.dot(p[:, 2:]-A, B-A) > 0) & any(np.dot(p[:, 2:]-B, A-B) > 0)

    return  is_left_and_right and is_between

Mohcine Chraibi's avatar
Mohcine Chraibi committed
97

98 99 100 101 102 103 104 105 106 107 108 109 110 111 112 113 114 115 116
def PassedLineX(p, exit):
    """
    check if pedestrian (given by matrix p) passed the vertical line x, [y1, y2] y1<y2
    """
    x = exit[0]
    y1 = exit[1]
    y2 = exit[2]
    return any(p[:, 2] <= x) & any(p[:, 2] >= x) & any(p[:, 3] >= y1) & any(p[:, 3] <= y2)


def PassedLineY(p, exit):
    """
    check if pedestrian (given by matrix p) passed the horizontal line y, [x1, x2] x1<x2
    """
    y = exit[0]
    x1 = exit[1]
    x2 = exit[2]
    return any(p[:, 3] <= y) & any(p[:, 3] >= y) & any(p[:, 2] >= x1) & any(p[:, 2] <= x2)

Mohcine Chraibi's avatar
Mohcine Chraibi committed
117 118 119 120 121 122 123 124 125 126 127 128 129
def get_num_threads(filename):
    """
    get num_threads
    """
    logging.info("parsing <%s>"%filename)
    try:
        xmldoc = minidom.parse(filename)
    except:
        logging.critical('could not parse file %s. exit'%filename)
        exit(FAILURE)
    num_threads = float(xmldoc.getElementsByTagName('num_threads')[0].firstChild.nodeValue)
    return num_threads
    
130 131 132 133 134 135 136 137
def get_maxtime(filename):
    """
    get max sim time
    """
    logging.info("parsing <%s>"%filename)
    try:
        xmldoc = minidom.parse(filename)
    except:
Mohcine Chraibi's avatar
Mohcine Chraibi committed
138
        logging.critical('could not parse file %s. exit'%filename)
139 140
        exit(FAILURE)
    maxtime = float(xmldoc.getElementsByTagName('max_sim_time')[0].firstChild.nodeValue)
141 142 143 144 145 146 147 148
    return maxtime


def parse_file(filename):
    """
    parse trajectories in Travisto-format and output results
    in the following  format: id    frame    x    y
    (no sorting of the data is performed)
149 150
    returns:
    fps: frames per second
151
    N: number of pedestrians
152
    data: trajectories (numpy.array) [id fr x y]
153 154 155 156 157 158 159 160
    """
    logging.info("parsing <%s>"%filename)
    try:
        xmldoc = minidom.parse(filename)
    except:
        logging.critical('could not parse file. exit')
        exit(FAILURE)
    N = int(xmldoc.getElementsByTagName('agents')[0].childNodes[0].data)
161
    fps = xmldoc.getElementsByTagName('frameRate')[0].childNodes[0].data #type unicode
162 163
    fps = float(fps)
    fps = int(fps)
Mohcine Chraibi's avatar
Mohcine Chraibi committed
164
    #print "fps=", fps
165
    #fps = int(xmldoc.getElementsByTagName('frameRate')[0].childNodes[0].data)
166
    logging.info("Npeds = %d, fps = %d"%(N, fps))
167 168 169 170 171 172 173 174 175
    frames = xmldoc.childNodes[0].getElementsByTagName('frame')
    data = []
    for frame in frames:
        frame_number = int(frame.attributes["ID"].value)
        for agent in frame.getElementsByTagName("agent"):
            agent_id = int(agent.attributes["ID"].value)
            x = float(agent.attributes["x"].value)
            y = float(agent.attributes["y"].value)
            data += [agent_id, frame_number, x, y]
176
    data = np.array(data).reshape((-1, 4))
177
    return fps, N, data
Mohcine Chraibi's avatar
Mohcine Chraibi committed
178 179 180 181 182 183


def flow(fps, N, data, x0):
    """
    measure the flow at a vertical line given by <x0>
    trajectories are given by <data> in the following format: id    frame    x    y
184
    input:
Mohcine Chraibi's avatar
Mohcine Chraibi committed
185 186 187 188 189 190 191
    - fps: frame per second
    - N: number of peds
    - data: trajectories
    - x0: x-coordinate of the vertical measurement line
    output:
    - flow
    """
Mohcine Chraibi's avatar
Mohcine Chraibi committed
192
    logging.info('Measure flow at %f'%x0)
Mohcine Chraibi's avatar
Mohcine Chraibi committed
193 194 195
    if not isinstance(data, np.ndarray):
        logging.critical("flow() accepts data of type <ndarray>. exit")
        exit(FAILURE)
196
    peds = np.unique(data[:, 0]).astype(int)
Mohcine Chraibi's avatar
Mohcine Chraibi committed
197 198
    times = []
    for ped in peds:
199 200
        d = data[data[:, 0] == ped]
        passed = d[d[:, 2] >= x0]
Mohcine Chraibi's avatar
Mohcine Chraibi committed
201 202 203
        if passed.size == 0:  # pedestrian did not pass the line
            logging.critical("Pedestrian <%d> did not pass the line at <%.2f>"%(ped, x0))
            exit(FAILURE)
204
        first = min(passed[:, 1])
Mohcine Chraibi's avatar
Mohcine Chraibi committed
205
        #print "ped= ", ped, "first=",first
206
        times.append(first)
Mohcine Chraibi's avatar
Mohcine Chraibi committed
207 208
    if len(times) < 2:
        logging.warning("Number of pedestrians passing the line is small. return 0")
Mohcine Chraibi's avatar
Mohcine Chraibi committed
209
        return 0
210 211
    logging.info("min(times)=%f max(times)=%f"%(min(times), max(times)))
    return fps * float(N-1) / (max(times) - min(times))
212 213


Mohcine Chraibi's avatar
Mohcine Chraibi committed
214

Mohcine Chraibi's avatar
Mohcine Chraibi committed
215