├── .gitignore ├── README.rst └── mm ├── __init__.py ├── log_math.py ├── log_math_test.py └── path_inference ├── HardFilter.py ├── SoftFilter.py ├── __init__.py ├── example.py ├── example_utils.py ├── json.py ├── learning_traj.py ├── learning_traj_elements.py ├── learning_traj_elements_test.py ├── learning_traj_em.py ├── learning_traj_em_test.py ├── learning_traj_filter.py ├── learning_traj_filter_test.py ├── learning_traj_optimizer.py ├── learning_traj_optimizer_test.py ├── learning_traj_smoother.py ├── learning_traj_smoother_test.py ├── learning_traj_test.py ├── learning_traj_viterbi.py ├── learning_traj_viterbi_test.py ├── mapping ├── Counter.py ├── FancyFeatureMapper.py ├── FeatureMapper.py ├── FlowAnalysis.py ├── HFDecimation.py ├── PathCounter.py ├── SparseDecimation.py ├── __init__.py ├── decimation.py ├── decimation_test.py └── utils.py ├── path_builder.py ├── projector.py ├── structures.py └── utils.py /.gitignore: -------------------------------------------------------------------------------- 1 | *pyc -------------------------------------------------------------------------------- /README.rst: -------------------------------------------------------------------------------- 1 | Path inference filter quick documentation 2 | ========================================== 3 | 4 | 5 | The path inference filter (PIF in short) maps GPS waypoints into trajectories 6 | on a road network. It is used to map this sort of data: 7 | http://youtu.be/OxCPL4KsDfI 8 | into this sort of output: http://youtu.be/tj53gGCCNgs 9 | 10 | This code is the academic code that goes along the paper: ... 11 | 12 | 13 | This code is designed to be readable and correct, not fast. As such, you will 14 | probably want to rewrite some core sections in your favourite programming 15 | language. It is also extensively covered by a test suite that you can use as 16 | a reference. If you want a (much faster and more complete) implementation in 17 | scala and java, please contact the author. 18 | 19 | Quick start guide 20 | ------------------ 21 | 22 | The python PIF uses the following libraries: 23 | 24 | - numpy >= 1.3 25 | 26 | - nose >= 1.5 (for testing only) 27 | 28 | All you should need is:: 29 | 30 | git clone git://github.com/tjhunter/Path-Inference-Filter 31 | cd Path-Inference-Filter 32 | nosetest 33 | 34 | If all the diagnostic tests return correctly, you should be all set! 35 | 36 | 37 | Basic filtering 38 | ---------------- 39 | 40 | A tutorial has been written to explain how to filter trajectories, which you 41 | can find in *mm/path_inference/example.py*. 42 | The python PIF code does not include mapping or path discovery due to licensing 43 | issues. You have to write your own code to interface the PIF with your favorite 44 | data source and road network. 45 | 46 | 47 | Learning 48 | --------- 49 | 50 | No tutorial has been written for learning yet. However, the learning functions 51 | should have enough documentation. You can take a look at the tests of the 52 | optimizer as a starting point: *mm/path_inference/learning_traj_optimizer.py*. -------------------------------------------------------------------------------- /mm/__init__.py: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/tjhunter/Path-Inference-Filter/4ea625ea586e0688ce8a9490533861afab71067e/mm/__init__.py -------------------------------------------------------------------------------- /mm/log_math.py: -------------------------------------------------------------------------------- 1 | #Copyright 2011, 2012 Timothy Hunter 2 | # 3 | #This library is free software; you can redistribute it and/or 4 | #modify it under the terms of the GNU Lesser General Public 5 | #License as published by the Free Software Foundation; either 6 | #version 2.1 of the License, or (at your option) version 3. 7 | # 8 | #This library is distributed in the hope that it will be useful, 9 | #but WITHOUT ANY WARRANTY; without even the implied warranty of 10 | #MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU 11 | #Lesser General Public License for more details. 12 | # 13 | #You should have received a copy of the GNU Lesser General Public 14 | #License along with this library. If not, see . 15 | 16 | # pylint: disable=W0105 17 | ''' 18 | Some operations to perform common procedures in log domain. 19 | 20 | Created on Sep 2, 2011 21 | 22 | @author: tjhunter 23 | ''' 24 | from math import exp, log 25 | import numpy as np 26 | 27 | MINUS_INF = -float('inf') 28 | INF = float('inf') 29 | MINUS_LOG = -100 30 | 31 | def lse(xs): 32 | """ Sum of scalars in log domain (result also returned in log domain). 33 | 34 | Input: 35 | - list of elements 36 | """ 37 | if len(xs) == 0: 38 | return MINUS_INF 39 | # print xs 40 | x_max = max(xs) 41 | if x_max == MINUS_INF: 42 | return MINUS_INF 43 | return x_max + log(sum([exp(x-x_max) for x in xs])) 44 | 45 | def lse_npy(xs): 46 | """ Sum of scalars in log domain (result also returned in log domain). 47 | 48 | Input: 49 | - 1-D numpy array 50 | """ 51 | if xs.shape[0] == 0: 52 | return MINUS_INF 53 | x_max = np.max(xs) 54 | if x_max == MINUS_INF: 55 | return MINUS_INF 56 | return x_max + np.log(np.sum(np.exp(np.clip(xs-x_max, MINUS_LOG, INF)))) 57 | 58 | 59 | def lse_vect(xs): 60 | """ Works on a MxN array, will return a M array. 61 | """ 62 | (M, N) = xs.shape 63 | if N == 0: 64 | return np.ones(M) * MINUS_INF 65 | xs_max = np.max(xs, axis=1) 66 | return xs_max + np.log(np.sum(np.exp(xs - \ 67 | np.outer(xs_max, 68 | np.ones(N))), axis=1)) 69 | 70 | def get_scaled_vector(v): 71 | """ Input: a numpy vector. 72 | Output a pair of (log(norm2), normalized vector). 73 | """ 74 | norm = np.abs(v).max() 75 | if norm == 0: 76 | return (MINUS_INF, np.zeros_like(v)) 77 | lnorm = log(norm) 78 | return (lnorm, v/norm) 79 | 80 | 81 | def lse_vec(xs): 82 | """ Sum of vectors in log domain. 83 | A vector in log_domain is represented as a pair of (scaling factor, vector) 84 | 85 | """ 86 | assert xs, xs 87 | norm_max = max([norm for (norm, v) in xs]) 88 | x = np.zeros_like(xs[0][1]) 89 | # Everything is zero, no need to continue. 90 | if norm_max == MINUS_INF: 91 | return (MINUS_INF, x) 92 | for (norm, v) in xs: 93 | if norm - norm_max > MINUS_LOG: 94 | x += exp(norm - norm_max) * v 95 | (n2, x_res) = get_scaled_vector(x) 96 | return (norm_max + n2, x_res) 97 | 98 | def lse_vec_(xs): 99 | """ Sum of vectors in log domain. 100 | A vector in log_domain is represented as a pair of (scaling factor, vector) 101 | 102 | """ 103 | norm_max = max([norm for (norm, v) in xs]) 104 | x = np.zeros_like(xs[0][1]) 105 | # Everything is zero, no need to continue. 106 | if norm_max == MINUS_INF: 107 | return (MINUS_INF, x) 108 | 109 | for (norm, v) in xs: 110 | if norm - norm_max > -50: 111 | x += exp(norm - norm_max) * v 112 | (n2, x_res) = get_scaled_vector(x) 113 | return (norm_max + n2, x_res) 114 | 115 | def lse_vec_2(xs_dirs, xs_norms): 116 | """ Sum of vectors in log domain. 117 | A vector in log_domain is represented as a pair of (scaling factor, vector) 118 | 119 | Arguments: 120 | - xs_dirs: list of arrays 121 | - xs_norms: list 122 | """ 123 | norm_max = max(xs_norms) 124 | x = np.zeros_like(xs_dirs[0]) 125 | # Everything is zero, no need to continue. 126 | if norm_max == MINUS_INF: 127 | return (MINUS_INF, x) 128 | L = len(xs_dirs) 129 | i = 0 130 | while i < L: 131 | norm = xs_norms[i] 132 | if norm - norm_max > MINUS_LOG: 133 | v = xs_dirs[i] 134 | x += exp(norm - norm_max) * v 135 | i += 1 136 | max_norm_x = np.max(np.abs(x)) 137 | return (norm_max + np.log(max_norm_x), x / max_norm_x) 138 | 139 | 140 | def lse_vec_npy(xs_dirs, xs_norms): 141 | """ Sum of vectors in log domain. 142 | A vector in log_domain is represented as a pair of (scaling factor, vector) 143 | 144 | Arguments: 145 | - xs_dirs: N by M array 146 | - xs_norms: N array 147 | """ 148 | norm_max = np.max(xs_norms) 149 | # Everything is zero, no need to continue. 150 | if norm_max == MINUS_INF: 151 | return (MINUS_INF, np.zeros_like(xs_dirs[0])) 152 | ws = np.exp(np.clip(xs_norms-norm_max, MINUS_LOG, INF)) 153 | x = np.dot(xs_dirs.T, ws) 154 | max_norm_x = np.max(np.abs(x)) 155 | return (norm_max + np.log(max_norm_x), x / max_norm_x) 156 | -------------------------------------------------------------------------------- /mm/log_math_test.py: -------------------------------------------------------------------------------- 1 | ''' 2 | Copyright 2011, 2012 Timothy Hunter 3 | 4 | This library is free software; you can redistribute it and/or 5 | modify it under the terms of the GNU Lesser General Public 6 | License as published by the Free Software Foundation; either 7 | version 2.1 of the License, or (at your option) version 3. 8 | 9 | This library is distributed in the hope that it will be useful, 10 | but WITHOUT ANY WARRANTY; without even the implied warranty of 11 | MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU 12 | Lesser General Public License for more details. 13 | 14 | You should have received a copy of the GNU Lesser General Public 15 | License along with this library. If not, see . 16 | ''' 17 | # pylint: disable=W0105 18 | ''' 19 | Created on Sep 2, 2011 20 | 21 | @author: tjhunter 22 | ''' 23 | 24 | from mm.log_math import lse, lse_vec, MINUS_INF 25 | import numpy as np 26 | # Forces Numpy to consider warnings as errors. 27 | np.seterr(all='raise') 28 | from numpy import log 29 | 30 | def log_safe(x): 31 | """ Ensure that log(0) == -inf and not an error. """ 32 | if x == 0.0: 33 | return -float('inf') 34 | return log(x) 35 | 36 | def test_1(): 37 | """ test_1 """ 38 | v = [1, 2, 0.0] 39 | lv = [log_safe(x) for x in v] 40 | assert abs(log(3) - lse(lv)) < 1e-6, (v, lv) 41 | 42 | def test_lse_vec_1(): 43 | ''' test_lse_vec_1 ''' 44 | # Trying to add a zero vector with itself 45 | xs = [(MINUS_INF, np.zeros(2))] 46 | (u, v) = lse_vec(xs) 47 | assert u == MINUS_INF, u 48 | assert np.abs(v).max() == 0 49 | 50 | def test_lse_vec_2(): 51 | ''' test_lse_vec_2 ''' 52 | # Trying to add a zero vector with itself 53 | xs = [(MINUS_INF, np.ones(2))] 54 | (u, v) = lse_vec(xs) 55 | assert u == MINUS_INF, u 56 | assert np.abs(v).max() == 0 57 | 58 | def test_2(): 59 | """ test_1 """ 60 | inf = float("inf") 61 | xs = [(log(3.0), np.ones(2)), (2, np.zeros(2)), \ 62 | (-inf, np.ones(2)), (-inf, np.zeros(2))] 63 | (norm, x) = lse_vec(xs) 64 | assert abs(norm - log(3)) < 1e-6, (norm, log(3), x) 65 | assert np.abs(x - 1).max() < 1e-6, (x) -------------------------------------------------------------------------------- /mm/path_inference/HardFilter.py: -------------------------------------------------------------------------------- 1 | #Copyright 2011, 2012 Timothy Hunter 2 | # 3 | #This library is free software; you can redistribute it and/or 4 | #modify it under the terms of the GNU Lesser General Public 5 | #License as published by the Free Software Foundation; either 6 | #version 2.1 of the License, or (at your option) version 3. 7 | # 8 | #This library is distributed in the hope that it will be useful, 9 | #but WITHOUT ANY WARRANTY; without even the implied warranty of 10 | #MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU 11 | #Lesser General Public License for more details. 12 | # 13 | #You should have received a copy of the GNU Lesser General Public 14 | #License along with this library. If not, see . 15 | ''' 16 | Created on Nov 25, 2011 17 | 18 | @author: tjhunter 19 | ''' 20 | 21 | class HardFilter(object): 22 | """ Interface for a filter that performs hard assignments. 23 | 24 | Interesting fields: 25 | - traj : a trajectory object 26 | - assignments : a list of assignment (integer index, for each feature 27 | element of the trajectory) 28 | """ 29 | # pylint: disable=w0201 30 | def __init__(self): 31 | pass 32 | 33 | def computeAssignments(self): 34 | """ Computes the assignments. 35 | """ 36 | self.assignments = None 37 | raise NotImplementedError() 38 | -------------------------------------------------------------------------------- /mm/path_inference/SoftFilter.py: -------------------------------------------------------------------------------- 1 | #Copyright 2011, 2012 Timothy Hunter 2 | # 3 | #This library is free software; you can redistribute it and/or 4 | #modify it under the terms of the GNU Lesser General Public 5 | #License as published by the Free Software Foundation; either 6 | #version 2.1 of the License, or (at your option) version 3. 7 | # 8 | #This library is distributed in the hope that it will be useful, 9 | #but WITHOUT ANY WARRANTY; without even the implied warranty of 10 | #MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU 11 | #Lesser General Public License for more details. 12 | # 13 | #You should have received a copy of the GNU Lesser General Public 14 | #License along with this library. If not, see . 15 | ''' 16 | Created on Nov 27, 2011 17 | 18 | @author: tjhunter 19 | ''' 20 | from mm.path_inference.HardFilter import HardFilter 21 | 22 | class SoftFilter(HardFilter): 23 | """ Interface for a filter that performs soft assignments (probability 24 | distributions). 25 | 26 | Interesting fields: 27 | - traj : a trajectory object 28 | - probabilities : list of numpy arrays 29 | - log_probabilities : list of numpy arrays 30 | - assignments : the most likely element, computed from the probabilities. 31 | """ 32 | # pylint: disable=w0201 33 | def __init__(self): 34 | HardFilter.__init__(self) 35 | 36 | def computeProbabilities(self): 37 | """ This function computes the values of self.probabilities 38 | and self.log_probabilities. 39 | """ 40 | self.probabilities = None 41 | raise NotImplementedError() 42 | 43 | def computeAssignments(self): 44 | self.assignments = [list(probs).index(max(probs)) for \ 45 | probs in self.probabilities] 46 | -------------------------------------------------------------------------------- /mm/path_inference/__init__.py: -------------------------------------------------------------------------------- 1 | #Copyright 2011, 2012 Timothy Hunter 2 | # 3 | #This library is free software; you can redistribute it and/or 4 | #modify it under the terms of the GNU Lesser General Public 5 | #License as published by the Free Software Foundation; either 6 | #version 2.1 of the License, or (at your option) version 3. 7 | # 8 | #This library is distributed in the hope that it will be useful, 9 | #but WITHOUT ANY WARRANTY; without even the implied warranty of 10 | #MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU 11 | #Lesser General Public License for more details. 12 | # 13 | #You should have received a copy of the GNU Lesser General Public 14 | #License along with this library. If not, see . 15 | ''' Main package for path inference. 16 | 17 | Take a look at at the example to see how to use the classes in the package. 18 | 19 | More documentation should be added to run the optimizer. 20 | ''' 21 | from SoftFilter import SoftFilter 22 | from HardFilter import HardFilter -------------------------------------------------------------------------------- /mm/path_inference/example.py: -------------------------------------------------------------------------------- 1 | #Copyright 2011, 2012 Timothy Hunter 2 | # 3 | #This library is free software; you can redistribute it and/or 4 | #modify it under the terms of the GNU Lesser General Public 5 | #License as published by the Free Software Foundation; either 6 | #version 2.1 of the License, or (at your option) version 3. 7 | # 8 | #This library is distributed in the hope that it will be useful, 9 | #but WITHOUT ANY WARRANTY; without even the implied warranty of 10 | #MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU 11 | #Lesser General Public License for more details. 12 | # 13 | #You should have received a copy of the GNU Lesser General Public 14 | #License along with this library. If not, see . 15 | 16 | # pylint: disable=W0105 17 | ''' 18 | Tutorial to the Path inference filter code. 19 | 20 | Created on Jan 26, 2012 21 | 22 | @author: tjhunter 23 | 24 | This code defines a simple maze as a road network, simulates the crossing of 25 | this maze by a vehicle and runs the filter to reconstruct the trajectory. 26 | ''' 27 | import random 28 | import math 29 | from mm.path_inference.example_utils import \ 30 | create_trajectory_for_lattice_network, LatticePathBuilder 31 | 32 | """ The road network is a an NxN lattice. Each of the nodes of the lattice is 33 | defined by its (x,y) coordinate ((0,0) at the bottom left). Each link of the 34 | lattice is directed. There only a single link between each node of the 35 | lattice: the lattice only contains links that go up or right. 36 | 37 | A link is defined as a pair of nodes (from, to). 38 | """ 39 | lattice_size = 5 40 | lattice_link_length = 100 # meters 41 | 42 | """ Since all the links have the same length, we use the travel time on each 43 | link as a feature. Each link has a random travel time that follows a Gaussian 44 | distribution. The standard deviation of all the distributions is the same. 45 | the mean is randomly chosen for each link. 46 | """ 47 | """ Standard deviation. 48 | """ 49 | sigma = 10 # meters 50 | 51 | """ The observation noise is simulated using a normal distribution. 52 | """ 53 | obs_sigma = 0 # meters 54 | 55 | """ Mean travel times on every link. 56 | It is a random value on each link to ensure that the travel times will be 57 | different by taking different paths. 58 | """ 59 | means = {} # nodeid -> seconds 60 | for x in range(lattice_size): 61 | for y in range(lattice_size): 62 | n = (x, y) 63 | n_right = (x+1, y) 64 | n_up = (x, y+1) 65 | means[(n, n_right)] = random.random() * 10 + 40 66 | means[(n, n_up)] = random.random() * 10 + 40 67 | 68 | """ The number of observations, minus 2. 69 | A trajectory goes from the (0,0) node up to the (N-1, N-1) node. 70 | We observe a number of points on this trajectory: one observation on 71 | the first link, one observation on the last link, and some additional 72 | observations picked at random among the links that define thr trajectory. 73 | """ 74 | num_observations = 1 75 | 76 | """ Generation of some synthetic observations on this road network, and some 77 | travel times between the observations. 78 | """ 79 | (observations, observed_travel_times) = \ 80 | create_trajectory_for_lattice_network(lattice_size, lattice_link_length, 81 | sigma, obs_sigma, num_observations, 82 | means) 83 | 84 | """ FILTERING CODE. 85 | 86 | This is an example of running the path inference filter on the observed data. 87 | 88 | Each point and each path is represented as a feature vector, with one feature 89 | for paths and one feature for observations. 90 | The gps noise model is a Gaussian model. The path model is a Gaussian model 91 | on the travel time of the vehicle. This model is different than the one used 92 | in the paper (based on traveled distances). The model in the paper would 93 | make no sense since all links have the same length. 94 | """ 95 | 96 | from mm.path_inference.learning_traj import LearningTrajectory 97 | from mm.path_inference.learning_traj_viterbi import TrajectoryViterbi1 98 | from mm.path_inference.learning_traj_smoother import TrajectorySmoother1 99 | import numpy as np 100 | 101 | """ Building the feature vectors and the transitions. 102 | """ 103 | 104 | path_builder = LatticePathBuilder() 105 | 106 | def distance(gps1, gps2): 107 | """ Distance between two gps points, defined as the Cartesian measure of the 108 | coordinates. 109 | """ 110 | return math.sqrt((gps1.lat-gps2.lat)**2 + (gps1.lng-gps2.lng)**2) 111 | 112 | def point_feature_vector(sc): 113 | """ The feature vector of a point. 114 | """ 115 | return [[0, -0.5 * distance(sc.gps_pos, s.gps_pos)] for s in sc.states] 116 | 117 | def path_feature_vector(path, tt): 118 | """ The feature vector of a path. 119 | """ 120 | (s1, p, s2) = path 121 | # We suppose we make this travel time observation for this vehicle: 122 | assert p[0] == s1.link_id 123 | assert p[-1] == s2.link_id 124 | # This is some "observed" travel time 125 | # Mean of some idealized travel time distribution. 126 | m = (1 - s1.offset / lattice_link_length) * means[s1.link_id] \ 127 | + sum([means[link] for link in p[1:-1]]) \ 128 | + s2.offset / lattice_link_length * means[s2.link_id] 129 | var = (sigma * (1 - s1.offset / lattice_link_length)) ** 2 130 | var += (sigma ** 2) * len(path[1:-1]) 131 | var += (sigma * s2.offset / lattice_link_length) ** 2 132 | return [-0.5 * ((m - tt) ** 2) / var, 0] 133 | 134 | """ The final list of lists of feature vectors.""" 135 | features = [] 136 | """ The transitions between the elements. """ 137 | transitions = [] 138 | features.append(point_feature_vector(observations[0])) 139 | for (sc1, sc2, tt_) in zip(observations[:-1], observations[1:], 140 | observed_travel_times): 141 | (trans1, ps, trans2) = path_builder.getPathsBetweenCollections(sc1, sc2) 142 | transitions.append(trans1) 143 | paths_features = [] 144 | for path_ in ps: 145 | paths_features.append(path_feature_vector(path_, tt_)) 146 | features.append(paths_features) 147 | transitions.append(trans2) 148 | features.append(point_feature_vector(sc2)) 149 | 150 | """ We can build a trajectory. 151 | """ 152 | traj = LearningTrajectory(features, transitions) 153 | 154 | """ *** Running filters *** 155 | """ 156 | 157 | """ Weight vector of the model. 158 | 159 | Note: these weights have not be tuned in any way and are here for 160 | demonstration only. 161 | """ 162 | theta = np.array([1, 1]) 163 | """ Viterbi filter (most likely elements) """ 164 | viterbi = TrajectoryViterbi1(traj, theta) 165 | viterbi.computeAssignments() 166 | # The indexes of the most likely elements of the trajectory 167 | # Point indexes and path indexes are interleaved. 168 | most_likely_indexes = viterbi.assignments 169 | 170 | """ Alpha-beta smoother (distributions) """ 171 | smoother = TrajectorySmoother1(traj, theta) 172 | smoother.computeProbabilities() 173 | probabilities = smoother.probabilities 174 | -------------------------------------------------------------------------------- /mm/path_inference/example_utils.py: -------------------------------------------------------------------------------- 1 | #Copyright 2011, 2012 Timothy Hunter 2 | # 3 | #This library is free software; you can redistribute it and/or 4 | #modify it under the terms of the GNU Lesser General Public 5 | #License as published by the Free Software Foundation; either 6 | #version 2.1 of the License, or (at your option) version 3. 7 | # 8 | #This library is distributed in the hope that it will be useful, 9 | #but WITHOUT ANY WARRANTY; without even the implied warranty of 10 | #MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU 11 | #Lesser General Public License for more details. 12 | # 13 | #You should have received a copy of the GNU Lesser General Public 14 | #License along with this library. If not, see . 15 | ''' 16 | Created on Jan 26, 2012 17 | 18 | @author: tjhunter 19 | ''' 20 | # pylint: disable=W0105 21 | import random 22 | random.seed = 2 # Fixing the seed to reproduce results. 23 | from mm.path_inference.structures import State, LatLng, StateCollection 24 | from mm.path_inference.path_builder import PathBuilder 25 | 26 | """ We sample some observation links from the trajectory. 27 | """ 28 | def get_pos(link_id, off, lattice_link_length): 29 | """ Returns the (lat, lng) that corrsponds to this position in the network. 30 | 31 | Paramters: 32 | - link_id: a tupe of (start node, end node) 33 | - off: offset length (meters) 34 | """ 35 | ((x0, y0), (x1, y1)) = link_id 36 | u = off / lattice_link_length 37 | lat = lattice_link_length * (u * x1 + (1-u) * x0) 38 | lng = lattice_link_length * (u * y1 + (1-u) * y0) 39 | return (lat, lng) 40 | 41 | 42 | def create_trajectory_for_lattice_network(lattice_size, lattice_link_length, 43 | sigma, obs_sigma, 44 | num_observations, means): 45 | 46 | """ Simulate a vehicle travelling on the maze. 47 | 48 | This is implemented as a random walk going from one node to the other 49 | in the lattice. 50 | 51 | Parameters: 52 | - lattice_size 53 | - lattice_link_length 54 | - sigma 55 | - obs_sigma 56 | - num_observations 57 | - means: dictionary of mean travel times on each link: link_id -> mean_tt 58 | (in seconds) 59 | """ 60 | traj_nodes = [(0, 0)] 61 | while True: 62 | (x, y) = traj_nodes[-1] 63 | if x == lattice_size-1 and y == lattice_size-1: 64 | break 65 | if x == lattice_size-1: 66 | traj_nodes.append((x, y+1)) 67 | elif y == lattice_size-1: 68 | traj_nodes.append((x+1, y)) 69 | else: 70 | if random.randint(0, 1) == 0: 71 | traj_nodes.append((x+1, y)) 72 | else: 73 | traj_nodes.append((x, y+1)) 74 | 75 | num_traj_nodes = len(traj_nodes) 76 | 77 | """ Some random times on each link: 78 | """ 79 | travel_times = {} 80 | for x in range(lattice_size): 81 | for y in range(lattice_size): 82 | n = (x, y) 83 | n_right = (x+1, y) 84 | n_up = (x, y+1) 85 | travel_times[(n, n_right)] = random.gauss(means[(n, n_right)], sigma) 86 | travel_times[(n, n_up)] = random.gauss(means[(n, n_up)], sigma) 87 | 88 | observation_link_indexes = [0] + random.sample(range(1, num_traj_nodes-2), 89 | num_observations) + [num_traj_nodes-2] 90 | """ Observation contains a list of states: this will be our observations. """ 91 | observations = [] 92 | for n_idx in observation_link_indexes: 93 | # The link index 94 | link = (traj_nodes[n_idx], traj_nodes[n_idx+1]) 95 | # Pick a random offset 96 | offset = lattice_link_length * random.random() 97 | # The simulated GPS observation 98 | # Lat and lng in meters 99 | (true_lat, true_lng) = get_pos(link, offset, lattice_link_length) 100 | pos = LatLng(lat=random.gauss(true_lat, obs_sigma), 101 | lng=random.gauss(true_lng, obs_sigma)) 102 | # Do a few projections in the neighboring links. 103 | states = [] 104 | ((x0, y0), _) = link 105 | for x in range(x0-2, x0+2): 106 | for y in range(y0-2, y0+2): 107 | if x < 0 or y < 0: 108 | continue 109 | for (dx, dy) in [(0, 1), (1, 0)]: 110 | x_end = x+dx 111 | y_end = y+dy 112 | if x_end >= lattice_size or y_end >= lattice_size: 113 | continue 114 | proj_link = ((x, y), (x_end, y_end)) 115 | # Find the projection on the link 116 | (lat_start, lng_start) = get_pos(proj_link, 0, lattice_link_length) 117 | off = dx * (pos.lat - lat_start) + dy * (pos.lng - lng_start) 118 | off = max(min(off, lattice_link_length), 0) 119 | s = State(proj_link, off, 120 | pos=LatLng(*get_pos(proj_link, off, 121 | lattice_link_length))) #Using magic 122 | states.append(s) 123 | sc = StateCollection(None, states, pos, None) 124 | sc.true_state = (link, offset) 125 | observations.append(sc) 126 | 127 | """ Simulate some travel times. 128 | """ 129 | observed_travel_times = [] 130 | traj_links = zip(traj_nodes[:-1], traj_nodes[1:]) 131 | for i in range(num_observations+1): 132 | tt = 0 133 | (start_link, start_offset) = observations[i].true_state 134 | tt += travel_times[start_link] * (1 - start_offset / lattice_link_length) 135 | links_between = traj_links[observation_link_indexes[i]+1:\ 136 | observation_link_indexes[i+1]] 137 | for l in links_between: 138 | tt += travel_times[l] 139 | (end_link, end_offset) = observations[i+1].true_state 140 | tt += travel_times[end_link] * end_offset / lattice_link_length 141 | observed_travel_times.append(tt) 142 | 143 | return (observations, observed_travel_times) 144 | 145 | 146 | class LatticePathBuilder(PathBuilder): 147 | """ Path builder for a lattice network. 148 | 149 | The path builder will generate all the paths between two state collections 150 | mapped in the lattice network. 151 | """ 152 | 153 | def get_paths_lattice(self, start_node, end_node): 154 | """ Computes all the paths between a start node and an end node. 155 | """ 156 | (x0, y0) = start_node 157 | (x1, y1) = end_node 158 | if start_node == end_node: 159 | return [[]] 160 | if x0 > x1 or y0 > y1: 161 | return [] 162 | next_up = (x0, y0+1) 163 | next_right = (x0+1, y0) 164 | res = [] 165 | for p in self.get_paths_lattice(next_up, end_node): 166 | res.append([(start_node, next_up)] + p) 167 | for p in self.get_paths_lattice(next_right, end_node): 168 | res.append([(start_node, next_right)] + p) 169 | return res 170 | 171 | def getPaths(self, s1, s2): 172 | """ Returns a set of candidate paths between state s1 and state s3. 173 | Arguments: 174 | - s1 : a State object 175 | - s2 : a State object 176 | """ 177 | start_link = s1.link_id 178 | end_link = s2.link_id 179 | # This simple code does not handle that kind of cases 180 | if start_link == end_link: 181 | return [] 182 | return [(s1, [start_link] + p + [end_link], s2) \ 183 | for p in self.get_paths_lattice(start_link[1], end_link[0])] 184 | -------------------------------------------------------------------------------- /mm/path_inference/json.py: -------------------------------------------------------------------------------- 1 | ''' 2 | Copyright 2011, 2012 Timothy Hunter 3 | 4 | This library is free software; you can redistribute it and/or 5 | modify it under the terms of the GNU Lesser General Public 6 | License as published by the Free Software Foundation; either 7 | version 2.1 of the License, or (at your option) version 3. 8 | 9 | This library is distributed in the hope that it will be useful, 10 | but WITHOUT ANY WARRANTY; without even the implied warranty of 11 | MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU 12 | Lesser General Public License for more details. 13 | 14 | You should have received a copy of the GNU Lesser General Public 15 | License along with this library. If not, see . 16 | ''' 17 | # pylint: disable=W0105 18 | ''' 19 | Created on Sep 20, 2011 20 | 21 | @author: tjhunter 22 | 23 | Encoding/decoding conversions. 24 | ''' 25 | from structures import LatLng, State, Path, StateCollection 26 | import datetime 27 | 28 | def encode_LatLng(gps): 29 | return {'lat' : gps.lat, 'lng' : gps.lng} 30 | 31 | def decode_LatLng(dct): 32 | return LatLng(dct['lat'], dct['lng']) 33 | 34 | def encode_link_id(link_id): 35 | (nid, direction) = link_id 36 | return {'id': nid, 'direction': direction} 37 | 38 | def decode_link_id(dct): 39 | return (dct['id'], dct['direction']) 40 | 41 | def encode_State(state): 42 | return {'link':encode_link_id(state.link_id), \ 43 | 'offset':state.offset,\ 44 | 'gps_pos':encode_LatLng(state.gps_pos)} 45 | 46 | def decode_State(dct): 47 | gps_pos = decode_LatLng(dct['gps_pos']) if 'gps_pos' in dct else None 48 | return State(decode_link_id(dct['link']), \ 49 | dct['offset'], gps_pos) 50 | 51 | def encode_Path(path): 52 | return {'start':encode_State(path.start), \ 53 | 'links':[encode_link_id(link_id) for link_id in path.links], \ 54 | 'end':encode_State(path.end), \ 55 | 'latlngs':[encode_LatLng(latlng) for latlng in path.latlngs]} 56 | 57 | def decode_Path(dct): 58 | latlngs = [decode_LatLng(dct2) for dct2 in dct['latlngs']] \ 59 | if 'latlngs' in dct else None 60 | return Path(decode_State(dct['start']), \ 61 | [decode_link_id(dct2) for dct2 in dct['links']], \ 62 | decode_State(dct['end']), \ 63 | latlngs) 64 | 65 | def encode_time(time): 66 | return {'year':time.year, \ 67 | 'month':time.month, \ 68 | 'day':time.day, \ 69 | 'hour':time.hour, \ 70 | 'minute':time.minute, \ 71 | 'second':time.second} 72 | 73 | def decode_time(dct): 74 | return datetime.datetime(dct['year'], dct['month'], dct['day'], \ 75 | dct['hour'], dct['minute'], dct['second']) 76 | 77 | def encode_StateCollection(sc): 78 | return {'id':sc.id, 'latlng':encode_LatLng(sc.gps_pos), \ 79 | 'time':encode_time(sc.time), \ 80 | 'states': [encode_State(state) for state in sc.states]} 81 | 82 | def decode_StateCollection(dct): 83 | return StateCollection(dct['id'], [decode_State(dct2) \ 84 | for dct2 in dct['states']], \ 85 | decode_LatLng(dct['latlng']), \ 86 | decode_time(dct['time'])) 87 | 88 | -------------------------------------------------------------------------------- /mm/path_inference/learning_traj.py: -------------------------------------------------------------------------------- 1 | #Copyright 2011, 2012 Timothy Hunter 2 | # 3 | #This library is free software; you can redistribute it and/or 4 | #modify it under the terms of the GNU Lesser General Public 5 | #License as published by the Free Software Foundation; either 6 | #version 2.1 of the License, or (at your option) version 3. 7 | # 8 | #This library is distributed in the hope that it will be useful, 9 | #but WITHOUT ANY WARRANTY; without even the implied warranty of 10 | #MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU 11 | #Lesser General Public License for more details. 12 | # 13 | #You should have received a copy of the GNU Lesser General Public 14 | #License along with this library. If not, see . 15 | """ Trajectory representation for learning, and associated utilities for I/O. 16 | """ 17 | import numpy as np 18 | 19 | class LearningTrajectory: 20 | """ Trajectory in the good format for learning. 21 | 22 | Useful fields: 23 | - features: list of list of vectors (a vector is itself a list). 24 | - connections: list of list of pairs. 25 | - features_np: numpy array, each row is a feature 26 | - num_choices 27 | 28 | This class is read only. 29 | """ 30 | 31 | def __init__(self, features, connections): 32 | self.features = features 33 | self.connections = connections 34 | self.L = len(features) 35 | self.num_choices = [len(l) for l in self.features] 36 | # Numpy copy for performance. 37 | self.features_np = [np.array(feats) for feats in self.features] 38 | # Make sure 39 | # Dictionary for performance: 40 | def build_backward(conns): 41 | ''' Builds fast indexing of backward connections (using a dictionary). 42 | ''' 43 | d = {} 44 | for (i, j) in conns: 45 | if j not in d: 46 | d[j] = [i] 47 | else: 48 | d[j].append(i) 49 | return d 50 | self.connections_backward = [None] \ 51 | + [build_backward(conns) for conns in self.connections] 52 | def build_forward(conns): 53 | ''' Builds fast indexing of forward connections (using a dictionary). 54 | ''' 55 | d = {} 56 | for (i, j) in conns: 57 | if i not in d: 58 | d[i] = [j] 59 | else: 60 | d[i].append(j) 61 | return d 62 | self.connections_forward = [build_forward(conns) for conns \ 63 | in self.connections] 64 | self.check_invariants() 65 | 66 | def check_invariants(self): 67 | ''' Checks a few invariants on the trajectory, 68 | Raises an exception if the invariants are not verified. 69 | ''' 70 | assert(len(self.features) == self.L) 71 | assert(len(self.connections) == self.L - 1) 72 | for l in range(self.L-1): 73 | for (u, v) in self.connections[l]: 74 | assert(u >= 0), l 75 | assert(v >= 0), l 76 | assert(u < self.num_choices[l]), (l, (u, v)) 77 | assert(v < self.num_choices[l+1]), (l, (u, v)) 78 | for l in range(1, self.L): 79 | for v in self.connections_backward[l]: 80 | assert self.connections_backward[l][v], \ 81 | (l, v, self.connections_backward[l]) 82 | for l in range(self.L-1): 83 | for u in self.connections_forward[l]: 84 | assert self.connections_forward[l][u] 85 | 86 | def truncated(self, start=None, end=None): 87 | ''' The truncated version of a list. 88 | ''' 89 | if start is None: 90 | start = 0 91 | if end is None: 92 | end = self.L 93 | return LearningTrajectory(self.features[start:end], \ 94 | self.connections[start:end-1]) 95 | -------------------------------------------------------------------------------- /mm/path_inference/learning_traj_elements.py: -------------------------------------------------------------------------------- 1 | #Copyright 2011, 2012 Timothy Hunter 2 | # 3 | #This library is free software; you can redistribute it and/or 4 | #modify it under the terms of the GNU Lesser General Public 5 | #License as published by the Free Software Foundation; either 6 | #version 2.1 of the License, or (at your option) version 3. 7 | # 8 | #This library is distributed in the hope that it will be useful, 9 | #but WITHOUT ANY WARRANTY; without even the implied warranty of 10 | #MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU 11 | #Lesser General Public License for more details. 12 | # 13 | #You should have received a copy of the GNU Lesser General Public 14 | #License along with this library. If not, see . 15 | """ Computation elements for the learning trajectory, simple version. 16 | 17 | This class is not optimized with respect to log underflows. 18 | """ 19 | import numpy as np 20 | from numpy import dot, exp, log, outer 21 | from mm.log_math import lse, lse_vec, MINUS_INF, lse_npy, lse_vec_npy, lse_vec_2 22 | 23 | def check_weight(weight): 24 | """ Checks the weight vector respects a number of assumptions. """ 25 | assert(len(weight.shape) == 1) 26 | assert(weight.sum() > 0.99) 27 | assert(weight.sum() < 1.01) 28 | assert((weight >= 0).all()) 29 | assert((weight <= 1).all()) 30 | 31 | class LearningElementsRef: 32 | """ Elements for computing value, gradients, etc. 33 | 34 | This is the reference implementation (slow but correct, for testing). 35 | This implementation does not handle large feature values. 36 | 37 | Useful fields: 38 | - traj: a LearningTrajectory object. 39 | - theta: the weight vector 40 | - lZ: the logarithm of Z 41 | - gradLZ 42 | - hessLZ 43 | """ 44 | # pylint: disable=W0201 45 | def __init__(self, learning_traj, theta, choices=None, weights=None): 46 | self.traj = learning_traj 47 | self.theta = np.array(theta) 48 | # Computing and checkin the weight vectors: 49 | assert(choices or weights) 50 | if weights: 51 | assert(self.traj.L == len(weights)) 52 | self.weights = [np.array(w, dtype=np.double) for w in weights] 53 | if choices: 54 | assert(self.traj.L == len(choices)), \ 55 | (self.traj.L, len(choices)) 56 | self.weights = [np.zeros([Nl]) for Nl in self.traj.num_choices] 57 | for l in range(self.traj.L): 58 | c = choices[l] 59 | assert c >= 0, (l, choices[l]) 60 | assert c < self.traj.num_choices[l], (l, choices[l], 61 | self.traj.num_choices[l]) 62 | self.weights[l][c] = 1.0 63 | self.checkAssertions() 64 | 65 | def checkAssertions(self): 66 | """ Tests if a number of invariants are respected. 67 | """ 68 | # Check theta 69 | assert(len(self.theta.shape) == 1) 70 | N = self.theta.shape[0] 71 | L = self.traj.L 72 | for l in range(L): 73 | weight = self.weights[l] 74 | # weight[weight<0.1] = 0 75 | # weight[weight>=0.1] = 1 76 | assert(abs(np.sum(weight)-1)<1e-5), weight 77 | assert(np.all(weight>=0)), weight 78 | feats = self.traj.features_np[l] 79 | assert(len(feats.shape) == 2) 80 | N_l = self.traj.num_choices[l] 81 | assert(feats.shape[0] == N_l) 82 | assert(feats.shape[1] == N) 83 | assert(weight.shape[0] == N_l) 84 | 85 | def computeSStats(self): 86 | """ Computes sufficient statistics. """ 87 | if 'sstats' in self.__dict__: 88 | return 89 | L = self.traj.L 90 | # Expected sufficient statistics: 91 | self.sstats = [dot(self.weights[l], self.traj.features_np[l]) \ 92 | for l in range(L)] 93 | 94 | def computeValue(self): 95 | """ 96 | Calls LogZ. 97 | Provides logValue 98 | """ 99 | if 'logValue' in self.__dict__: 100 | return 101 | self.computeSStats() 102 | self.computeLogZ() 103 | L = self.traj.L 104 | # Expected sufficient statistics: 105 | self.logV1 = sum([dot(self.sstats[l], self.theta) for l in range(L)]) 106 | assert self.logV1 <= self.logZ, (self.logV1, self.logZ, self.sstats) 107 | self.logValue = self.logV1 - self.logZ 108 | 109 | def computeGradientValue(self): 110 | """ 111 | Calls LogZ. 112 | Provides logValue 113 | """ 114 | if 'grad_logValue' in self.__dict__: 115 | return 116 | self.computeSStats() 117 | L = self.traj.L 118 | # Expected sufficient statistics: 119 | self.grad_logV1 = sum([self.sstats[l] for l in range(L)]) 120 | self.computeGradientLogZ() 121 | self.grad_logValue = self.grad_logV1 - self.grad_logZ 122 | 123 | def computeHessianValue(self): 124 | """ 125 | Calls LogZ. 126 | Provides logValue 127 | """ 128 | if 'hess_logValue' in self.__dict__: 129 | return 130 | # Expected sufficient statistics: 131 | self.computeHessianLogZ() 132 | self.hess_logValue = - self.hess_logZ 133 | 134 | def computeLogZ(self): 135 | """ Reference computation of Z and logZ. Very inefficient but correct. 136 | Provides: 137 | - logZ 138 | Debug info: 139 | - Z 140 | """ 141 | if 'logZ' in self.__dict__: 142 | return 143 | L = self.traj.L 144 | # Weighting statistics. 145 | self.Z = 0 146 | def inner(indices): 147 | """ Inner working function. """ 148 | L_ = len(indices) 149 | if L_ == L: 150 | v1 = np.zeros_like(self.theta) 151 | for l in range(L): 152 | v1 += self.traj.features_np[l][indices[l]] 153 | self.Z += exp(dot(v1, self.theta)) 154 | else: 155 | i = indices[-1] 156 | if i in self.traj.connections_forward[L_-1]: 157 | for j in self.traj.connections_forward[L_-1][i]: 158 | inner(indices + [j]) 159 | for i in range(self.traj.num_choices[0]): 160 | inner([i]) 161 | self.logZ = log(self.Z) 162 | 163 | def computeGradientLogZ(self): 164 | """ Computes the gradient of the logarithm of Z. 165 | """ 166 | if 'grad_logZ' in self.__dict__: 167 | return 168 | self.computeLogZ() 169 | self.grad_Z = np.zeros_like(self.theta) 170 | L = self.traj.L 171 | def inner(indices): 172 | """ Inner work function. """ 173 | L_ = len(indices) 174 | if L_ == L: 175 | v1 = np.zeros_like(self.theta) 176 | for l in range(L): 177 | v1 += self.traj.features_np[l][indices[l]] 178 | g = exp(dot(v1, self.theta)) * v1 179 | # print(v1, self.theta, dot(v1, self.theta), g) 180 | self.grad_Z += g 181 | else: 182 | i = indices[-1] 183 | if i in self.traj.connections_forward[L_-1]: 184 | for j in self.traj.connections_forward[L_-1][i]: 185 | inner(indices + [j]) 186 | for i in range(self.traj.num_choices[0]): 187 | inner([i]) 188 | # print('Z=', self.Z) 189 | # print('grad_Z=', self.grad_Z) 190 | self.grad_logZ = self.grad_Z / self.Z 191 | 192 | def computeHessianLogZ(self): 193 | """ Hessian of log(Z). """ 194 | if 'hess_logZ' in self.__dict__: 195 | return 196 | N = len(self.theta) 197 | L = self.traj.L 198 | self.computeLogZ() 199 | self.computeGradientLogZ() 200 | self.hess_Z = np.zeros((N, N)) 201 | # Inner recursive function. 202 | # The stack should be large enough for our purpose. 203 | def inner(indices): 204 | """ Inner working function. """ 205 | L_ = len(indices) 206 | if L_ == L: 207 | v1 = np.zeros_like(self.theta) 208 | for l in range(L): 209 | v1 += self.traj.features_np[l][indices[l]] 210 | h = exp(dot(v1, self.theta)) * outer(v1, v1) 211 | self.hess_Z += h 212 | else: 213 | i = indices[-1] 214 | if i in self.traj.connections_forward[L_-1]: 215 | for j in self.traj.connections_forward[L_-1][i]: 216 | inner(indices + [j]) 217 | for i in range(self.traj.num_choices[0]): 218 | inner([i]) 219 | self.hess_logZ = self.hess_Z / self.Z \ 220 | - outer(self.grad_Z, self.grad_Z) / (self.Z * self.Z) 221 | 222 | class LearningElements0Bis(LearningElementsRef): 223 | """ Another test implementation that is secure against underflows (i.e., 224 | large feature vectors) but is exponentially slow to compute. 225 | Can be used on production data for small trajectories to make sure the 226 | optimization procedures are correct. 227 | """ 228 | def computeLogZ(self): 229 | """ Reference computation of Z and logZ, in the log domain. 230 | Very inefficient. 231 | Provides: 232 | - logZ 233 | Debug info: 234 | - Z 235 | """ 236 | if 'logZ' in self.__dict__: 237 | return 238 | L = self.traj.L 239 | # Weighting statistics. 240 | self.Z = 0 241 | self.logZ = MINUS_INF 242 | # List of the log weights of all the possible trajectories. 243 | intermediate_log_vals = [] 244 | def inner(indices): 245 | """ Inner working function. """ 246 | L_ = len(indices) 247 | if L_ == L: 248 | v1 = np.zeros_like(self.theta) 249 | for l in range(L): 250 | v1 += self.traj.features_np[l][indices[l]] 251 | intermediate_log_vals.append(dot(v1, self.theta)) 252 | else: 253 | i = indices[-1] 254 | for j in self.traj.connections_forward[L_-1][i]: 255 | inner(indices + [j]) 256 | for i in range(self.traj.num_choices[0]): 257 | inner([i]) 258 | self.logZ = lse(intermediate_log_vals) 259 | # Make sure we do not overflow. 260 | if self.logZ < 100 and self.logZ > -100: 261 | self.Z = exp(self.logZ) 262 | 263 | # pylint: disable=W0201 264 | def computeGradientLogZ(self): 265 | """ Computes the gradient of the logarithm of Z. 266 | """ 267 | if 'grad_logZ' in self.__dict__: 268 | return 269 | self.computeLogZ() 270 | self.grad_Z = np.zeros_like(self.theta) 271 | # List of the log weighted gradients of all the possible trajectories. 272 | intermediate_log_grads = [] 273 | L = self.traj.L 274 | def inner(indices): 275 | """ Inner work function. """ 276 | L_ = len(indices) 277 | if L_ == L: 278 | v1 = np.zeros_like(self.theta) 279 | for l in range(L): 280 | v1 += self.traj.features_np[l][indices[l]] 281 | log_g = (dot(v1, self.theta), v1) 282 | # print(v1, self.theta, dot(v1, self.theta), g) 283 | intermediate_log_grads.append(log_g) 284 | else: 285 | i = indices[-1] 286 | if i in self.traj.connections_forward[L_-1]: 287 | for j in self.traj.connections_forward[L_-1][i]: 288 | inner(indices + [j]) 289 | for i in range(self.traj.num_choices[0]): 290 | inner([i]) 291 | (log_scale, v) = lse_vec(intermediate_log_grads) 292 | self.grad_logZ = exp(log_scale - self.logZ) * v 293 | self.log_gradZ = (log_scale, v) 294 | 295 | # pylint: disable=W0201 296 | def computeHessianLogZ(self): 297 | """ Hessian of log(Z). """ 298 | if 'hess_logZ' in self.__dict__: 299 | return 300 | L = self.traj.L 301 | self.computeLogZ() 302 | self.computeGradientLogZ() 303 | intermediate_log_hessians = [] 304 | # Inner recursive function. 305 | # The stack should be large enough for our purpose. 306 | def inner(indices): 307 | """ Inner working function. """ 308 | L_ = len(indices) 309 | if L_ == L: 310 | v1 = np.zeros_like(self.theta) 311 | for l in range(L): 312 | v1 += self.traj.features_np[l][indices[l]] 313 | h = (dot(v1, self.theta), outer(v1, v1)) 314 | intermediate_log_hessians.append(h) 315 | else: 316 | i = indices[-1] 317 | if i in self.traj.connections_forward[L_-1]: 318 | for j in self.traj.connections_forward[L_-1][i]: 319 | inner(indices + [j]) 320 | for i in range(self.traj.num_choices[0]): 321 | inner([i]) 322 | self.log_hess_Z = lse_vec(intermediate_log_hessians) 323 | (grad_log_scale, grad_vec) = self.log_gradZ 324 | (hess_log_scale, hess_vec) = self.log_hess_Z 325 | (lhess_log_scale, lhess_vec) = \ 326 | lse_vec([(hess_log_scale - self.logZ, hess_vec), 327 | (2 * grad_log_scale - 2 * self.logZ, -outer(grad_vec, 328 | grad_vec))]) 329 | self.hess_logZ = exp(lhess_log_scale) * lhess_vec 330 | 331 | class LearningElements0(LearningElementsRef): 332 | """ Naive implementation for computing the elements. 333 | 334 | This implementation is much faster than the reference implementation (linear 335 | as opposed to exponential) but is not safe against log underflows. 336 | 337 | Useful fields: 338 | - traj: a LearningTrajectory object. 339 | - theta: the weight vector 340 | - ... 341 | """ 342 | # pylint: disable=W0201 343 | def computeLogZ(self): 344 | """ 345 | Provides Zs, logZ 346 | """ 347 | if 'logZ' in self.__dict__: 348 | return 349 | L = self.traj.L 350 | # Sufficient statistics before the weight. 351 | self.w_sstats = [exp(dot(self.traj.features_np[l], self.theta)) \ 352 | for l in range(L)] 353 | self.Zs = [self.w_sstats[0]] 354 | for l in range(1, L): 355 | N_l = self.traj.num_choices[l] 356 | vec = np.zeros((N_l)) 357 | conns_back = self.traj.connections_backward[l] 358 | for i in range(N_l): 359 | if i in conns_back: 360 | for j in conns_back[i]: 361 | vec[i] += self.w_sstats[l][i] * self.Zs[l-1][j] 362 | self.Zs.append(vec) 363 | assert(len(self.Zs) == L) 364 | self.Z = sum(self.Zs[L-1]) 365 | self.logZ = log(self.Z) 366 | 367 | def computeGradientLogZ(self): 368 | if 'grad_logZ' in self.__dict__: 369 | return 370 | self.computeLogZ() 371 | N = len(self.theta) 372 | L = self.traj.L 373 | # The initial values: 374 | N_0 = self.traj.num_choices[0] 375 | grad_Zs0 = np.zeros((N_0, N)) 376 | for i in range(N_0): 377 | T_i_0 = self.traj.features_np[0][i] 378 | grad_Zs0[i] += exp(dot(T_i_0, self.theta)) * T_i_0 379 | self.grad_Zs = [grad_Zs0] 380 | # Recursion: 381 | for l in range(1, L): 382 | N_l = self.traj.num_choices[l] 383 | vec = np.zeros((N_l, N)) 384 | conns_back = self.traj.connections_backward[l] 385 | for i in range(N_l): 386 | vec[i] += self.Zs[l][i] * self.traj.features_np[l][i] 387 | if i in conns_back: 388 | for j in conns_back[i]: 389 | vec[i] += self.w_sstats[l][i] * self.grad_Zs[l-1][j] 390 | self.grad_Zs.append(vec) 391 | assert(len(self.grad_Zs) == L) 392 | self.grad_Z = sum(self.grad_Zs[L-1]) 393 | self.grad_logZ = self.grad_Z / self.Z 394 | 395 | def computeHessianLogZ(self): 396 | """ Hessian of log(Z). """ 397 | if 'hess_logZ' in self.__dict__: 398 | return 399 | self.computeLogZ() 400 | self.computeGradientLogZ() 401 | N = len(self.theta) 402 | L = self.traj.L 403 | # The initial values: 404 | N_0 = self.traj.num_choices[0] 405 | hess_Zs0 = [np.zeros((N, N)) for i in range(N_0)] 406 | for i in range(N_0): 407 | T_i_0 = self.traj.features_np[0][i] 408 | hess_Zs0[i] = exp(dot(T_i_0, self.theta)) * outer(T_i_0, T_i_0) 409 | self.hess_Zs = [hess_Zs0] 410 | # Recursion: 411 | for l in range(1, L): 412 | N_l = self.traj.num_choices[l] 413 | vec = [np.zeros((N, N)) for i in range(N_l)] 414 | conns_back = self.traj.connections_backward[l] 415 | for i in range(N_l): 416 | T_i_l = self.traj.features_np[l][i] 417 | vec[i] += self.Zs[l][i] * outer(T_i_l, T_i_l) 418 | if i in conns_back: 419 | for j in conns_back[i]: 420 | vec[i] += self.w_sstats[l][i] * self.hess_Zs[l-1][j] 421 | g_vec = np.zeros(N) 422 | for j in conns_back[i]: 423 | g_vec += self.grad_Zs[l-1][j] 424 | vec[i] += self.w_sstats[l][i] * outer(g_vec, T_i_l) 425 | vec[i] += self.w_sstats[l][i] * outer(T_i_l, g_vec) 426 | self.hess_Zs.append(vec) 427 | assert(len(self.hess_Zs) == L) 428 | self.hess_Z = sum(self.hess_Zs[L-1]) 429 | self.hess_logZ = self.hess_Z / self.Z \ 430 | - outer(self.grad_Z, self.grad_Z) / (self.Z * self.Z) 431 | 432 | class LearningElements2(LearningElementsRef): 433 | """ Safe implementation for computing the elements, optimized for Numpy. 434 | 435 | This implementation is faster and much less readable than LearningElements1, 436 | so do not try to use it to reimplement an algorithm. 437 | 438 | Useful fields: 439 | - traj: a LearningTrajectory object. 440 | - theta: the weight vector 441 | - ... 442 | """ 443 | # pylint: disable=W0201 444 | def computeLogZ(self): 445 | """ 446 | Provides Zs, logZ 447 | """ 448 | if 'logZ' in self.__dict__: 449 | return 450 | L = self.traj.L 451 | self.logZs = [dot(self.traj.features_np[0], self.theta)] 452 | assert not np.isnan(self.logZs[0]).any(), \ 453 | (self.traj.features_np[0], self.theta) 454 | for l in range(1, L): 455 | N_l = self.traj.num_choices[l] 456 | l_vec = MINUS_INF * np.ones((N_l)) 457 | conns_back = self.traj.connections_backward[l] 458 | w = dot(self.traj.features_np[l], self.theta) 459 | assert not np.isnan(w).any() 460 | for i in range(N_l): 461 | if i in conns_back: 462 | l_vec[i] = lse_npy(w[i] + self.logZs[l-1][conns_back[i]]) 463 | assert not np.isnan(l_vec[i]).any() 464 | assert not np.isnan(l_vec).any() 465 | self.logZs.append(l_vec) 466 | assert(len(self.logZs) == L) 467 | self.logZ = lse_npy(self.logZs[L-1]) 468 | assert not np.isnan(self.logZ).any() 469 | # Make sure we do not overflow. 470 | if self.logZ < 100 and self.logZ > -100: 471 | self.Z = exp(self.logZ) 472 | 473 | def computeGradientLogZ(self): 474 | if 'grad_logZ' in self.__dict__: 475 | return 476 | self.computeLogZ() 477 | N = len(self.theta) 478 | L = self.traj.L 479 | assert not np.isnan(self.theta).any() 480 | log_grad_Zs0_dirs = self.traj.features_np[0] 481 | log_grad_Zs0_norms = dot(self.traj.features_np[0], self.theta) 482 | self.log_grad_Zs_norms = [log_grad_Zs0_norms] 483 | self.log_grad_Zs_dirs = [log_grad_Zs0_dirs] 484 | # Recursion: 485 | for l in range(1, L): 486 | N_l = self.traj.num_choices[l] 487 | l_vec_dirs = np.zeros((N_l, N)) 488 | l_vec_norms = MINUS_INF * np.ones(N_l) 489 | conns_back = self.traj.connections_backward[l] 490 | w = dot(self.traj.features_np[l], self.theta) 491 | assert not np.isnan(w).any() 492 | for i in range(N_l): 493 | vs0_dir = np.array([self.traj.features_np[l][i]]) 494 | vs0_norm = np.array([self.logZs[l][i]]) 495 | if i not in conns_back: 496 | (n, d) = lse_vec_npy(vs0_dir, vs0_norm) 497 | l_vec_dirs[i] = d 498 | l_vec_norms[i] = n 499 | else: 500 | vs_dirs = np.vstack((vs0_dir, \ 501 | self.log_grad_Zs_dirs[l-1][conns_back[i]])) 502 | vs_norms = np.hstack((vs0_norm, \ 503 | w[i] + \ 504 | self.log_grad_Zs_norms[l-1][conns_back[i]])) 505 | (n, d) = lse_vec_npy(vs_dirs, vs_norms) 506 | l_vec_dirs[i] = d 507 | l_vec_norms[i] = n 508 | self.log_grad_Zs_norms.append(l_vec_norms) 509 | self.log_grad_Zs_dirs.append(l_vec_dirs) 510 | assert(len(self.log_grad_Zs_norms) == L) 511 | self.log_grad_Z = lse_vec_npy(self.log_grad_Zs_dirs[L-1], 512 | self.log_grad_Zs_norms[L-1]) 513 | (l_norm, v) = self.log_grad_Z 514 | if l_norm < 100 and l_norm > -100: 515 | self.grad_Z = exp(l_norm) * v 516 | self.grad_logZ = exp(l_norm - self.logZ) * v 517 | 518 | def computeHessianLogZ(self): 519 | """ Hessian of log(Z). """ 520 | if 'hess_logZ' in self.__dict__: 521 | return 522 | inf = float('inf') 523 | self.computeLogZ() 524 | self.computeGradientLogZ() 525 | N = len(self.theta) 526 | L = self.traj.L 527 | # The initial values: 528 | N_0 = self.traj.num_choices[0] 529 | log_hess_Zs0_norms = np.dot(self.traj.features_np[0], self.theta) 530 | log_hess_Zs0_dirs = np.zeros((N_0, N, N)) 531 | for i in range(N_0): 532 | log_hess_Zs0_dirs[i] = np.outer(self.traj.features_np[0][i], \ 533 | self.traj.features_np[0][i]) 534 | self.log_hess_Zs_norms = [log_hess_Zs0_norms] 535 | self.log_hess_Zs_dirs = [log_hess_Zs0_dirs] 536 | # Recursion: 537 | for l in range(1, L): 538 | N_l = self.traj.num_choices[l] 539 | l_vec_norm = -inf * np.ones(N_l) 540 | l_vec_dir = np.zeros((N_l, N, N)) 541 | conns_back = self.traj.connections_backward[l] 542 | w = dot(self.traj.features_np[l], self.theta) 543 | for i in range(N_l): 544 | T_i_l = self.traj.features_np[l][i] 545 | vs0_norm = np.array([self.logZs[l][i]]) 546 | vs0_dir = np.array([outer(T_i_l, T_i_l)]) 547 | if i in conns_back: 548 | us_norm = self.log_grad_Zs_norms[l-1][conns_back[i]] 549 | us_dir = self.log_grad_Zs_dirs[l-1][conns_back[i]] 550 | (l_norm, u_g_vec) = lse_vec_npy(us_dir, us_norm) 551 | vs_norm = np.hstack((vs0_norm, \ 552 | w[i] + \ 553 | self.log_hess_Zs_norms[l-1][conns_back[i]], \ 554 | w[i] + l_norm, \ 555 | w[i] + l_norm)) 556 | M = np.array([outer(u_g_vec, T_i_l)]) 557 | Mt = np.array([outer(T_i_l, u_g_vec)]) 558 | vs_dir = np.vstack((vs0_dir, \ 559 | self.log_hess_Zs_dirs[l-1][conns_back[i]], \ 560 | M, Mt)) 561 | (li_vec_norm, li_vec_dir) = lse_vec_npy(vs_dir, vs_norm) 562 | l_vec_norm[i] = li_vec_norm 563 | l_vec_dir[i] = li_vec_dir 564 | else: 565 | l_vec_norm[i] = vs0_norm 566 | l_vec_dir[i] = vs0_dir 567 | self.log_hess_Zs_dirs.append(l_vec_dir) 568 | self.log_hess_Zs_norms.append(l_vec_norm) 569 | assert(len(self.log_hess_Zs_dirs) == L) 570 | self.log_hess_Z = lse_vec_npy(self.log_hess_Zs_dirs[-1], \ 571 | self.log_hess_Zs_norms[-1]) 572 | (l_norm, h) = self.log_hess_Z 573 | if l_norm < 100 and l_norm > -100: 574 | self.hess_Z = exp(l_norm) * h 575 | (l_norm_g, g) = self.log_grad_Z 576 | self.hess_logZ = np.zeros_like(h) 577 | if l_norm - self.logZ > -60: 578 | self.hess_logZ += exp(l_norm - self.logZ) * h 579 | if l_norm_g - self.logZ > -30: 580 | self.hess_logZ -= exp(2 * l_norm_g - 2 * self.logZ) * outer(g, g) 581 | 582 | 583 | class LearningElements2_(LearningElementsRef): 584 | """ Safe implementation for computing the elements, optimized for Numpy. 585 | 586 | This implementation is faster and much less readable than LearningElements1, 587 | so do not try to use it to reimplement an algorithm. 588 | 589 | Useful fields: 590 | - traj: a LearningTrajectory object. 591 | - theta: the weight vector 592 | - ... 593 | """ 594 | # pylint: disable=W0201 595 | def computeLogZ(self): 596 | """ 597 | Provides Zs, logZ 598 | """ 599 | if 'logZ' in self.__dict__: 600 | return 601 | L = self.traj.L 602 | self.logZs = [dot(self.traj.features_np[0], self.theta)] 603 | assert not np.isnan(self.logZs[0]).any(), \ 604 | (self.traj.features_np[0], self.theta) 605 | for l in range(1, L): 606 | N_l = self.traj.num_choices[l] 607 | l_vec = MINUS_INF * np.ones((N_l)) 608 | conns_back = self.traj.connections_backward[l] 609 | w = dot(self.traj.features_np[l], self.theta) 610 | assert not np.isnan(w).any() 611 | for i in range(N_l): 612 | if i in conns_back: 613 | l_vec[i] = lse_npy(w[i] + self.logZs[l-1][conns_back[i]]) 614 | assert not np.isnan(l_vec[i]).any() 615 | assert not np.isnan(l_vec).any() 616 | self.logZs.append(l_vec) 617 | assert(len(self.logZs) == L) 618 | self.logZ = lse_npy(self.logZs[L-1]) 619 | assert not np.isnan(self.logZ).any() 620 | # Make sure we do not overflow. 621 | if self.logZ < 100 and self.logZ > -100: 622 | self.Z = exp(self.logZ) 623 | 624 | def computeGradientLogZ(self): 625 | if 'grad_logZ' in self.__dict__: 626 | return 627 | self.computeLogZ() 628 | N = len(self.theta) 629 | L = self.traj.L 630 | assert not np.isnan(self.theta).any() 631 | log_grad_Zs0_dirs = self.traj.features_np[0] 632 | log_grad_Zs0_norms = dot(self.traj.features_np[0], self.theta) 633 | self.log_grad_Zs_norms = [log_grad_Zs0_norms] 634 | self.log_grad_Zs_dirs = [log_grad_Zs0_dirs] 635 | # Recursion: 636 | for l in range(1, L): 637 | N_l = self.traj.num_choices[l] 638 | l_vec_dirs = np.zeros((N_l, N)) 639 | l_vec_norms = MINUS_INF * np.ones(N_l) 640 | conns_back = self.traj.connections_backward[l] 641 | w = dot(self.traj.features_np[l], self.theta) 642 | assert not np.isnan(w).any() 643 | for i in range(N_l): 644 | vs0_dir = [self.traj.features_np[l][i]] 645 | vs0_norm = [self.logZs[l][i]] 646 | if i not in conns_back: 647 | (n, d) = lse_vec_2(vs0_dir, vs0_norm) 648 | l_vec_dirs[i] = d 649 | l_vec_norms[i] = n 650 | else: 651 | vs_dirs = [vs0_dir, \ 652 | self.log_grad_Zs_dirs[l-1][conns_back[i]]] 653 | vs_norms = [vs0_norm, \ 654 | w[i] + \ 655 | self.log_grad_Zs_norms[l-1][conns_back[i]]] 656 | (n, d) = lse_vec_2(vs_dirs, vs_norms) 657 | l_vec_dirs[i] = d 658 | l_vec_norms[i] = n 659 | self.log_grad_Zs_norms.append(l_vec_norms) 660 | self.log_grad_Zs_dirs.append(l_vec_dirs) 661 | assert(len(self.log_grad_Zs_norms) == L) 662 | self.log_grad_Z = lse_vec_2(self.log_grad_Zs_dirs[L-1], 663 | self.log_grad_Zs_norms[L-1]) 664 | (l_norm, v) = self.log_grad_Z 665 | if l_norm < 100 and l_norm > -100: 666 | self.grad_Z = exp(l_norm) * v 667 | self.grad_logZ = exp(l_norm - self.logZ) * v 668 | 669 | def computeHessianLogZ(self): 670 | """ Hessian of log(Z). """ 671 | if 'hess_logZ' in self.__dict__: 672 | return 673 | inf = float('inf') 674 | self.computeLogZ() 675 | self.computeGradientLogZ() 676 | N = len(self.theta) 677 | L = self.traj.L 678 | # The initial values: 679 | N_0 = self.traj.num_choices[0] 680 | log_hess_Zs0_norms = np.dot(self.traj.features_np[0], self.theta) 681 | log_hess_Zs0_dirs = np.zeros((N_0, N, N)) 682 | for i in range(N_0): 683 | log_hess_Zs0_dirs[i] = np.outer(self.traj.features_np[0][i], \ 684 | self.traj.features_np[0][i]) 685 | self.log_hess_Zs_norms = [log_hess_Zs0_norms] 686 | self.log_hess_Zs_dirs = [log_hess_Zs0_dirs] 687 | # Recursion: 688 | for l in range(1, L): 689 | N_l = self.traj.num_choices[l] 690 | l_vec_norm = -inf * np.ones(N_l) 691 | l_vec_dir = np.zeros((N_l, N, N)) 692 | conns_back = self.traj.connections_backward[l] 693 | w = dot(self.traj.features_np[l], self.theta) 694 | for i in range(N_l): 695 | T_i_l = self.traj.features_np[l][i] 696 | vs0_norm = np.array([self.logZs[l][i]]) 697 | vs0_dir = np.array([outer(T_i_l, T_i_l)]) 698 | if i in conns_back: 699 | us_norm = self.log_grad_Zs_norms[l-1][conns_back[i]] 700 | us_dir = self.log_grad_Zs_dirs[l-1][conns_back[i]] 701 | (l_norm, u_g_vec) = lse_vec_2(us_dir, us_norm) 702 | vs_norm = (vs0_norm, \ 703 | w[i] + \ 704 | self.log_hess_Zs_norms[l-1][conns_back[i]], \ 705 | w[i] + l_norm, \ 706 | w[i] + l_norm) 707 | M = np.array([outer(u_g_vec, T_i_l)]) 708 | Mt = np.array([outer(T_i_l, u_g_vec)]) 709 | vs_dir = (vs0_dir, \ 710 | self.log_hess_Zs_dirs[l-1][conns_back[i]], \ 711 | M, Mt) 712 | (li_vec_norm, li_vec_dir) = lse_vec_2(vs_dir, vs_norm) 713 | l_vec_norm[i] = li_vec_norm 714 | l_vec_dir[i] = li_vec_dir 715 | else: 716 | l_vec_norm[i] = vs0_norm 717 | l_vec_dir[i] = vs0_dir 718 | self.log_hess_Zs_dirs.append(l_vec_dir) 719 | self.log_hess_Zs_norms.append(l_vec_norm) 720 | assert(len(self.log_hess_Zs_dirs) == L) 721 | self.log_hess_Z = lse_vec_2(self.log_hess_Zs_dirs[-1], \ 722 | self.log_hess_Zs_norms[-1]) 723 | (l_norm, h) = self.log_hess_Z 724 | if l_norm < 100 and l_norm > -100: 725 | self.hess_Z = exp(l_norm) * h 726 | (l_norm_g, g) = self.log_grad_Z 727 | self.hess_logZ = np.zeros_like(h) 728 | if l_norm - self.logZ > -60: 729 | self.hess_logZ += exp(l_norm - self.logZ) * h 730 | if l_norm_g - self.logZ > -30: 731 | self.hess_logZ -= exp(2 * l_norm_g - 2 * self.logZ) * outer(g, g) 732 | 733 | class LearningElements1(LearningElementsRef): 734 | """ Safe implementation for computing the elements. 735 | 736 | This implementation performs all computations in log domain. It is slightly 737 | slower than the reference fast implementation but works for a much larger 738 | domain of feature values. 739 | 740 | Useful fields: 741 | - traj: a LearningTrajectory object. 742 | - theta: the weight vector 743 | - ... 744 | """ 745 | 746 | # pylint: disable=W0201 747 | def computeValue(self): 748 | """ 749 | Calls LogZ. 750 | Provides logValue 751 | """ 752 | if 'logValue' in self.__dict__: 753 | return 754 | self.computeSStats() 755 | self.computeLogV1() 756 | self.computeLogZ() 757 | L = self.traj.L 758 | self.logValues = [dot(self.traj.features_np[0], 759 | self.theta) - dot(self.sstats[0], self.theta)] 760 | assert not np.isnan(self.logValues[0]).any(), \ 761 | (self.traj.features_np[0], self.theta) 762 | assert lse(self.logValues[0]) >= 0 763 | for l in range(1, L): 764 | N_l = self.traj.num_choices[l] 765 | l_vec = MINUS_INF * np.ones((N_l)) 766 | conns_back = self.traj.connections_backward[l] 767 | w = dot(self.traj.features_np[l], 768 | self.theta) - dot(self.sstats[l], self.theta) 769 | assert not np.isnan(w).any() 770 | for i in range(N_l): 771 | if i in conns_back: 772 | l_vec[i] = w[i] + lse([self.logValues[l-1][j] \ 773 | for j in conns_back[i]]) 774 | assert not np.isnan(l_vec[i]).any() 775 | self.logValues.append(l_vec) 776 | # assert(lse(l_vec) >= -1e-2) 777 | assert not np.isnan(l_vec).any() 778 | assert(len(self.logValues) == L) 779 | self.logValue = -lse(self.logValues[L-1]) 780 | assert not np.isnan(self.logZ).any() 781 | # assert self.logV1 <= self.logZ, (self.logV1, self.logZ, self.sstats) 782 | 783 | 784 | def computeLogV1(self): 785 | """ Log V1. 786 | """ 787 | if 'logV1' in self.__dict__: 788 | return 789 | self.computeSStats() 790 | L = self.traj.L 791 | # Expected sufficient statistics: 792 | self.logV1 = sum([dot(self.sstats[l], self.theta) for l in range(L)]) 793 | 794 | # pylint: disable=W0201 795 | def computeLogZ(self): 796 | """ 797 | Provides Zs, logZ 798 | """ 799 | if 'logZ' in self.__dict__: 800 | return 801 | self.computeLogV1() 802 | L = self.traj.L 803 | self.logZs = [dot(self.traj.features_np[0], self.theta)] 804 | assert not np.isnan(self.logZs[0]).any(), \ 805 | (self.traj.features_np[0], self.theta) 806 | for l in range(1, L): 807 | N_l = self.traj.num_choices[l] 808 | l_vec = MINUS_INF * np.ones((N_l)) 809 | conns_back = self.traj.connections_backward[l] 810 | w = dot(self.traj.features_np[l], self.theta) 811 | assert not np.isnan(w).any() 812 | for i in range(N_l): 813 | if i in conns_back: 814 | l_vec[i] = w[i] + lse([self.logZs[l-1][j] for j in conns_back[i]]) 815 | assert not np.isnan(l_vec[i]).any() 816 | assert not np.isnan(l_vec).any() 817 | self.logZs.append(l_vec) 818 | assert(len(self.logZs) == L) 819 | self.logZ = lse(self.logZs[L-1]) 820 | assert not np.isnan(self.logZ).any() 821 | # Make sure we do not overflow. 822 | if self.logZ < 100 and self.logZ > -100: 823 | self.Z = exp(self.logZ) 824 | 825 | def computeGradientLogZ(self): 826 | if 'grad_logZ' in self.__dict__: 827 | return 828 | self.computeLogZ() 829 | inf = float('inf') 830 | N = len(self.theta) 831 | L = self.traj.L 832 | # The initial values: 833 | N_0 = self.traj.num_choices[0] 834 | assert not np.isnan(self.theta).any() 835 | log_grad_Zs0 = [(dot(self.traj.features_np[0][i], self.theta), \ 836 | self.traj.features_np[0][i]) for i in range(N_0)] 837 | self.log_grad_Zs = [log_grad_Zs0] 838 | # Recursion: 839 | for l in range(1, L): 840 | N_l = self.traj.num_choices[l] 841 | l_vec = [(-inf, np.zeros(N)) for i in range(N_l)] 842 | conns_back = self.traj.connections_backward[l] 843 | w = dot(self.traj.features_np[l], self.theta) 844 | assert not np.isnan(w).any() 845 | for i in range(N_l): 846 | vs = [(self.logZs[l][i], self.traj.features_np[l][i])] 847 | assert not np.isnan(vs[0][0]).any() 848 | assert not np.isnan(vs[0][1]).any() 849 | if i in conns_back: 850 | for j in conns_back[i]: 851 | (l_norm, v) = self.log_grad_Zs[l-1][j] 852 | assert not np.isnan(v).any() 853 | assert not np.isnan(l_norm).any() 854 | vs.append((w[i] + l_norm, v)) 855 | l_vec[i] = lse_vec(vs) 856 | assert not np.isnan(l_vec[i][0]).any(), (l_vec[i], vs) 857 | assert not np.isnan(l_vec[i][1]).any(), (l_vec[i], vs) 858 | self.log_grad_Zs.append(l_vec) 859 | assert(len(self.log_grad_Zs) == L) 860 | self.log_grad_Z = lse_vec(self.log_grad_Zs[L-1]) 861 | (l_norm, v) = self.log_grad_Z 862 | if l_norm < 100 and l_norm > -100: 863 | self.grad_Z = exp(l_norm) * v 864 | self.grad_logZ = exp(l_norm - self.logZ) * v 865 | 866 | def computeHessianLogZ(self): 867 | """ Hessian of log(Z). """ 868 | if 'hess_logZ' in self.__dict__: 869 | return 870 | inf = float('inf') 871 | self.computeLogZ() 872 | self.computeGradientLogZ() 873 | N = len(self.theta) 874 | L = self.traj.L 875 | # The initial values: 876 | N_0 = self.traj.num_choices[0] 877 | log_hess_Zs0 = [] 878 | for i in range(N_0): 879 | T_i_0 = self.traj.features_np[0][i] 880 | log_hess_Zs0.append((dot(T_i_0, self.theta), outer(T_i_0, T_i_0))) 881 | self.log_hess_Zs = [log_hess_Zs0] 882 | # Recursion: 883 | for l in range(1, L): 884 | N_l = self.traj.num_choices[l] 885 | l_vec = [(-inf, np.zeros((N, N))) for i in range(N_l)] 886 | conns_back = self.traj.connections_backward[l] 887 | w = dot(self.traj.features_np[l], self.theta) 888 | for i in range(N_l): 889 | T_i_l = self.traj.features_np[l][i] 890 | vs = [(self.logZs[l][i], outer(T_i_l, T_i_l))] 891 | if i in conns_back: 892 | assert conns_back[i], (i, conns_back) 893 | for j in conns_back[i]: 894 | (l_norm, h) = self.log_hess_Zs[l-1][j] 895 | vs.append((w[i] + l_norm, h)) 896 | log_g_vec = lse_vec([self.log_grad_Zs[l-1][j] for j in conns_back[i]]) 897 | (l_norm, u_g_vec) = log_g_vec 898 | vs.append((w[i] + l_norm, outer(u_g_vec, T_i_l))) 899 | vs.append((w[i] + l_norm, outer(T_i_l, u_g_vec))) 900 | l_vec[i] = lse_vec(vs) 901 | self.log_hess_Zs.append(l_vec) 902 | assert(len(self.log_hess_Zs) == L) 903 | self.log_hess_Z = lse_vec(self.log_hess_Zs[-1]) 904 | (l_norm, h) = self.log_hess_Z 905 | if l_norm < 100 and l_norm > -100: 906 | self.hess_Z = exp(l_norm) * h 907 | (l_norm_g, g) = self.log_grad_Z 908 | self.hess_logZ = exp(l_norm - self.logZ) * h\ 909 | - exp(2 * l_norm_g - 2 * self.logZ) * outer(g, g) 910 | -------------------------------------------------------------------------------- /mm/path_inference/learning_traj_elements_test.py: -------------------------------------------------------------------------------- 1 | ''' 2 | Copyright 2011, 2012 Timothy Hunter 3 | 4 | This library is free software; you can redistribute it and/or 5 | modify it under the terms of the GNU Lesser General Public 6 | License as published by the Free Software Foundation; either 7 | version 2.1 of the License, or (at your option) version 3. 8 | 9 | This library is distributed in the hope that it will be useful, 10 | but WITHOUT ANY WARRANTY; without even the implied warranty of 11 | MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU 12 | Lesser General Public License for more details. 13 | 14 | You should have received a copy of the GNU Lesser General Public 15 | License along with this library. If not, see . 16 | ''' 17 | # pylint: disable=W0105 18 | """ Test module. 19 | Author: Timothy Hunter 20 | 21 | Most of these tests correspond to implementation mistakes. 22 | """ 23 | from mm.path_inference.learning_traj_test import simple_traj1, \ 24 | simple_traj2, simple_traj4, simple_traj0, simple_traj5, simple_traj8,\ 25 | simple_traj10 26 | from mm.path_inference.learning_traj_elements \ 27 | import LearningElements0 as LearningElements 28 | from mm.path_inference.learning_traj_elements \ 29 | import LearningElements1 as LearningElementsSecure 30 | from mm.path_inference.learning_traj_elements import LearningElementsRef 31 | import numpy as np 32 | # Forces Numpy to consider warnings as errors. 33 | np.seterr(all='raise') 34 | 35 | def within(x1, x2, epsi): 36 | """ Epsilon equality. 37 | """ 38 | return (abs(x1-x2) < epsi) 39 | 40 | def compute_grad(fun, x, dx=1e-8): 41 | """ EMpirical gradient. """ 42 | N = len(x) 43 | res = np.zeros_like(x) 44 | fx = fun(x) 45 | for i in range(N): 46 | x2 = np.array(x) 47 | x2[i] += dx 48 | fxdx = fun(x2) 49 | res[i] = (fxdx - fx) / dx 50 | return res 51 | 52 | def compute_hess(fun, x, dx=1e-4): 53 | """ Empirical hessian. """ 54 | N = len(x) 55 | res = np.zeros((N, N), dtype=np.float64) 56 | for i in range(N): 57 | def fun2(y): 58 | """ Inner function as closure. """ 59 | g = compute_grad(fun, y, dx) 60 | return g[i] 61 | res[i] = compute_grad(fun2, x, dx) 62 | return res 63 | 64 | def test_emp_hessian_1(): 65 | """ test_emp_hessian_1 """ 66 | def f0(x): 67 | """ Inner. """ 68 | return np.dot(x, x) / 2 69 | # from mm.path_inference.learning_traj_elements_test import * 70 | x_0 = np.array([10.0, -100.0]) 71 | h_ref = np.array([[1, 0], [0, 1]], dtype=np.float64) 72 | h = compute_hess(f0, x_0) 73 | assert(np.abs(h - h_ref).max() < 1e-3), (h, h_ref) 74 | 75 | def get_fun_ref(traj, choices): 76 | """ Reference closure for logZ. """ 77 | def res(theta): 78 | """ Inner closure. """ 79 | elts = LearningElementsRef(traj, theta, choices) 80 | elts.computeLogZ() 81 | return elts.logZ 82 | return res 83 | 84 | def get_fun_ref0(traj, choices): 85 | """ Reference closure for Z. """ 86 | def res(theta): 87 | """ Inner closure. """ 88 | elts = LearningElementsRef(traj, theta, choices) 89 | elts.computeLogZ() 90 | return elts.Z 91 | return res 92 | 93 | def get_grad_ref(traj, choices): 94 | """ Reference closure for the gradient. """ 95 | def res(theta): 96 | """ inner closure. """ 97 | elts = LearningElementsRef(traj, theta, choices) 98 | elts.computeGradientLogZ() 99 | return elts.grad_logZ 100 | return res 101 | 102 | def get_hess_ref(traj, choices): 103 | """ Reference closure for the gradient. """ 104 | def res(theta): 105 | """ inner closure. """ 106 | elts = LearningElementsRef(traj, theta, choices) 107 | elts.computeHessianLogZ() 108 | return elts.hess_logZ 109 | return res 110 | 111 | def get_hess_ref0(traj, choices): 112 | """ Reference closure for the hessian of Z. """ 113 | def res(theta): 114 | """ inner closure. """ 115 | elts = LearningElementsRef(traj, theta, choices) 116 | elts.computeHessianLogZ() 117 | return elts.hess_Z 118 | return res 119 | 120 | def test_gradient_ref1(): 121 | """ Simple test to check that the reference implementation of the gradient 122 | is equal to a empirical definition based on logZ. 123 | """ 124 | traj = simple_traj1() 125 | choices = [1, 0] 126 | fun = get_fun_ref(traj, choices) 127 | grad_fun = get_grad_ref(traj, choices) 128 | # Do not forget to define the vector as floating point numbers!!! 129 | theta = np.array([0.0, 0.0]) 130 | g_emp = compute_grad(fun, theta) 131 | g = grad_fun(theta) 132 | assert(np.abs(g - g_emp).max() < 1e-3), (g, g_emp) 133 | 134 | def test_gradient_ref2(): 135 | """ Test at a different point. """ 136 | traj = simple_traj1() 137 | choices = [1, 0] 138 | fun = get_fun_ref(traj, choices) 139 | grad_fun = get_grad_ref(traj, choices) 140 | theta = np.array([1.0, -1.0]) 141 | g_emp = compute_grad(fun, theta) 142 | g = grad_fun(theta) 143 | assert(np.abs(g - g_emp).max() < 1e-3), (g, g_emp) 144 | 145 | def test_gradient_ref_traj4(): 146 | """ test_gradient_ref_traj4 147 | Simple test to check that the reference implementation of the gradient 148 | is equal to a empirical definition based on logZ. 149 | """ 150 | traj = simple_traj4() 151 | choices = [1, 0, 2] 152 | fun = get_fun_ref(traj, choices) 153 | grad_fun = get_grad_ref(traj, choices) 154 | # Do not forget to define the vector as floating point numbers!!! 155 | theta = np.array([0.0, 0.0]) 156 | g_emp = compute_grad(fun, theta) 157 | g = grad_fun(theta) 158 | assert(np.abs(g - g_emp).max() < 1e-3), (g, g_emp) 159 | 160 | def test_gradient_ref_traj0_1(): 161 | """ test_gradient_ref_traj0_1. """ 162 | traj = simple_traj0() 163 | choices = [1, 0] 164 | fun = get_fun_ref(traj, choices) 165 | grad_fun = get_grad_ref(traj, choices) 166 | # Do not forget to define the vector as floating point numbers!!! 167 | theta = np.array([1.0]) 168 | g_emp = compute_grad(fun, theta) 169 | g = grad_fun(theta) 170 | assert(np.abs(g - g_emp).max() < 1e-3), (g, g_emp) 171 | 172 | def test_gradient_ref_traj5_1(): 173 | """ test_gradient_ref_traj5_1. """ 174 | traj = simple_traj5() 175 | choices = [1, 0, 2] 176 | fun = get_fun_ref(traj, choices) 177 | grad_fun = get_grad_ref(traj, choices) 178 | # Do not forget to define the vector as floating point numbers!!! 179 | theta = np.array([1.0]) 180 | g_emp = compute_grad(fun, theta) 181 | g = grad_fun(theta) 182 | assert(np.abs(g - g_emp).max() < 1e-3), (g, g_emp) 183 | 184 | 185 | def test_hessian_ref1(): 186 | """ Simple test to check that the reference implementation of the hessian 187 | is equal to a empirical definition based on logZ. 188 | """ 189 | traj = simple_traj1() 190 | choices = [1, 0] 191 | fun = get_fun_ref(traj, choices) 192 | hess_fun = get_hess_ref(traj, choices) 193 | # Do not forget to define the vector as floating point numbers!!! 194 | theta = np.array([0.0, 0.0]) 195 | h_emp = compute_hess(fun, theta) 196 | h = hess_fun(theta) 197 | assert(np.abs(h - h_emp).max() < 1e-3), (h, h_emp) 198 | 199 | def test_hessian_ref2(): 200 | """ Simple test to check that the reference implementation of the hessian 201 | is equal to a empirical definition based on logZ. 202 | """ 203 | traj = simple_traj1() 204 | choices = [1, 0] 205 | # Do not forget to define the vector as floating point numbers!!! 206 | theta = np.array([1.0, -1.0]) 207 | # Z 208 | fun = get_fun_ref0(traj, choices) 209 | hess_fun = get_hess_ref0(traj, choices) 210 | h_emp = compute_hess(fun, theta) 211 | h = hess_fun(theta) 212 | assert(np.abs(h - h_emp).max() < 1e-3), (h, h_emp) 213 | # LogZ 214 | fun = get_fun_ref(traj, choices) 215 | hess_fun = get_hess_ref(traj, choices) 216 | h_emp = compute_hess(fun, theta) 217 | h = hess_fun(theta) 218 | assert(np.abs(h - h_emp).max() < 1e-3), (h, h_emp) 219 | 220 | def test_hessian_ref3(): 221 | """ Simple test to check that the reference implementation of the hessian 222 | is equal to a empirical definition based on logZ. 223 | """ 224 | traj = simple_traj1() 225 | choices = [0, 0] 226 | # Do not forget to define the vector as floating point numbers!!! 227 | theta = np.array([-0.1, 0.1]) 228 | # Z 229 | fun = get_fun_ref0(traj, choices) 230 | hess_fun = get_hess_ref0(traj, choices) 231 | h_emp = compute_hess(fun, theta) 232 | h = hess_fun(theta) 233 | assert(np.abs(h - h_emp).max() < 1e-3), (h, h_emp) 234 | # LogZ 235 | fun = get_fun_ref(traj, choices) 236 | hess_fun = get_hess_ref(traj, choices) 237 | h_emp = compute_hess(fun, theta) 238 | h = hess_fun(theta) 239 | assert(np.abs(h - h_emp).max() < 1e-3), (h, h_emp) 240 | 241 | def test_hessian_ref_traj5_1(): 242 | """ test_hessian_ref_traj5_1 243 | 244 | Simple test to check that the reference implementation of the hessian 245 | is equal to a empirical definition based on logZ. 246 | """ 247 | traj = simple_traj5() 248 | choices = [1, 0, 2] 249 | # Do not forget to define the vector as floating point numbers!!! 250 | theta = np.array([1.0]) 251 | # Z 252 | fun = get_fun_ref0(traj, choices) 253 | hess_fun = get_hess_ref0(traj, choices) 254 | h_emp = compute_hess(fun, theta) 255 | h = hess_fun(theta) 256 | assert(np.abs(h - h_emp).max() < 1e-2), (h, h_emp) 257 | # LogZ 258 | fun = get_fun_ref(traj, choices) 259 | hess_fun = get_hess_ref(traj, choices) 260 | h_emp = compute_hess(fun, theta) 261 | h = hess_fun(theta) 262 | assert(np.abs(h - h_emp).max() < 1e-3), (h, h_emp) 263 | 264 | def test_Z1(): 265 | """ Test of implementation 1. """ 266 | traj = simple_traj1() 267 | theta = np.array([0.0, -1.0]) 268 | choices = [1, 0] 269 | elts = LearningElements(traj, theta, choices) 270 | elts.computeLogZ() 271 | elts_ref = LearningElementsRef(traj, theta, choices) 272 | elts_ref.computeLogZ() 273 | assert(within(elts.logZ, elts_ref.logZ, 1e-5)) 274 | 275 | def test_traj_5_1(): 276 | """ test_traj_5_1 """ 277 | traj = simple_traj5() 278 | theta = np.array([-1.0]) 279 | choices = [1, 0, 2] 280 | elts = LearningElements(traj, theta, choices) 281 | elts.computeLogZ() 282 | elts_ref = LearningElementsRef(traj, theta, choices) 283 | elts_ref.computeLogZ() 284 | assert(within(elts.Z, elts_ref.Z, 1e-5)), (elts.Z, elts_ref.Z, 1e-5) 285 | assert(within(elts.logZ, elts_ref.logZ, 1e-5)), \ 286 | (elts.logZ, elts_ref.logZ, 1e-5) 287 | 288 | def test_traj_5_2(): 289 | """ test_traj_5_2 """ 290 | traj = simple_traj5() 291 | theta = np.array([-1.0]) 292 | choices = [1, 0, 2] 293 | elts = LearningElementsSecure(traj, theta, choices) 294 | elts.computeLogZ() 295 | elts_ref = LearningElementsRef(traj, theta, choices) 296 | elts_ref.computeLogZ() 297 | assert(within(elts.Z, elts_ref.Z, 1e-5)), (elts.Z, elts_ref.Z, 1e-5) 298 | assert(within(elts.logZ, elts_ref.logZ, 1e-5)), \ 299 | (elts.logZ, elts_ref.logZ, 1e-5) 300 | 301 | def test_traj_1_2(): 302 | """ test_traj_1_2 """ 303 | traj = simple_traj1() 304 | theta = np.array([1.0, -1.0]) 305 | choices = [1, 0] 306 | elts = LearningElementsSecure(traj, theta, choices) 307 | elts.computeLogZ() 308 | elts_ref = LearningElementsRef(traj, theta, choices) 309 | elts_ref.computeLogZ() 310 | assert(within(elts.Z, elts_ref.Z, 1e-5)), (elts.Z, elts_ref.Z, 1e-5) 311 | assert(within(elts.logZ, elts_ref.logZ, 1e-5)), \ 312 | (elts.logZ, elts_ref.logZ, 1e-5) 313 | 314 | def test_grad_traj1_1(): 315 | """ test_grad_traj1_1 316 | Test of implementation 1 of gradient. """ 317 | traj = simple_traj1() 318 | theta = np.array([0.0, -1.0]) 319 | choices = [1, 0] 320 | elts = LearningElements(traj, theta, choices) 321 | elts.computeGradientLogZ() 322 | elts_ref = LearningElementsRef(traj, theta, choices) 323 | elts_ref.computeGradientLogZ() 324 | g = elts.grad_logZ 325 | g_ref = elts_ref.grad_logZ 326 | assert(np.abs(g - g_ref).max() < 1e-3), (g, g_ref) 327 | 328 | def test_grad_traj1_2(): 329 | """ test_grad_traj1_2 330 | Test of implementation 1 of gradient. """ 331 | traj = simple_traj1() 332 | theta = np.array([0.0, -1.0]) 333 | choices = [1, 0] 334 | elts = LearningElementsSecure(traj, theta, choices) 335 | elts.computeGradientLogZ() 336 | elts_ref = LearningElementsRef(traj, theta, choices) 337 | elts_ref.computeGradientLogZ() 338 | g = elts.grad_logZ 339 | g_ref = elts_ref.grad_logZ 340 | assert(np.abs(g - g_ref).max() < 1e-3), (g, g_ref) 341 | 342 | def test_grad_traj5_2(): 343 | """ test_grad_traj5_2 344 | Test of implementation 1 of gradient. """ 345 | traj = simple_traj5() 346 | theta = np.array([-1.0]) 347 | choices = [1, 0, 2] 348 | elts = LearningElementsSecure(traj, theta, choices) 349 | elts.computeGradientLogZ() 350 | elts_ref = LearningElementsRef(traj, theta, choices) 351 | elts_ref.computeGradientLogZ() 352 | g = elts.grad_logZ 353 | g_ref = elts_ref.grad_logZ 354 | assert(np.abs(g - g_ref).max() < 1e-3), (g, g_ref) 355 | 356 | def test_hess_traj1_1(): 357 | """ test_hess_traj1_1 """ 358 | traj = simple_traj1() 359 | theta = np.array([0.0, -1.0]) 360 | choices = [1, 0] 361 | elts = LearningElements(traj, theta, choices) 362 | elts.computeHessianLogZ() 363 | elts_ref = LearningElementsRef(traj, theta, choices) 364 | elts_ref.computeHessianLogZ() 365 | h = elts.hess_logZ 366 | h_ref = elts_ref.hess_logZ 367 | assert(np.abs(h - h_ref).max() < 1e-3), (h, h_ref) 368 | 369 | def test_hess_traj1_2(): 370 | """ test_hess_traj1_2 """ 371 | traj = simple_traj1() 372 | theta = np.array([0.0, -1.0]) 373 | choices = [1, 0] 374 | elts = LearningElementsSecure(traj, theta, choices) 375 | elts.computeHessianLogZ() 376 | elts_ref = LearningElementsRef(traj, theta, choices) 377 | elts_ref.computeHessianLogZ() 378 | h = elts.hess_logZ 379 | h_ref = elts_ref.hess_logZ 380 | assert(np.abs(h - h_ref).max() < 1e-3), (h, h_ref) 381 | 382 | def test_hess_traj2_1(): 383 | """ test_hess_traj1_1 """ 384 | traj = simple_traj2() 385 | theta = np.array([0.0, -1.0]) 386 | choices = [0, 1] 387 | elts = LearningElements(traj, theta, choices) 388 | elts.computeHessianLogZ() 389 | elts_ref = LearningElementsRef(traj, theta, choices) 390 | elts_ref.computeHessianLogZ() 391 | h = elts.hess_logZ 392 | h_ref = elts_ref.hess_logZ 393 | assert(np.abs(h - h_ref).max() < 1e-3), (h, h_ref) 394 | 395 | def test_hess_traj4_1(): 396 | """ test_hess_traj4_1 """ 397 | traj = simple_traj4() 398 | theta = np.array([0.0, -1.0]) 399 | choices = [1, 0, 2] 400 | elts = LearningElements(traj, theta, choices) 401 | elts.computeHessianLogZ() 402 | elts_ref = LearningElementsRef(traj, theta, choices) 403 | elts_ref.computeHessianLogZ() 404 | h = elts.hess_logZ 405 | h_ref = elts_ref.hess_logZ 406 | assert(np.abs(h - h_ref).max() < 1e-3), (h, h_ref) 407 | 408 | def test_hess_traj5_1(): 409 | """ test_hess_traj5_1 """ 410 | traj = simple_traj5() 411 | theta = np.array([-1.0]) 412 | choices = [1, 0, 2] 413 | elts = LearningElements(traj, theta, choices) 414 | elts.computeHessianLogZ() 415 | elts_ref = LearningElementsRef(traj, theta, choices) 416 | elts_ref.computeHessianLogZ() 417 | h = elts.hess_logZ 418 | h_ref = elts_ref.hess_logZ 419 | assert(np.abs(h - h_ref).max() < 1e-3), \ 420 | (h, h_ref, elts.hess_Z, elts_ref.hess_Z, \ 421 | elts.grad_Z, elts_ref.grad_Z, elts.Z, elts_ref.Z,) 422 | 423 | def test_hess_traj5_2(): 424 | """ test_hess_traj5_2 """ 425 | traj = simple_traj5() 426 | theta = np.array([-1.0]) 427 | choices = [1, 0, 2] 428 | elts = LearningElementsSecure(traj, theta, choices) 429 | elts.computeHessianLogZ() 430 | elts_ref = LearningElementsRef(traj, theta, choices) 431 | elts_ref.computeHessianLogZ() 432 | h = elts.hess_logZ 433 | h_ref = elts_ref.hess_logZ 434 | assert(np.abs(h - h_ref).max() < 1e-3), \ 435 | (h, h_ref, elts.hess_Z, elts_ref.hess_Z, \ 436 | elts.grad_Z, elts_ref.grad_Z, elts.Z, elts_ref.Z,) 437 | 438 | def test_hess_traj8_2(): 439 | """ test_hess_traj8_2 """ 440 | traj = simple_traj8() 441 | theta = np.array([0.0]) 442 | choices = [1, 0] 443 | elts = LearningElementsSecure(traj, theta, choices) 444 | elts.computeHessianLogZ() 445 | elts_ref = LearningElementsRef(traj, theta, choices) 446 | elts_ref.computeHessianLogZ() 447 | h = elts.hess_logZ 448 | h_ref = elts_ref.hess_logZ 449 | assert(np.abs(h - h_ref).max() < 1e-3), \ 450 | (h, h_ref, elts.hess_Z, elts_ref.hess_Z, \ 451 | elts.grad_Z, elts_ref.grad_Z, elts.Z, elts_ref.Z,) 452 | 453 | def test_Z2(): 454 | """ Test of implementation 2. """ 455 | traj = simple_traj2() 456 | theta = np.array([0.0, -1.0]) 457 | choices = [0, 1] 458 | elts = LearningElements(traj, theta, choices) 459 | elts.computeLogZ() 460 | elts_ref = LearningElementsRef(traj, theta, choices) 461 | elts_ref.computeLogZ() 462 | assert(within(elts.logZ, elts_ref.logZ, 1e-5)) 463 | 464 | def test_Z3(): 465 | """ Test of implementation 2. """ 466 | traj = simple_traj10() 467 | theta = np.array([500.0, -100.0]) 468 | weights = [[0.1, 0.9], [0.9, 0.1]] 469 | elts = LearningElementsSecure(traj, theta, weights=weights) 470 | elts.computeLogZ() 471 | 472 | def test_grad_Z2(): 473 | """ Test of implementation 2 of gradient. """ 474 | traj = simple_traj2() 475 | theta = np.array([0.0, -1.0]) 476 | choices = [0, 1] 477 | elts = LearningElements(traj, theta, choices) 478 | elts.computeGradientLogZ() 479 | elts_ref = LearningElementsRef(traj, theta, choices) 480 | elts_ref.computeGradientLogZ() 481 | g = elts.grad_logZ 482 | g_ref = elts_ref.grad_logZ 483 | assert(np.abs(g - g_ref).max() < 1e-3), (g, g_ref) 484 | 485 | if __name__ == '__main__': 486 | def f(x): 487 | """ Inner. """ 488 | return np.dot(x, x) / 2 489 | # from mm.path_inference.learning_traj_elements_test import * 490 | x0 = np.array([10.0, -100.0]) 491 | compute_hess(f, x0) -------------------------------------------------------------------------------- /mm/path_inference/learning_traj_em.py: -------------------------------------------------------------------------------- 1 | #Copyright 2011, 2012 Timothy Hunter 2 | # 3 | #This library is free software; you can redistribute it and/or 4 | #modify it under the terms of the GNU Lesser General Public 5 | #License as published by the Free Software Foundation; either 6 | #version 2.1 of the License, or (at your option) version 3. 7 | # 8 | #This library is distributed in the hope that it will be useful, 9 | #but WITHOUT ANY WARRANTY; without even the implied warranty of 10 | #MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU 11 | #Lesser General Public License for more details. 12 | # 13 | #You should have received a copy of the GNU Lesser General Public 14 | #License along with this library. If not, see . 15 | ''' EM algorithm to optimize the parameters. 16 | 17 | Created on Sep 2, 2011 18 | 19 | @author: tjhunter 20 | ''' 21 | import numpy as np 22 | from mm.path_inference.learning_traj_smoother import \ 23 | TrajectorySmoother1 24 | from mm.path_inference.learning_traj_optimizer import optimize_function 25 | 26 | def get_traj_estims(trajs, theta): 27 | """ Given a set of trajectories, returns a set of trajectory estimates. 28 | 29 | i.e., a list of (traj, weights). weights is a sequence of probability 30 | vectors that contain the posterior probabilities from the smoother. 31 | """ 32 | def inner(traj): 33 | """ Inner function that does the work. """ 34 | smoother = TrajectorySmoother1(traj, theta) 35 | smoother.computeProbs() 36 | probabilities = smoother.probabilities 37 | del smoother 38 | return (traj, probabilities) 39 | return [inner(traj) for traj in trajs] 40 | 41 | def learn_em(opt_function_provider, trajs, start_value, \ 42 | max_iters=10, max_inner_iters=5): 43 | """ Learn EM function. 44 | 45 | Returns the history of the progression: list of: 46 | - log likelihood 47 | - learned parameter 48 | 49 | Arguments: 50 | - opt_function_provider: traj_estims -> (optimizable function) 51 | - trajs: a set of trajectories 52 | - start_value: numpy array, start vector 53 | - max_iters: maximum number of iterations 54 | """ 55 | theta = np.array(start_value) 56 | history = [] 57 | for step in range(max_iters): 58 | traj_estims = get_traj_estims(trajs, theta) 59 | opt_fun = opt_function_provider(traj_estims) 60 | (theta1, ys) = optimize_function(opt_fun, theta, max_inner_iters) 61 | print 'Epoch %i : %f' % (step, ys[-1]) 62 | history.append((ys[-1], theta1)) 63 | theta = theta1 64 | return history -------------------------------------------------------------------------------- /mm/path_inference/learning_traj_em_test.py: -------------------------------------------------------------------------------- 1 | #Copyright 2011, 2012 Timothy Hunter 2 | # 3 | #This library is free software; you can redistribute it and/or 4 | #modify it under the terms of the GNU Lesser General Public 5 | #License as published by the Free Software Foundation; either 6 | #version 2.1 of the License, or (at your option) version 3. 7 | # 8 | #This library is distributed in the hope that it will be useful, 9 | #but WITHOUT ANY WARRANTY; without even the implied warranty of 10 | #MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU 11 | #Lesser General Public License for more details. 12 | # 13 | #You should have received a copy of the GNU Lesser General Public 14 | #License along with this library. If not, see . 15 | ''' Tests for the EM. 16 | 17 | Created on Sep 5, 2011 18 | 19 | @author: tjhunter 20 | ''' 21 | from mm.path_inference.learning_traj_test import (simple_traj1, 22 | simple_traj4, simple_traj3) 23 | from mm.path_inference.learning_traj_optimizer import trajs_estim_obj_fun_1 24 | from mm.path_inference.learning_traj_em import learn_em 25 | import numpy as np 26 | # Forces Numpy to consider warnings as errors. 27 | np.seterr(all='raise') 28 | 29 | def test_em_1(): 30 | """ Very simple test: we pick some trajectories and verify that 31 | the LL increases with EM. 32 | """ 33 | trajs = [simple_traj1(), simple_traj4(), simple_traj3()] 34 | theta_start = 0.1 * np.ones(2) 35 | history = learn_em(trajs_estim_obj_fun_1, trajs, theta_start) 36 | # (ll_end, theta_end) = history[-1] 37 | # Very simple check here: we verify the progression goes upward 38 | # the likelihood: 39 | for t in range(len(history)-1): 40 | (ll_1, _) = history[t] 41 | (ll_2, _) = history[t+1] 42 | assert ll_1 <= ll_2 43 | -------------------------------------------------------------------------------- /mm/path_inference/learning_traj_filter.py: -------------------------------------------------------------------------------- 1 | #Copyright 2011, 2012 Timothy Hunter 2 | # 3 | #This library is free software; you can redistribute it and/or 4 | #modify it under the terms of the GNU Lesser General Public 5 | #License as published by the Free Software Foundation; either 6 | #version 2.1 of the License, or (at your option) version 3. 7 | # 8 | #This library is distributed in the hope that it will be useful, 9 | #but WITHOUT ANY WARRANTY; without even the implied warranty of 10 | #MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU 11 | #Lesser General Public License for more details. 12 | # 13 | #You should have received a copy of the GNU Lesser General Public 14 | #License along with this library. If not, see . 15 | ''' k-lagged filter for a trajectory. 16 | 17 | Created on Sep 2, 2011 18 | 19 | @author: tjhunter 20 | ''' 21 | import numpy as np 22 | from numpy import dot, exp 23 | from mm.log_math import lse 24 | from mm.path_inference import SoftFilter 25 | 26 | class TrajectoryFilterRef(SoftFilter): 27 | """ Runs the k-lagged filtering on a trajectory.. 28 | 29 | k == 0 corresponds to pure filtering. 30 | k == L is equivalent to running forward-backward smoothing 31 | (warning, in this case, performance is *terrible*). 32 | 33 | Reference implementation (unsafe against log over/underflows), only use 34 | for testing! 35 | 36 | Interesting fields computed: 37 | - probabilities 38 | """ 39 | # pylint: disable=W0201, w0231 40 | def __init__(self, traj, theta, k=1): 41 | self.traj = traj 42 | self.theta = theta 43 | self.k = k 44 | 45 | def computeProbabilities(self): 46 | """ Compute probabilities. 47 | """ 48 | self.computeProbs() 49 | 50 | def computeProbs(self): 51 | """ Compute probabilities. 52 | """ 53 | if 'probabilities' in self.__dict__: 54 | return 55 | # Compute the weights 56 | self.log_ws = [] 57 | for l in range(self.traj.L): 58 | self.log_ws.append(exp(dot(self.traj.features_np[l], self.theta))) 59 | # Forward pass 60 | # Nothing changes here. 61 | # Init: 62 | self.forward = [np.zeros(N_l) for N_l in self.traj.num_choices] 63 | self.forward[0] = self.log_ws[0] / sum(self.log_ws[0]) 64 | for l in range(1, self.traj.L): 65 | N_l = self.traj.num_choices[l] 66 | qs = np.zeros(N_l) 67 | for i in range(N_l): 68 | if i in self.traj.connections_backward[l]: 69 | s = sum([self.forward[l-1][j] \ 70 | for j in self.traj.connections_backward[l][i]]) 71 | qs[i] = self.log_ws[l][i] * s 72 | assert sum(qs) > 0 73 | self.forward[l] = qs / sum(qs) 74 | # Backward pass 75 | # A bit more complicated because we need to do it again for every chunk. 76 | self.backward = [np.zeros(N_l, dtype=np.float64) \ 77 | for N_l in self.traj.num_choices] 78 | for l in range(self.traj.L): 79 | self.backward[l] = self.computeBackward(l, min(l+self.k, self.traj.L-1)) 80 | # Multplication: 81 | self.probabilities = [] 82 | for l in range(self.traj.L): 83 | ps = self.forward[l] * self.backward[l] 84 | assert sum(ps) > 0 85 | self.probabilities.append(ps / sum(ps)) 86 | 87 | def computeBackward(self, t, t_start): 88 | """ Computes backward recursion for index t, \ 89 | starting at index t_start >= t. 90 | 91 | Does not modify the function. Returns the vector of backward values. 92 | """ 93 | assert t_start >= t 94 | assert self.traj.L > t_start 95 | # Intermediate vectors 96 | vs = [] 97 | # Initizaliation 98 | N_t_start = self.traj.num_choices[t_start] 99 | vs.append(np.ones(N_t_start, dtype=np.float64)) 100 | for l in range(t, t_start)[::-1]: 101 | N_l = self.traj.num_choices[l] 102 | qs = np.zeros(N_l) 103 | for i in range(N_l): 104 | if i in self.traj.connections_forward[l]: 105 | s = sum([vs[-1][j] \ 106 | for j in self.traj.connections_forward[l][i]]) 107 | qs[i] += self.log_ws[l][i] * s 108 | assert sum(qs) > 0, l 109 | vs.append(qs / sum(qs)) 110 | return vs[-1] 111 | 112 | 113 | class TrajectoryFilter1(TrajectoryFilterRef): 114 | """ Runs the k-lagged filtering on a trajectory.. 115 | 116 | k == 0 corresponds to pure filtering. 117 | k == L is equivalent to running forward-backward smoothing 118 | (warning, in this case, performance is *terrible*). 119 | 120 | Reference implementation (unsafe against log over/underflows), only use 121 | for testing! 122 | 123 | Interesting fields computed: 124 | - probabilities 125 | """ 126 | # pylint: disable=W0201 127 | 128 | def computeProbs(self): 129 | """ Compute probabilities. 130 | """ 131 | if 'probabilities' in self.__dict__: 132 | return 133 | inf = float('inf') 134 | # Compute the weights 135 | self.log_ws = [] 136 | for l in range(self.traj.L): 137 | self.log_ws.append(dot(self.traj.features_np[l], self.theta)) 138 | # Forward pass 139 | # Init: 140 | self.log_forward = [np.zeros(N_l) for N_l in self.traj.num_choices] 141 | self.log_forward[0] = self.log_ws[0] - lse(self.log_ws[0]) 142 | for l in range(1, self.traj.L): 143 | N_l = self.traj.num_choices[l] 144 | qs = -inf * np.ones(N_l) 145 | for i in range(N_l): 146 | if i in self.traj.connections_backward[l]: 147 | log_s = lse([self.log_forward[l-1][j] \ 148 | for j in self.traj.connections_backward[l][i]]) 149 | qs[i] = self.log_ws[l][i] + log_s 150 | self.log_forward[l] = qs - lse(qs) 151 | # Backward pass 152 | self.log_backward = [np.zeros(N_l, dtype=np.float64) \ 153 | for N_l in self.traj.num_choices] 154 | # Backward pass 155 | # A bit more complicated because we need to do it again for every chunk. 156 | for l in range(self.traj.L): 157 | self.log_backward[l] = self.computeLogBackward(l, min(l+self.k, \ 158 | self.traj.L-1)) 159 | # Product: 160 | self.probabilities = [] 161 | self.log_probabilities = [] 162 | for l in range(self.traj.L): 163 | log_ps = self.log_forward[l] + self.log_backward[l] 164 | log_ps -= max(log_ps) 165 | # Aggressively clamp the values that are too low to prevent an underflow 166 | log_ps[log_ps < -200] = -200 167 | self.log_probabilities.append(log_ps - lse(log_ps)) 168 | ps = exp(log_ps) 169 | assert sum(ps) > 0, (l, log_ps, self.log_forward[l], self.log_backward[l]) 170 | ps /= sum(ps) 171 | self.probabilities.append(ps) 172 | 173 | def computeLogBackward(self, t, t_start): 174 | """ Computes backward recursion for index t, \ 175 | starting at index t_start >= t. 176 | 177 | Does not modify the function. Returns the vector of backward values. 178 | """ 179 | assert t_start >= t 180 | assert self.traj.L > t_start 181 | inf = float('inf') 182 | # Intermediate vectors 183 | vs = [] 184 | # Initizaliation 185 | N_t_start = self.traj.num_choices[t_start] 186 | vs.append(np.zeros(N_t_start, dtype=np.float64)) 187 | for l in range(t, t_start)[::-1]: 188 | N_l = self.traj.num_choices[l] 189 | qs = -inf * np.ones(N_l) 190 | for i in range(N_l): 191 | if i in self.traj.connections_forward[l]: 192 | log_s = lse([vs[-1][j] \ 193 | for j in self.traj.connections_forward[l][i]]) 194 | qs[i] = self.log_ws[l][i] + log_s 195 | vs.append(qs - lse(qs)) 196 | return vs[-1] 197 | 198 | -------------------------------------------------------------------------------- /mm/path_inference/learning_traj_filter_test.py: -------------------------------------------------------------------------------- 1 | #Copyright 2011, 2012 Timothy Hunter 2 | # 3 | #This library is free software; you can redistribute it and/or 4 | #modify it under the terms of the GNU Lesser General Public 5 | #License as published by the Free Software Foundation; either 6 | #version 2.1 of the License, or (at your option) version 3. 7 | # 8 | #This library is distributed in the hope that it will be useful, 9 | #but WITHOUT ANY WARRANTY; without even the implied warranty of 10 | #MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU 11 | #Lesser General Public License for more details. 12 | # 13 | #You should have received a copy of the GNU Lesser General Public 14 | #License along with this library. If not, see . 15 | ''' 16 | Test files for the filter. 17 | 18 | Created on Sep 5, 2011 19 | 20 | @author: tjhunter 21 | ''' 22 | from mm.path_inference.learning_traj_smoother import TrajectorySmootherRef 23 | from mm.path_inference.learning_traj_filter import TrajectoryFilterRef, \ 24 | TrajectoryFilter1 25 | from mm.path_inference.learning_traj_test import simple_traj1, \ 26 | simple_traj2, simple_traj6 27 | from mm.path_inference.learning_traj_smoother_test import check_probs 28 | import numpy as np 29 | # Forces Numpy to consider warnings as errors. 30 | np.seterr(all='raise') 31 | 32 | def check_prob_fields(probs1, probs2): 33 | """ Compares the two probability sequences. """ 34 | assert len(probs1) == len(probs2) 35 | for l in range(len(probs1)): 36 | assert np.abs(probs1[l] - probs2[l]).max() < 1e-3, \ 37 | (l, probs1[l], probs2[l], probs1, probs2) 38 | 39 | def test_filter_ref_1(): 40 | """ test_filter_ref_1 41 | """ 42 | traj = simple_traj1() 43 | theta = np.array([1.0, -1.0]) 44 | filter_0 = TrajectoryFilterRef(traj, theta, 0) 45 | filter_0.computeProbs() 46 | # The forward probabilities should equal the probabilities 47 | check_prob_fields(filter_0.forward, filter_0.probabilities) 48 | # Run the filter in inneficient smooting mode 49 | filter_L = TrajectoryFilterRef(traj, theta, traj.L) 50 | filter_L.computeProbs() 51 | smoother = TrajectorySmootherRef(traj, theta) 52 | smoother.computeProbs() 53 | check_prob_fields(filter_L.forward, smoother.forward) 54 | check_prob_fields(filter_L.backward, smoother.backward) 55 | check_prob_fields(filter_L.probabilities, smoother.probabilities) 56 | 57 | def test_filter_ref_2(): 58 | """ test_filter_ref_2 59 | """ 60 | traj = simple_traj2() 61 | theta = np.array([1.0, -1.0]) 62 | filter_0 = TrajectoryFilterRef(traj, theta, 0) 63 | filter_0.computeProbs() 64 | # The forward probabilities should equal the probabilities 65 | check_prob_fields(filter_0.forward, filter_0.probabilities) 66 | # Run the filter in inneficient smooting mode 67 | filter_L = TrajectoryFilterRef(traj, theta, traj.L) 68 | filter_L.computeProbs() 69 | smoother = TrajectorySmootherRef(traj, theta) 70 | smoother.computeProbs() 71 | check_prob_fields(filter_L.forward, smoother.forward) 72 | check_prob_fields(filter_L.backward, smoother.backward) 73 | check_prob_fields(filter_L.probabilities, smoother.probabilities) 74 | 75 | def test_filter_ref_traj6_1(): 76 | """ test_filter_ref_traj6_1 """ 77 | traj = simple_traj6() 78 | theta = np.array([-1.0, 1.0]) 79 | for k in range(traj.L): 80 | filter_ref = TrajectoryFilterRef(traj, theta, k) 81 | filter_ref.computeProbs() 82 | filter_1 = TrajectoryFilter1(traj, theta, k) 83 | filter_1.computeProbs() 84 | check_probs(filter_1, filter_ref) 85 | 86 | def test_filter_ref_traj1_1(): 87 | """ test_filter_ref_traj1_1 """ 88 | traj = simple_traj1() 89 | theta = np.array([-1.0, 1.0]) 90 | for k in range(traj.L): 91 | filter_ref = TrajectoryFilterRef(traj, theta, k) 92 | filter_ref.computeProbs() 93 | filter_1 = TrajectoryFilter1(traj, theta, k) 94 | filter_1.computeProbs() 95 | check_probs(filter_1, filter_ref) 96 | 97 | def test_filter_ref_traj2_1(): 98 | """ test_filter_ref_traj2_1 """ 99 | traj = simple_traj2() 100 | theta = np.array([-1.0, 1.0]) 101 | for k in range(traj.L): 102 | filter_ref = TrajectoryFilterRef(traj, theta, k) 103 | filter_ref.computeProbs() 104 | filter_1 = TrajectoryFilter1(traj, theta, k) 105 | filter_1.computeProbs() 106 | check_probs(filter_1, filter_ref) 107 | 108 | -------------------------------------------------------------------------------- /mm/path_inference/learning_traj_optimizer.py: -------------------------------------------------------------------------------- 1 | #Copyright 2011, 2012 Timothy Hunter 2 | # 3 | #This library is free software; you can redistribute it and/or 4 | #modify it under the terms of the GNU Lesser General Public 5 | #License as published by the Free Software Foundation; either 6 | #version 2.1 of the License, or (at your option) version 3. 7 | # 8 | #This library is distributed in the hope that it will be useful, 9 | #but WITHOUT ANY WARRANTY; without even the implied warranty of 10 | #MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU 11 | #Lesser General Public License for more details. 12 | # 13 | #You should have received a copy of the GNU Lesser General Public 14 | #License along with this library. If not, see . 15 | """ Set of functions for computing the most likely parameter vector. 16 | """ 17 | 18 | from numpy.linalg import inv 19 | from numpy import dot 20 | import numpy as np 21 | from mm.path_inference.learning_traj_elements import LearningElementsRef, \ 22 | LearningElements0, \ 23 | LearningElements1, \ 24 | LearningElements0Bis, \ 25 | LearningElements2 26 | 27 | def optimize_function(fun, start_point, max_iters=5, regularizer=0): 28 | """ Optimization procedure for a function. 29 | 30 | Arguments: 31 | - fun: function to optimize: 1-nparray -> (y, grad, hessian) 32 | - start_point: 1-nparray 33 | - max_iters: the maximum number of iterations 34 | Returns: 35 | (x, ys) 36 | where x is the best value and and ys is the list of evaluated values 37 | of the function. 38 | """ 39 | ys = [] 40 | x = start_point.copy() 41 | n = len(x) 42 | d = regularizer * np.eye(n) / 2 43 | while len(ys) < max_iters: 44 | print 'iter %s, x=' % len(ys), x 45 | (y, g, h) = fun(x) 46 | y += - dot(x, dot(d, x)) 47 | g += - 2 * dot(d, x) 48 | h += - 2 * d 49 | ys.append(y) 50 | print 'y=',y 51 | search_dir = - dot(inv(h), g) 52 | t = 1 53 | alpha = 0.2 54 | beta = 0.7 55 | # print "LINE SEARCH" 56 | while True: 57 | x2 = x + t * search_dir 58 | # print 'Looking t = ',t 59 | (y_, _, _) = fun(x2) 60 | # print 'Y_ = ',y_ 61 | if y_ >= y + alpha * t * dot(g, search_dir): 62 | break 63 | else: 64 | t *= beta 65 | if t < 1e-5: 66 | print 't is too small' 67 | break 68 | x = x2 69 | return (x, ys) 70 | 71 | def trajs_obj_fun_ref(traj_choices): 72 | """ Uses the reference implementation to create an objective function. 73 | """ 74 | def inner(theta): 75 | """ Returned closure. """ 76 | y = 0.0 77 | g = np.zeros_like(theta) 78 | n = len(theta) 79 | h = np.zeros((n, n)) 80 | for (traj, choice) in traj_choices: 81 | elts = LearningElementsRef(traj, theta, choice) 82 | elts.computeValue() 83 | y += elts.logValue 84 | elts.computeGradientValue() 85 | g += elts.grad_logValue 86 | elts.computeHessianValue() 87 | h += elts.hess_logValue 88 | return (y, g, h) 89 | return inner 90 | 91 | def trajs_obj_fun_0(traj_choices): 92 | """ Uses the reference implementation to create an objective function. 93 | """ 94 | def inner(theta): 95 | """ Returned closure. """ 96 | y = 0.0 97 | g = np.zeros_like(theta) 98 | n = len(theta) 99 | h = np.zeros((n, n)) 100 | for (traj, choice) in traj_choices: 101 | elts = LearningElements0(traj, theta, choice) 102 | elts.computeValue() 103 | y += elts.logValue 104 | elts.computeGradientValue() 105 | g += elts.grad_logValue 106 | elts.computeHessianValue() 107 | h += elts.hess_logValue 108 | return (y, g, h) 109 | return inner 110 | 111 | def trajs_obj_fun_0bis(traj_choices): 112 | """ Uses the reference implementation to create an objective function. 113 | """ 114 | def inner(theta): 115 | """ Returned closure. """ 116 | y = 0.0 117 | g = np.zeros_like(theta) 118 | n = len(theta) 119 | h = np.zeros((n, n)) 120 | for (traj, choice) in traj_choices: 121 | elts = LearningElements0Bis(traj, theta, choice) 122 | elts.computeValue() 123 | y += elts.logValue 124 | elts.computeGradientValue() 125 | g += elts.grad_logValue 126 | elts.computeHessianValue() 127 | h += elts.hess_logValue 128 | return (y, g, h) 129 | return inner 130 | 131 | def trajs_obj_fun_1(traj_choices): 132 | """ Uses the rsecure implementation to create an objective function. 133 | """ 134 | def inner(theta): 135 | """ Returned closure. """ 136 | y = 0.0 137 | g = np.zeros_like(theta) 138 | n = len(theta) 139 | h = np.zeros((n, n)) 140 | for (traj, choice) in traj_choices: 141 | elts = LearningElements1(traj, theta, choice) 142 | elts.computeValue() 143 | y += elts.logValue 144 | elts.computeGradientValue() 145 | g += elts.grad_logValue 146 | elts.computeHessianValue() 147 | h += elts.hess_logValue 148 | return (y, g, h) 149 | return inner 150 | 151 | def trajs_obj_fun_2(traj_choices): 152 | """ Uses the rsecure implementation to create an objective function. 153 | """ 154 | def inner(theta): 155 | """ Returned closure. """ 156 | y = 0.0 157 | g = np.zeros_like(theta) 158 | n = len(theta) 159 | h = np.zeros((n, n)) 160 | for (traj, choice) in traj_choices: 161 | elts = LearningElements2(traj, theta, choice) 162 | elts.computeValue() 163 | y += elts.logValue 164 | elts.computeGradientValue() 165 | g += elts.grad_logValue 166 | elts.computeHessianValue() 167 | h += elts.hess_logValue 168 | return (y, g, h) 169 | return inner 170 | 171 | def trajs_estim_obj_fun_ref(traj_estims): 172 | """ Uses the reference implementation to create an objective function. 173 | """ 174 | def inner(theta): 175 | """ Returned closure. """ 176 | y = 0.0 177 | g = np.zeros_like(theta) 178 | n = len(theta) 179 | h = np.zeros((n, n)) 180 | for (traj, weights) in traj_estims: 181 | elts = LearningElementsRef(traj, theta, weights=weights) 182 | elts.computeValue() 183 | y += elts.logValue 184 | elts.computeGradientValue() 185 | g += elts.grad_logValue 186 | elts.computeHessianValue() 187 | h += elts.hess_logValue 188 | return (y, g, h) 189 | return inner 190 | 191 | def trajs_estim_obj_fun_1(traj_estims): 192 | """ Uses the reference implementation to create an objective function. 193 | """ 194 | def inner(theta): 195 | """ Returned closure. """ 196 | y = 0.0 197 | g = np.zeros_like(theta) 198 | n = len(theta) 199 | h = np.zeros((n, n)) 200 | for (traj, weights) in traj_estims: 201 | elts = LearningElements1(traj, theta, weights=weights) 202 | elts.computeValue() 203 | y += elts.logValue 204 | elts.computeGradientValue() 205 | g += elts.grad_logValue 206 | elts.computeHessianValue() 207 | h += elts.hess_logValue 208 | return (y, g, h) 209 | return inner 210 | -------------------------------------------------------------------------------- /mm/path_inference/learning_traj_optimizer_test.py: -------------------------------------------------------------------------------- 1 | #Copyright 2011, 2012 Timothy Hunter 2 | # 3 | #This library is free software; you can redistribute it and/or 4 | #modify it under the terms of the GNU Lesser General Public 5 | #License as published by the Free Software Foundation; either 6 | #version 2.1 of the License, or (at your option) version 3. 7 | # 8 | #This library is distributed in the hope that it will be useful, 9 | #but WITHOUT ANY WARRANTY; without even the implied warranty of 10 | #MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU 11 | #Lesser General Public License for more details. 12 | # 13 | #You should have received a copy of the GNU Lesser General Public 14 | #License along with this library. If not, see . 15 | """ Optim test. 16 | """ 17 | 18 | #from mm.path_inference.learning_traj_test import simple_traj1,\ 19 | # simple_traj2, simple_traj4, simple_traj0, simple_traj5 20 | 21 | from mm.path_inference.learning_traj_elements_test \ 22 | import compute_grad, compute_hess, within 23 | from mm.path_inference.learning_traj_optimizer import optimize_function, \ 24 | trajs_obj_fun_0, trajs_obj_fun_1, trajs_obj_fun_ref, \ 25 | trajs_estim_obj_fun_ref, trajs_obj_fun_0bis, trajs_obj_fun_2 26 | from mm.path_inference.learning_traj_test import \ 27 | simple_traj10, simple_traj4, simple_traj9, simple_traj5, simple_traj8 28 | import numpy as np 29 | # Forces Numpy to consider warnings as errors. 30 | np.seterr(all='raise') 31 | 32 | def wrap(fun): 33 | """ Wrapper that uses empirical functions only. 34 | """ 35 | def inner(x): 36 | """ Closure. """ 37 | y = fun(x) 38 | g = compute_grad(fun, x) 39 | h = compute_hess(fun, x) 40 | return (y, g, h) 41 | return inner 42 | 43 | def fun1(x): 44 | """ Simple nearly quadratic function. 45 | """ 46 | return -pow(np.dot(x - np.ones_like(x), x - np.ones_like(x)), 0.7) 47 | 48 | def test_1(): 49 | """ Basic test for the optimizer. """ 50 | start = np.zeros(2) 51 | opt_fun = wrap(fun1) 52 | (x, ys) = optimize_function(opt_fun, start, 10) 53 | assert(np.abs(x-np.ones_like(x)).max() < 1e-3) 54 | assert ys[-1] >= -1e-3 55 | assert ys[-1] <= 0 56 | assert np.abs(start).max() == 0, (start, x) 57 | 58 | def test_opt_traj5_1ref(): 59 | """ test_opt_traj5_1 """ 60 | traj = simple_traj5() 61 | start_theta = np.array([-1.0]) 62 | choices = [1, 0, 2] 63 | traj_choices = [(traj, choices)] 64 | obj_fun = trajs_obj_fun_ref(traj_choices) 65 | (theta, ys) = optimize_function(obj_fun, start_theta, max_iters=20) 66 | assert ys[0] <= ys[-1], (theta, ys, len(ys)) 67 | 68 | def test_opt_traj4_1ref(): 69 | """ test_opt_traj4_1 """ 70 | traj = simple_traj4() 71 | start_theta = np.array([0.0, 0.0]) 72 | choices = [1, 0, 2] 73 | traj_choices = [(traj, choices)] 74 | obj_fun = trajs_obj_fun_ref(traj_choices) 75 | (theta, ys) = optimize_function(obj_fun, start_theta, max_iters=20) 76 | assert ys[0] <= ys[-1], (theta, ys, len(ys)) 77 | 78 | def test_opt0_traj4_1ref(): 79 | """ test_opt0_traj4_1 """ 80 | traj = simple_traj4() 81 | start_theta = np.array([0.0, 0.0]) 82 | choices = [1, 0, 2] 83 | estims = [np.array([0.0, 1.0]), np.array([1.0]), np.array([0.0, 0.0, 1.0])] 84 | traj_choices = [(traj, choices)] 85 | traj_estims = [(traj, estims)] 86 | obj_fun_2 = trajs_obj_fun_2(traj_choices) 87 | obj_fun_1 = trajs_obj_fun_1(traj_choices) 88 | obj_fun_0 = trajs_obj_fun_0(traj_choices) 89 | obj_fun_ref = trajs_obj_fun_ref(traj_choices) 90 | obj_fun_estim_ref = trajs_estim_obj_fun_ref(traj_estims) 91 | max_iters = 5 92 | (theta_0, ys_0) = optimize_function(obj_fun_0, start_theta, max_iters) 93 | (theta_1, ys_1) = optimize_function(obj_fun_1, start_theta, max_iters) 94 | (theta_2, ys_2) = optimize_function(obj_fun_2, start_theta, max_iters) 95 | (theta_ref, ys_ref) = optimize_function(obj_fun_ref, start_theta, max_iters) 96 | (theta_estim_ref, ys_estim_ref) = optimize_function(obj_fun_estim_ref, \ 97 | start_theta, max_iters) 98 | assert(np.abs(theta_0 - theta_ref).max() < 1e-3), (theta_0, theta_ref) 99 | assert(np.abs(theta_1 - theta_ref).max() < 1e-3), (theta_1, theta_ref) 100 | assert(np.abs(theta_2 - theta_ref).max() < 1e-3), (theta_2, theta_ref) 101 | assert(np.abs(theta_estim_ref - theta_ref).max() < 1e-3), (theta_estim_ref, \ 102 | theta_ref) 103 | assert within(ys_0[0], ys_ref[0], 1e-3), (theta_ref, ys_ref, theta_0, ys_0) 104 | assert within(ys_1[0], ys_ref[0], 1e-3), (theta_ref, ys_ref, theta_1, ys_1) 105 | assert within(ys_2[0], ys_ref[0], 1e-3), (theta_ref, ys_ref, theta_2, ys_2) 106 | assert within(ys_estim_ref[0], ys_ref[0], 1e-3), (theta_ref, ys_ref, \ 107 | theta_estim_ref, \ 108 | ys_estim_ref) 109 | 110 | def test_opt0_traj5_1(): 111 | """ test_opt0_traj5_1 """ 112 | traj = simple_traj5() 113 | start_theta = np.array([-1.0]) 114 | choices = [1, 0, 2] 115 | traj_choices = [(traj, choices)] 116 | obj_fun_1 = trajs_obj_fun_1(traj_choices) 117 | obj_fun_0 = trajs_obj_fun_0(traj_choices) 118 | obj_fun_ref = trajs_obj_fun_ref(traj_choices) 119 | max_iters = 5 120 | (theta_0, ys_0) = optimize_function(obj_fun_0, start_theta, max_iters) 121 | (theta_1, ys_1) = optimize_function(obj_fun_1, start_theta, max_iters) 122 | (theta_ref, ys_ref) = optimize_function(obj_fun_ref, start_theta, max_iters) 123 | assert(np.abs(theta_0 - theta_ref).max() < 1e-3), (theta_0, theta_ref) 124 | assert(np.abs(theta_1 - theta_ref).max() < 1e-3), (theta_1, theta_ref) 125 | assert within(ys_0[0], ys_ref[0], 1e-3), (theta_ref, ys_ref, theta_0, ys_0) 126 | assert within(ys_1[0], ys_ref[0], 1e-3), (theta_ref, ys_ref, theta_1, ys_1) 127 | 128 | def test_opt0_traj8_1(): 129 | """ test_opt0_traj8_1 """ 130 | traj = simple_traj8() 131 | start_theta = np.array([-1.0]) 132 | choices = [1, 0] 133 | traj_choices = [(traj, choices)] 134 | obj_fun_1 = trajs_obj_fun_1(traj_choices) 135 | obj_fun_0 = trajs_obj_fun_0(traj_choices) 136 | obj_fun_0bis = trajs_obj_fun_0bis(traj_choices) 137 | obj_fun_ref = trajs_obj_fun_ref(traj_choices) 138 | max_iters = 5 139 | (theta_ref, ys_ref) = optimize_function(obj_fun_ref, start_theta, \ 140 | max_iters, regularizer=1e-4) 141 | print ys_ref, theta_ref 142 | assert np.abs(theta_ref) < 1e-4 143 | (theta_0bis, ys_0bis) = optimize_function(obj_fun_0bis, \ 144 | start_theta, max_iters, \ 145 | regularizer=1e-4) 146 | (theta_0, ys_0) = optimize_function(obj_fun_0, \ 147 | start_theta, max_iters, regularizer=1e-4) 148 | (theta_1, ys_1) = optimize_function(obj_fun_1, \ 149 | start_theta, max_iters, regularizer=1e-4) 150 | assert(np.abs(theta_0bis - theta_ref).max() < 1e-3), (theta_0bis, theta_ref) 151 | assert(np.abs(theta_0 - theta_ref).max() < 1e-3), (theta_0, theta_ref) 152 | assert(np.abs(theta_1 - theta_ref).max() < 1e-3), (theta_1, theta_ref) 153 | assert within(ys_0bis[0], ys_ref[0], 1e-3), \ 154 | (theta_ref, ys_ref, theta_0bis, ys_0bis) 155 | assert within(ys_0[0], ys_ref[0], 1e-3), (theta_ref, ys_ref, theta_0, ys_0) 156 | assert within(ys_1[0], ys_ref[0], 1e-3), (theta_ref, ys_ref, theta_1, ys_1) 157 | 158 | def test_opt0_traj9_1(): 159 | """ test_opt0_traj9_1 """ 160 | traj = simple_traj9() 161 | start_theta = np.array([-1.0]) 162 | choices = [0, 0] 163 | traj_choices = [(traj, choices)] 164 | obj_fun_2 = trajs_obj_fun_2(traj_choices) 165 | obj_fun_1 = trajs_obj_fun_1(traj_choices) 166 | obj_fun_0 = trajs_obj_fun_0(traj_choices) 167 | obj_fun_0bis = trajs_obj_fun_0bis(traj_choices) 168 | obj_fun_ref = trajs_obj_fun_ref(traj_choices) 169 | max_iters = 5 170 | (theta_ref, ys_ref) = \ 171 | optimize_function(obj_fun_ref, start_theta, max_iters, regularizer=1e-4) 172 | print ys_ref, theta_ref 173 | (theta_0bis, ys_0bis) = \ 174 | optimize_function(obj_fun_0bis, start_theta, max_iters, regularizer=1e-4) 175 | (theta_0, ys_0) = \ 176 | optimize_function(obj_fun_0, start_theta, max_iters, regularizer=1e-4) 177 | (theta_1, ys_1) = \ 178 | optimize_function(obj_fun_1, start_theta, max_iters, regularizer=1e-4) 179 | (theta_2, ys_2) = \ 180 | optimize_function(obj_fun_2, start_theta, max_iters, regularizer=1e-4) 181 | assert(np.abs(theta_0bis - theta_ref).max() < 1e-3), (theta_0bis, theta_ref) 182 | assert(np.abs(theta_0 - theta_ref).max() < 1e-3), (theta_0, theta_ref) 183 | assert(np.abs(theta_1 - theta_ref).max() < 1e-3), (theta_1, theta_ref) 184 | assert(np.abs(theta_2 - theta_ref).max() < 1e-3), (theta_2, theta_ref) 185 | assert within(ys_0bis[0], ys_ref[0], 1e-3), \ 186 | (theta_ref, ys_ref, theta_0bis, ys_0bis) 187 | assert within(ys_0[0], ys_ref[0], 1e-3), (theta_ref, ys_ref, theta_0, ys_0) 188 | assert within(ys_1[0], ys_ref[0], 1e-3), (theta_ref, ys_ref, theta_1, ys_1) 189 | assert within(ys_2[0], ys_ref[0], 1e-3), (theta_ref, ys_ref, theta_2, ys_2) 190 | 191 | def test_opt0_traj10_1(): 192 | """ test_opt0_traj10_1 """ 193 | traj = simple_traj10() 194 | start_theta = np.array([-1.0, 2.0]) 195 | choices = [0, 0] 196 | traj_choices = [(traj, choices)] 197 | obj_fun_2 = trajs_obj_fun_2(traj_choices) 198 | obj_fun_1 = trajs_obj_fun_1(traj_choices) 199 | obj_fun_0bis = trajs_obj_fun_0bis(traj_choices) 200 | max_iters = 5 201 | (theta_0bis, ys_0bis) = \ 202 | optimize_function(obj_fun_0bis, start_theta, max_iters, regularizer=1e-4) 203 | (theta_1, ys_1) = \ 204 | optimize_function(obj_fun_1, start_theta, max_iters, regularizer=1e-4) 205 | (theta_2, ys_2) = \ 206 | optimize_function(obj_fun_2, start_theta, max_iters, regularizer=1e-4) 207 | assert(np.abs(theta_1 - theta_0bis).max() < 1e-3), (theta_1, theta_0bis) 208 | assert(np.abs(theta_2 - theta_0bis).max() < 1e-3), (theta_2, theta_0bis) 209 | assert within(ys_1[0], ys_0bis[0], 1e-3), (theta_0bis, ys_0bis, theta_1, ys_1) 210 | assert within(ys_2[0], ys_0bis[0], 1e-3), (theta_0bis, ys_0bis, theta_2, ys_2) 211 | -------------------------------------------------------------------------------- /mm/path_inference/learning_traj_smoother.py: -------------------------------------------------------------------------------- 1 | #Copyright 2011, 2012 Timothy Hunter 2 | # 3 | #This library is free software; you can redistribute it and/or 4 | #modify it under the terms of the GNU Lesser General Public 5 | #License as published by the Free Software Foundation; either 6 | #version 2.1 of the License, or (at your option) version 3. 7 | # 8 | #This library is distributed in the hope that it will be useful, 9 | #but WITHOUT ANY WARRANTY; without even the implied warranty of 10 | #MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU 11 | #Lesser General Public License for more details. 12 | # 13 | #You should have received a copy of the GNU Lesser General Public 14 | #License along with this library. If not, see . 15 | ''' Alpha beta smoother for a trajectory. 16 | 17 | Created on Sep 2, 2011 18 | 19 | @author: tjhunter 20 | ''' 21 | import numpy as np 22 | from numpy import dot, exp 23 | from mm.log_math import lse 24 | from mm.path_inference import SoftFilter 25 | 26 | class TrajectorySmootherRef(SoftFilter): 27 | """ Runs the alpha/beta recursion on the trajectory. 28 | 29 | Reference implementation (unsafe against log underflows). 30 | 31 | Interesting fields computed: 32 | - probabilities 33 | """ 34 | # pylint: disable=W0201 35 | def __init__(self, traj, theta): 36 | SoftFilter.__init__(self) 37 | self.traj = traj 38 | self.theta = theta 39 | 40 | def computeProbabilities(self): 41 | self.computeProbs() 42 | 43 | def computeProbs(self): 44 | """ Compute probabilities. 45 | """ 46 | if 'probabilities' in self.__dict__: 47 | return 48 | # Compute the weights 49 | self.log_ws = [] 50 | for l in range(self.traj.L): 51 | self.log_ws.append(exp(dot(self.traj.features_np[l], self.theta))) 52 | # Forward pass 53 | # Init: 54 | self.forward = [np.zeros(N_l) for N_l in self.traj.num_choices] 55 | self.forward[0] = self.log_ws[0] / sum(self.log_ws[0]) 56 | for l in range(1, self.traj.L): 57 | N_l = self.traj.num_choices[l] 58 | qs = np.zeros(N_l) 59 | for i in range(N_l): 60 | if i in self.traj.connections_backward[l]: 61 | s = sum([self.forward[l-1][j] \ 62 | for j in self.traj.connections_backward[l][i]]) 63 | qs[i] = self.log_ws[l][i] * s 64 | assert sum(qs) > 0 65 | self.forward[l] = qs / sum(qs) 66 | # Backward pass 67 | self.backward = [np.zeros(N_l, dtype=np.float64) \ 68 | for N_l in self.traj.num_choices] 69 | self.backward[-1].fill(1.0) 70 | for l in range(self.traj.L-2, -1, -1): 71 | N_l = self.traj.num_choices[l] 72 | qs = np.zeros(N_l) 73 | for i in range(N_l): 74 | if i in self.traj.connections_forward[l]: 75 | s = sum([self.backward[l+1][j] \ 76 | for j in self.traj.connections_forward[l][i]]) 77 | qs[i] += self.log_ws[l][i] * s 78 | assert sum(qs) > 0, l 79 | self.backward[l] = qs / sum(qs) 80 | # Multplication: 81 | self.probabilities = [] 82 | for l in range(self.traj.L): 83 | ps = self.forward[l] * self.backward[l] 84 | assert sum(ps) > 0 85 | self.probabilities.append(ps / sum(ps)) 86 | 87 | class TrajectorySmoother1(TrajectorySmootherRef): 88 | """ Runs the alpha/beta recursion on the trajectory. 89 | 90 | Secure implementation with respect to log underflows. 91 | 92 | Interesting fields computed: 93 | - probabilities 94 | """ 95 | # pylint: disable=W0201 96 | 97 | def computeProbs(self): 98 | """ Compute probabilities. 99 | """ 100 | if 'probabilities' in self.__dict__: 101 | return 102 | inf = float('inf') 103 | # Compute the weights 104 | self.log_ws = [] 105 | for l in range(self.traj.L): 106 | self.log_ws.append(dot(self.traj.features_np[l], self.theta)) 107 | # Forward pass 108 | # Init: 109 | self.log_forward = [np.zeros(N_l) for N_l in self.traj.num_choices] 110 | self.log_forward[0] = self.log_ws[0] - lse(self.log_ws[0]) 111 | for l in range(1, self.traj.L): 112 | N_l = self.traj.num_choices[l] 113 | qs = -inf * np.ones(N_l) 114 | for i in range(N_l): 115 | if i in self.traj.connections_backward[l]: 116 | log_s = lse([self.log_forward[l-1][j] \ 117 | for j in self.traj.connections_backward[l][i]]) 118 | qs[i] = self.log_ws[l][i] + log_s 119 | self.log_forward[l] = qs - lse(qs) 120 | # Backward pass 121 | self.log_backward = [np.zeros(N_l, dtype=np.float64) \ 122 | for N_l in self.traj.num_choices] 123 | self.log_backward[-1].fill(0.0) 124 | for l in range(self.traj.L-2, -1, -1): 125 | N_l = self.traj.num_choices[l] 126 | qs = -inf * np.ones(N_l) 127 | for i in range(N_l): 128 | if i in self.traj.connections_forward[l]: 129 | log_s = lse([self.log_backward[l+1][j] \ 130 | for j in self.traj.connections_forward[l][i]]) 131 | qs[i] = self.log_ws[l][i] + log_s 132 | self.log_backward[l] = qs - lse(qs) 133 | # Product: 134 | self.probabilities = [] 135 | self.log_probabilities = [] 136 | for l in range(self.traj.L): 137 | log_ps = self.log_forward[l] + self.log_backward[l] 138 | log_ps -= max(log_ps) 139 | # Aggressively clamp the values that are too low to prevent an underflow 140 | log_ps[log_ps < -200] = -200 141 | ps = exp(log_ps) 142 | self.log_probabilities.append(log_ps - lse(log_ps)) 143 | assert sum(ps) > 0, (l, log_ps, self.log_forward[l], self.log_backward[l]) 144 | ps /= sum(ps) 145 | self.probabilities.append(ps) 146 | -------------------------------------------------------------------------------- /mm/path_inference/learning_traj_smoother_test.py: -------------------------------------------------------------------------------- 1 | #Copyright 2011, 2012 Timothy Hunter 2 | # 3 | #This library is free software; you can redistribute it and/or 4 | #modify it under the terms of the GNU Lesser General Public 5 | #License as published by the Free Software Foundation; either 6 | #version 2.1 of the License, or (at your option) version 3. 7 | # 8 | #This library is distributed in the hope that it will be useful, 9 | #but WITHOUT ANY WARRANTY; without even the implied warranty of 10 | #MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU 11 | #Lesser General Public License for more details. 12 | # 13 | #You should have received a copy of the GNU Lesser General Public 14 | #License along with this library. If not, see . 15 | ''' 16 | Created on Sep 2, 2011 17 | 18 | @author: tjhunter 19 | ''' 20 | from mm.path_inference.learning_traj_smoother import (TrajectorySmootherRef, 21 | TrajectorySmoother1) 22 | from mm.path_inference.learning_traj_test import simple_traj1, \ 23 | simple_traj2, simple_traj3, simple_traj6, simple_traj5 24 | import numpy as np 25 | # Forces Numpy to consider warnings as errors. 26 | np.seterr(all='raise') 27 | 28 | def check_probs(smoother1, smoother2): 29 | """ Compares the two probability sequences. """ 30 | assert len(smoother1.probabilities) == len(smoother2.probabilities) 31 | for l in range(len(smoother2.probabilities)): 32 | assert np.abs(smoother1.probabilities[l] \ 33 | - smoother2.probabilities[l]).max() < 1e-3, \ 34 | (l, smoother1.probabilities[l], smoother2.probabilities[l]) 35 | 36 | def test_smoother_ref_traj1_1(): 37 | """ test_smoother_ref_traj1_1 """ 38 | traj = simple_traj1() 39 | theta = np.array([0.0, 0.0]) 40 | smoother_ref = TrajectorySmootherRef(traj, theta) 41 | smoother_ref.computeProbs() 42 | smoother_1 = TrajectorySmoother1(traj, theta) 43 | smoother_1.computeProbs() 44 | check_probs(smoother_1, smoother_ref) 45 | 46 | def test_smoother_ref_traj2_1(): 47 | """ test_smoother_ref_traj2_1 """ 48 | traj = simple_traj2() 49 | theta = np.array([1.0, -1.0]) 50 | smoother_ref = TrajectorySmootherRef(traj, theta) 51 | smoother_ref.computeProbs() 52 | smoother_1 = TrajectorySmoother1(traj, theta) 53 | smoother_1.computeProbs() 54 | check_probs(smoother_1, smoother_ref) 55 | 56 | def test_smoother_ref_traj5_1(): 57 | """ test_smoother_ref_traj5_1 """ 58 | traj = simple_traj5() 59 | theta = np.array([-1.0]) 60 | smoother_ref = TrajectorySmootherRef(traj, theta) 61 | smoother_ref.computeProbs() 62 | smoother_1 = TrajectorySmoother1(traj, theta) 63 | smoother_1.computeProbs() 64 | check_probs(smoother_1, smoother_ref) 65 | 66 | def test_smoother_ref_traj6_1(): 67 | """ test_smoother_ref_traj6_1 """ 68 | traj = simple_traj6() 69 | theta = np.array([-1.0, 1.0]) 70 | smoother_ref = TrajectorySmootherRef(traj, theta) 71 | smoother_ref.computeProbs() 72 | smoother_1 = TrajectorySmoother1(traj, theta) 73 | smoother_1.computeProbs() 74 | check_probs(smoother_1, smoother_ref) 75 | 76 | def test_smoother_ref_traj3_1(): 77 | """ test_smoother_ref_traj3_1 . 78 | Just check if it can be computed and does not trigger underflow warnings. """ 79 | traj = simple_traj3() 80 | theta = np.array([-1.0, 1.0]) 81 | smoother_1 = TrajectorySmoother1(traj, theta) 82 | smoother_1.computeProbs() 83 | 84 | def test_smoother_ref_traj3_2(): 85 | """ test_smoother_ref_traj3_1 . 86 | Just check if it can be computed and does not trigger underflow warnings. """ 87 | traj = simple_traj3() 88 | theta = np.array([-1.0, 1.0]) 89 | 90 | smoother_1 = TrajectorySmootherRef(traj, theta) 91 | smoother_1.computeProbs() 92 | -------------------------------------------------------------------------------- /mm/path_inference/learning_traj_test.py: -------------------------------------------------------------------------------- 1 | #Copyright 2011, 2012 Timothy Hunter 2 | # 3 | #This library is free software; you can redistribute it and/or 4 | #modify it under the terms of the GNU Lesser General Public 5 | #License as published by the Free Software Foundation; either 6 | #version 2.1 of the License, or (at your option) version 3. 7 | # 8 | #This library is distributed in the hope that it will be useful, 9 | #but WITHOUT ANY WARRANTY; without even the implied warranty of 10 | #MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU 11 | #Lesser General Public License for more details. 12 | # 13 | #You should have received a copy of the GNU Lesser General Public 14 | #License along with this library. If not, see . 15 | """ Docstring. 16 | """ 17 | from mm.path_inference.learning_traj import LearningTrajectory 18 | 19 | def simple_traj0(): 20 | """ Simple trajectory. 21 | """ 22 | features = [ [ [1.0], [-2.0] ], 23 | [ [-1.0] ], 24 | ] 25 | connections = [ [ (1, 0), (0, 0) ] ] 26 | traj = LearningTrajectory(features, connections) 27 | return traj 28 | 29 | def simple_traj1(): 30 | """ Simple trajectory. 31 | """ 32 | features = [ [ [1.0, 0.0], [-2.0, 0.0] ], 33 | [ [0.0, 1.0] ], 34 | ] 35 | connections = [ [ (1, 0), (0, 0) ] ] 36 | traj = LearningTrajectory(features, connections) 37 | return traj 38 | 39 | def simple_traj2(): 40 | """ Simple trajectory, tests correct indices. 41 | """ 42 | features = [ [ [0.0, 2.0] ], 43 | [ [2.0, 0.0], [-1.0, 0.0] ], 44 | ] 45 | connections = [ [ (0, 0), (0, 1) ] ] 46 | traj = LearningTrajectory(features, connections) 47 | return traj 48 | 49 | def simple_traj3(): 50 | """ Simple trajectory with large features, 51 | breaks non log-based implementations. 52 | """ 53 | features = [ [ [0, 200] ], 54 | [ [200, 0], [-100, 0] ], 55 | ] 56 | connections = [ [ (0, 0), (0, 1) ] ] 57 | traj = LearningTrajectory(features, connections) 58 | return traj 59 | 60 | def simple_traj4(): 61 | """ Simple trajectory. 62 | """ 63 | features = [ [ [1.0, 0.0], [-2.0, 0.0] ], 64 | [ [0.0, 1.0] ], 65 | [ [0.0, -1.0], [0.0, 2.0], [0.5, 0.5] ], 66 | ] 67 | connections = [ [ (1, 0), (0, 0) ], 68 | [ (0, 0), (0, 1), (0, 2) ], 69 | ] 70 | traj = LearningTrajectory(features, connections) 71 | return traj 72 | 73 | def simple_traj5(): 74 | """ Simple trajectory. 75 | """ 76 | features = [ [ [1.0], [-2.0] ], 77 | [ [-1.0] ], 78 | [ [-1.0], [2.0], [0.0] ], 79 | ] 80 | connections = [ [ (1, 0), (0, 0) ], 81 | [ (0, 0), (0, 1), (0, 2) ], 82 | ] 83 | traj = LearningTrajectory(features, connections) 84 | return traj 85 | 86 | 87 | def simple_traj6(): 88 | """ Simple trajectory with more than 3 elements. 89 | """ 90 | features = [ [ [1.0, 0.0], [-2.0, 0.0] ], 91 | [ [0.0, 1.0] ], 92 | [ [0.0, -1.0], [0.0, 2.0], [0.5, 0.5] ], 93 | [ [0.0, -1.0], [0.0, 2.0], [0.5, 0.5] ], 94 | ] 95 | connections = [ [ (1, 0), (0, 0) ], 96 | [ (0, 0), (0, 1), (0, 2) ], 97 | [ (1, 0), (2, 1), (0, 2) ], 98 | ] 99 | traj = LearningTrajectory(features, connections) 100 | return traj 101 | 102 | def simple_traj7(): 103 | """ Simple trajectory, with a disconnected point. 104 | This case should be properly handled. 105 | """ 106 | features = [ [ [1.0], [-2.0] ], 107 | [ [-1.0], [-2.0] ], 108 | ] 109 | connections = [ [ (0, 0) ] ] 110 | traj = LearningTrajectory(features, connections) 111 | return traj 112 | 113 | def simple_traj8(): 114 | """ Simple trajectory, with a single feature and feature values 115 | that are all zeros. 116 | Feature values are small enough to be safe against underflows. 117 | """ 118 | features = [ [ [0.0], [0.0] ], 119 | [ [0.0], [0.0] ], 120 | ] 121 | connections = [ [ (0, 0), (0, 1), (1, 1) ] ] 122 | traj = LearningTrajectory(features, connections) 123 | return traj 124 | 125 | def simple_traj9(): 126 | """ Simple trajectory, with a single feature and feature values 127 | that are all zeros. 128 | Feature values are small enough to be safe against underflows. 129 | """ 130 | features = [ [ [1.0], [2.0] ], 131 | [ [1.0], [2.0] ], 132 | ] 133 | connections = [ [ (0, 0), (0, 1), (1, 1) ] ] 134 | traj = LearningTrajectory(features, connections) 135 | return traj 136 | 137 | def simple_traj10(): 138 | """ Simple trajectory, with a single feature and feature values 139 | that are very big. Should cause some underflow problems. 140 | """ 141 | features = [ [ [1000.0, 0.0], [2000.0, 0.0] ], 142 | [ [500.0, 0.0], [3000.0, 0.0] ], 143 | ] 144 | connections = [ [ (0, 0), (0, 1), (1, 1) ] ] 145 | traj = LearningTrajectory(features, connections) 146 | return traj 147 | 148 | def test_traj7(): 149 | """ test_traj7 """ 150 | traj = simple_traj7() 151 | assert traj.L == 2 152 | 153 | def test_traj1(): 154 | """ Simple test. """ 155 | traj = simple_traj1() 156 | assert traj.L == 2 157 | 158 | def test_traj2(): 159 | """ Simple test 2. """ 160 | traj = simple_traj2() 161 | assert traj.L == 2 162 | 163 | if __name__ == '__main__': 164 | test_traj1() 165 | -------------------------------------------------------------------------------- /mm/path_inference/learning_traj_viterbi.py: -------------------------------------------------------------------------------- 1 | #Copyright 2011, 2012 Timothy Hunter 2 | # 3 | #This library is free software; you can redistribute it and/or 4 | #modify it under the terms of the GNU Lesser General Public 5 | #License as published by the Free Software Foundation; either 6 | #version 2.1 of the License, or (at your option) version 3. 7 | # 8 | #This library is distributed in the hope that it will be useful, 9 | #but WITHOUT ANY WARRANTY; without even the implied warranty of 10 | #MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU 11 | #Lesser General Public License for more details. 12 | # 13 | #You should have received a copy of the GNU Lesser General Public 14 | #License along with this library. If not, see . 15 | ''' 16 | Created on Oct 14, 2011 17 | 18 | @author: tjhunter 19 | 20 | Viterbi filter for trajectories: will find the most likely trace 21 | in a trajectory. 22 | ''' 23 | 24 | import numpy as np 25 | from numpy import dot, exp 26 | from mm.path_inference.HardFilter import HardFilter 27 | 28 | class TrajectoryViterbiRef(HardFilter): 29 | # pylint: disable=W0201 30 | """ Finds the most likely complete trajectory. 31 | 32 | Interesting fields: 33 | - traj: a Trajectory object 34 | - theta: the parameter vector 35 | - most_likely: list of indexes (as long as traj), each of the index 36 | is the element of the most likely trajectory at this step 37 | - most_likely_tree: list of indexes, for each element, gives the index of 38 | the previous element ending this point. 39 | - instant_most_likely: the start index of the most likely trajectory, up 40 | to that index. 41 | Internal: 42 | - partial_probs: (list of lists) the probabilities of trajectories ending 43 | at each element of the trajectory. 44 | """ 45 | 46 | def __init__(self, traj, theta): 47 | HardFilter.__init__(self) 48 | self.traj = traj 49 | self.theta = theta 50 | if not self.traj.L: 51 | raise Exception("Empty trajectory") 52 | if self.theta.shape[0] != self.traj.features_np[0].shape[1]: 53 | raise Exception("Shape of theta is %s while shape of features is %s" % \ 54 | (self.theta.shape, self.traj.features_np[0].shape[1])) 55 | 56 | 57 | def computeProbabilities(self): 58 | """ COmpute probabilities (nothing to do for Viterbi). 59 | """ 60 | pass 61 | 62 | def computeAssignments(self): 63 | self.computeMostLikely() 64 | self.assignments = self.most_likely 65 | 66 | # pylint: disable=W0201 67 | def computeMostLikely(self): 68 | """ Computes most_likely, most_likely_tree, partial_probs 69 | """ 70 | if 'most_likely' in self.__dict__: 71 | return 72 | # Compute the weights 73 | self.ws = [] 74 | for l in range(self.traj.L): 75 | self.ws.append(exp(dot(self.traj.features_np[l], self.theta))) 76 | # Initialization: 77 | self.partial_probs = [] 78 | self.most_likely_tree = [] 79 | self.partial_probs.append(self.ws[0]) 80 | # Fill with some dummy values, so that the indexes match. 81 | self.most_likely_tree.append([None for _ in 82 | range(self.traj.num_choices[0])]) 83 | # Recursion along the trajectory: 84 | for l in range(1, self.traj.L): 85 | this_partial_probs = [] 86 | this_most_likely_tree = [] 87 | for i in range(self.traj.num_choices[l]): 88 | partial_prob = 0 89 | previous = None 90 | # Any backward connection to propagate to that point? 91 | if i in self.traj.connections_backward[l]: 92 | for j in self.traj.connections_backward[l][i]: 93 | new_prob = self.partial_probs[l-1][j] * self.ws[l][i] 94 | if new_prob > partial_prob: 95 | partial_prob = new_prob 96 | previous = j 97 | this_partial_probs.append(partial_prob) 98 | this_most_likely_tree.append(previous) 99 | self.partial_probs.append(this_partial_probs) 100 | self.most_likely_tree.append(this_most_likely_tree) 101 | self.instant_most_likely = [np.argmax(probs) for probs \ 102 | in self.partial_probs] 103 | # At the end, find the most likely partial traj 104 | idx = np.argmax(self.partial_probs[-1]) 105 | # Trace back the most likely traj from the tree. 106 | most_likely = [idx] 107 | for l in range(self.traj.L-1, 0, -1): 108 | most_likely.append(self.most_likely_tree[l][most_likely[-1]]) 109 | most_likely.reverse() # In place 110 | self.most_likely = most_likely 111 | 112 | 113 | class TrajectoryViterbi1(TrajectoryViterbiRef): 114 | """ Finds the most likely complete trajectory, in a way that is safe against 115 | double underflows. 116 | 117 | Interesting fields: 118 | - traj: a Trajectory object 119 | - theta: the parameter vector 120 | - most_likely: list of indexes (as long as traj), each of the index 121 | is the element of the most likely trajectory at this step 122 | - most_likely_tree: list of indexes, for each element, gives the index of 123 | the previous element ending this point. 124 | - instant_most_likely: the start index of the most likely trajectory, up 125 | to that index. 126 | Internal: 127 | - log_partial_probs: (list of lists) the log probabilities of trajectories ending 128 | at each element of the trajectory. (Unscaled values) 129 | - log_ws 130 | """ 131 | 132 | # pylint: disable=W0201 133 | def computeMostLikely(self): 134 | """ Computes most_likely, most_likely_tree, log_partial_probs 135 | """ 136 | if 'most_likely' in self.__dict__: 137 | return 138 | # Compute the weights 139 | self.log_ws = [] 140 | for l in range(self.traj.L): 141 | self.log_ws.append(dot(self.traj.features_np[l], self.theta)) 142 | # Initialization: 143 | self.log_partial_probs = [] 144 | self.most_likely_tree = [] 145 | self.log_partial_probs.append(self.log_ws[0]) 146 | # Fill with some dummy values, so that the indexes match. 147 | self.most_likely_tree.append([None for _ in 148 | range(self.traj.num_choices[0])]) 149 | # Recursion along the trajectory: 150 | minf = float('-inf') 151 | for l in range(1, self.traj.L): 152 | this_partial_probs = [] 153 | this_most_likely_tree = [] 154 | for i in range(self.traj.num_choices[l]): 155 | log_partial_prob = minf 156 | previous = None 157 | if i in self.traj.connections_backward[l]: 158 | for j in self.traj.connections_backward[l][i]: 159 | new_prob = self.log_partial_probs[l-1][j] + self.log_ws[l][i] 160 | if new_prob > log_partial_prob: 161 | log_partial_prob = new_prob 162 | previous = j 163 | this_partial_probs.append(log_partial_prob) 164 | this_most_likely_tree.append(previous) 165 | self.log_partial_probs.append(this_partial_probs) 166 | self.most_likely_tree.append(this_most_likely_tree) 167 | self.instant_most_likely = [np.argmax(log_probs) for log_probs \ 168 | in self.log_partial_probs] 169 | # At the end, find the most likely partial traj 170 | idx = np.argmax(self.log_partial_probs[-1]) 171 | # Trace back the most likely traj from the tree. 172 | most_likely = [idx] 173 | for l in range(self.traj.L-1, 0, -1): 174 | most_likely.append(self.most_likely_tree[l][most_likely[-1]]) 175 | most_likely.reverse() # In place 176 | self.most_likely = most_likely 177 | 178 | class TrajectoryViterbiRealTime1(TrajectoryViterbi1): 179 | """ Uses the instantaneous most likely element of the trajectory. 180 | 181 | Requires no backtracking, but may be wrong and may provide a physically 182 | disconnected trajectory. 183 | """ 184 | 185 | def computeAssignments(self): 186 | self.computeMostLikely() 187 | self.assignments = self.instant_most_likely 188 | -------------------------------------------------------------------------------- /mm/path_inference/learning_traj_viterbi_test.py: -------------------------------------------------------------------------------- 1 | #Copyright 2011, 2012 Timothy Hunter 2 | # 3 | #This library is free software; you can redistribute it and/or 4 | #modify it under the terms of the GNU Lesser General Public 5 | #License as published by the Free Software Foundation; either 6 | #version 2.1 of the License, or (at your option) version 3. 7 | # 8 | #This library is distributed in the hope that it will be useful, 9 | #but WITHOUT ANY WARRANTY; without even the implied warranty of 10 | #MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU 11 | #Lesser General Public License for more details. 12 | # 13 | #You should have received a copy of the GNU Lesser General Public 14 | #License along with this library. If not, see . 15 | ''' 16 | Created on Oct 14, 2011 17 | 18 | @author: tjhunter 19 | 20 | Test suite for viterbi. 21 | ''' 22 | 23 | from mm.path_inference.learning_traj_test import (simple_traj1, 24 | simple_traj2, 25 | simple_traj6, 26 | simple_traj7) 27 | from mm.path_inference.learning_traj_viterbi import (TrajectoryViterbiRef, 28 | TrajectoryViterbi1) 29 | import numpy as np 30 | # Forces Numpy to consider warnings as errors. 31 | np.seterr(all='raise') 32 | 33 | def test_viterbi_ref_1(): 34 | """ test_viterbi_ref_1 35 | """ 36 | traj = simple_traj1() 37 | theta = np.array([1.0, 0.0]) 38 | viterbi = TrajectoryViterbiRef(traj, theta) 39 | viterbi.computeMostLikely() 40 | assert len(viterbi.most_likely) == traj.L 41 | assert viterbi.most_likely[0] == 0 42 | assert viterbi.most_likely[1] == 0 43 | 44 | def test_viterbi_1_1(): 45 | """ test_viterbi_1_1 46 | """ 47 | traj = simple_traj1() 48 | theta = np.array([1.0, 0.0]) 49 | viterbi_ref = TrajectoryViterbiRef(traj, theta) 50 | viterbi_ref.computeMostLikely() 51 | viterbi_1 = TrajectoryViterbi1(traj, theta) 52 | viterbi_1.computeMostLikely() 53 | assert len(viterbi_1.most_likely) == traj.L 54 | for l in range(traj.L): 55 | assert viterbi_1.most_likely[l] == viterbi_ref.most_likely[l] 56 | assert traj.num_choices[l] == len(viterbi_ref.most_likely_tree[l]) 57 | for i in range(traj.num_choices[l]): 58 | assert viterbi_ref.most_likely_tree[l][i] == \ 59 | viterbi_1.most_likely_tree[l][i] 60 | 61 | def test_viterbi_1_2(): 62 | """ test_viterbi_1_2 63 | """ 64 | traj = simple_traj2() 65 | theta = np.array([1.0, -1.0]) 66 | viterbi_ref = TrajectoryViterbiRef(traj, theta) 67 | viterbi_ref.computeMostLikely() 68 | viterbi_1 = TrajectoryViterbi1(traj, theta) 69 | viterbi_1.computeMostLikely() 70 | assert len(viterbi_1.most_likely) == traj.L 71 | for l in range(traj.L): 72 | assert viterbi_1.most_likely[l] == viterbi_ref.most_likely[l] 73 | assert traj.num_choices[l] == len(viterbi_ref.most_likely_tree[l]) 74 | for i in range(traj.num_choices[l]): 75 | assert viterbi_ref.most_likely_tree[l][i] == \ 76 | viterbi_1.most_likely_tree[l][i] 77 | 78 | def test_viterbi_1_6(): 79 | """ test_viterbi_1_6 80 | """ 81 | traj = simple_traj6() 82 | theta = np.array([1.0, -1.0]) 83 | viterbi_ref = TrajectoryViterbiRef(traj, theta) 84 | viterbi_ref.computeMostLikely() 85 | viterbi_1 = TrajectoryViterbi1(traj, theta) 86 | viterbi_1.computeMostLikely() 87 | assert len(viterbi_1.most_likely) == traj.L 88 | for l in range(traj.L): 89 | assert viterbi_1.most_likely[l] == viterbi_ref.most_likely[l] 90 | assert traj.num_choices[l] == len(viterbi_ref.most_likely_tree[l]) 91 | for i in range(traj.num_choices[l]): 92 | assert viterbi_ref.most_likely_tree[l][i] == \ 93 | viterbi_1.most_likely_tree[l][i] 94 | 95 | def test_viterbi_1_7(): 96 | """ test_viterbi_1_7 97 | """ 98 | traj = simple_traj7() 99 | theta = np.array([1.0]) 100 | viterbi_ref = TrajectoryViterbiRef(traj, theta) 101 | viterbi_ref.computeMostLikely() 102 | viterbi_1 = TrajectoryViterbi1(traj, theta) 103 | viterbi_1.computeMostLikely() 104 | assert len(viterbi_1.most_likely) == traj.L 105 | for l in range(traj.L): 106 | assert viterbi_1.most_likely[l] == viterbi_ref.most_likely[l] 107 | assert traj.num_choices[l] == len(viterbi_ref.most_likely_tree[l]) 108 | for i in range(traj.num_choices[l]): 109 | assert viterbi_ref.most_likely_tree[l][i] == \ 110 | viterbi_1.most_likely_tree[l][i] 111 | -------------------------------------------------------------------------------- /mm/path_inference/mapping/Counter.py: -------------------------------------------------------------------------------- 1 | ''' 2 | Copyright 2011, 2012 Timothy Hunter 3 | 4 | This library is free software; you can redistribute it and/or 5 | modify it under the terms of the GNU Lesser General Public 6 | License as published by the Free Software Foundation; either 7 | version 2.1 of the License, or (at your option) version 3. 8 | 9 | This library is distributed in the hope that it will be useful, 10 | but WITHOUT ANY WARRANTY; without even the implied warranty of 11 | MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU 12 | Lesser General Public License for more details. 13 | 14 | You should have received a copy of the GNU Lesser General Public 15 | License along with this library. If not, see . 16 | ''' 17 | # pylint: disable=W0105 18 | ''' 19 | Created on Nov 8, 2011 20 | 21 | @author: tjhunter 22 | ''' 23 | from mm.path_inference.json import decode_StateCollection 24 | 25 | class Counter(object): 26 | """ Provide a counter class for collecting statistics on the objects. 27 | """ 28 | def __init__(self, filter_non_hired=False): 29 | self.count = 0 30 | self.num_states = [] 31 | self.filter_non_hired = filter_non_hired 32 | def call(self, dct): 33 | ''' closure ''' 34 | sc = decode_StateCollection(dct) 35 | # if self.filter_non_hired and sc.hired != True: 36 | # self.num_states.append(0) 37 | # else: 38 | self.num_states.append(len(sc.states)) 39 | self.count += 1 40 | return None 41 | 42 | -------------------------------------------------------------------------------- /mm/path_inference/mapping/FancyFeatureMapper.py: -------------------------------------------------------------------------------- 1 | ''' 2 | Copyright 2011, 2012 Timothy Hunter 3 | 4 | This library is free software; you can redistribute it and/or 5 | modify it under the terms of the GNU Lesser General Public 6 | License as published by the Free Software Foundation; either 7 | version 2.1 of the License, or (at your option) version 3. 8 | 9 | This library is distributed in the hope that it will be useful, 10 | but WITHOUT ANY WARRANTY; without even the implied warranty of 11 | MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU 12 | Lesser General Public License for more details. 13 | 14 | You should have received a copy of the GNU Lesser General Public 15 | License along with this library. If not, see . 16 | ''' 17 | # pylint: disable=W0105 18 | ''' 19 | Created on Nov 22, 2011 20 | 21 | @author: tjhunter 22 | ''' 23 | 24 | from FeatureMapper import FeatureMapper 25 | from utils import mm_dis 26 | from mm.path_inference.json import decode_link_id, decode_LatLng 27 | import math 28 | 29 | def path_length(p): 30 | """ Returns the lenght of a path (meters). """ 31 | res = 0.0 32 | for (latlng1, latlng2) in zip(p.latlngs[1:], p.latlngs[:-1]): 33 | res += mm_dis(latlng1, latlng2) 34 | return res 35 | 36 | def num_stops(p, links): 37 | """ Returns the number of stops on a path. 38 | """ 39 | return sum([links[lid]['stop'] for lid in p.links[:-1]]) 40 | 41 | def num_signals(p, links): 42 | """ Returns the number of red lights on a path. 43 | """ 44 | return sum([links[lid]['signal'] for lid in p.links[:-1]]) 45 | 46 | def angle(lid1, lid2, links): 47 | """ Returns the angle (in radians) between two paths. 48 | """ 49 | lla1 = decode_LatLng(links[lid1]['geom'][0]) 50 | lla2 = decode_LatLng(links[lid1]['geom'][-1]) 51 | llb1 = decode_LatLng(links[lid2]['geom'][0]) 52 | llb2 = decode_LatLng(links[lid2]['geom'][-1]) 53 | dxa = lla1.lat - lla2.lat 54 | dxb = llb1.lat - llb2.lat 55 | dya = lla1.lng - lla2.lng 56 | dyb = llb1.lng - llb2.lng 57 | na = math.sqrt(dxa * dxa + dya * dya) 58 | nb = math.sqrt(dxb * dxb + dyb * dyb) 59 | dxa /= na 60 | dya /= na 61 | dxb /= nb 62 | dyb /= nb 63 | return dxa * dxb + dya * dyb 64 | 65 | def num_left_turns(p, links): 66 | """ The number of left turns on a path. 67 | """ 68 | cosines = [angle(lid1, lid2, links) for (lid1, lid2) in zip(p.links[:-1], p.links[1:])] 69 | res = 0.0 70 | for c in cosines: 71 | if c > 0.2: 72 | res += 1 73 | return res 74 | 75 | def num_right_turns(p, links): 76 | """ The number of right turns on a path. 77 | """ 78 | cosines = [angle(lid1, lid2, links) \ 79 | for (lid1, lid2) in zip(p.links[:-1], p.links[1:])] 80 | res = 0.0 81 | for c in cosines: 82 | if c < -0.2: 83 | res += 1 84 | return res 85 | 86 | def average_tt(p, links): 87 | """ The average travel time on a path. 88 | """ 89 | total_tt = 0.0 90 | # Sum over all the complete links: 91 | for lid in p.links: 92 | total_tt += links[lid]['length'] / links[lid]['speed_limit'] 93 | total_tt -= p.start.offset / links[p.links[0]]['speed_limit'] 94 | end_link_length = links[p.links[-1]]['length'] 95 | total_tt -= (end_link_length - p.end.offset) \ 96 | / links[p.links[-1]]['speed_limit'] 97 | return total_tt 98 | 99 | def average_speed(p, links): 100 | """ Average speed on a path. 101 | """ 102 | l = path_length(p) 103 | if l < 1: 104 | return 0 105 | else: 106 | return average_tt(p, links) / path_length(p) 107 | 108 | def max_lanes(p, links): 109 | """ Returns the maximum number of lanes on all links of a path. 110 | """ 111 | return max([links[lid]['lanes'] for lid in p.links]) 112 | 113 | def min_lanes(p, links): 114 | """ Returns the mainimum number of lanes on all links of a path. 115 | """ 116 | return min([links[lid]['lanes'] for lid in p.links]) 117 | 118 | class FancyFeatureMapper(FeatureMapper): 119 | """ Feature mapper class for fancy features. 120 | 121 | TODO: doc 122 | """ 123 | NUM_PATH_FEATURES = 9 124 | NUM_STATE_FEATURES = 1 125 | # Some random buggy warning: 126 | def __init__(self, links): 127 | FeatureMapper.__init__(self) 128 | self.num_path_features = FancyFeatureMapper.NUM_PATH_FEATURES 129 | self.num_state_features = FancyFeatureMapper.NUM_STATE_FEATURES 130 | self.links = dict([(decode_link_id(link_dct['link_id']), link_dct) \ 131 | for link_dct in links]) 132 | 133 | def path_features(self, path): 134 | return [path_length(path), num_stops(path, self.links), \ 135 | num_signals(path, self.links), num_left_turns(path, self.links), \ 136 | num_right_turns(path, self.links), \ 137 | average_tt(path, self.links), average_speed(path, self.links), \ 138 | max_lanes(path, self.links), min_lanes(path, self.links)] 139 | -------------------------------------------------------------------------------- /mm/path_inference/mapping/FeatureMapper.py: -------------------------------------------------------------------------------- 1 | ''' 2 | Copyright 2011, 2012 Timothy Hunter 3 | 4 | This library is free software; you can redistribute it and/or 5 | modify it under the terms of the GNU Lesser General Public 6 | License as published by the Free Software Foundation; either 7 | version 2.1 of the License, or (at your option) version 3. 8 | 9 | This library is distributed in the hope that it will be useful, 10 | but WITHOUT ANY WARRANTY; without even the implied warranty of 11 | MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU 12 | Lesser General Public License for more details. 13 | 14 | You should have received a copy of the GNU Lesser General Public 15 | License along with this library. If not, see . 16 | ''' 17 | # pylint: disable=W0105 18 | ''' 19 | Created on Nov 8, 2011 20 | 21 | @author: tjhunter 22 | ''' 23 | 24 | from mm.path_inference.json import decode_Path, decode_StateCollection 25 | from utils import mm_dis 26 | import numpy as np 27 | 28 | def path_length(p): 29 | """ Length of a path. """ 30 | res = 0.0 31 | for (latlng1, latlng2) in zip(p.latlngs[1:], p.latlngs[:-1]): 32 | res += mm_dis(latlng1, latlng2) 33 | return res 34 | 35 | 36 | class FeatureMapper(object): 37 | ''' Maps a track to feature vectors interleaved with connection data. 38 | 39 | Each vector contains first all the information relative to a path, and 40 | then to a state. 41 | For each path and each state the feature vectors are stored together 42 | row-wise in a 2d array. 43 | ''' 44 | NUM_PATH_FEATURES = 1 45 | NUM_STATE_FEATURES = 1 46 | 47 | def __init__(self): 48 | self.count = 0 49 | self.feature_trajs = [] 50 | self.num_path_features = FeatureMapper.NUM_PATH_FEATURES 51 | self.num_state_features = FeatureMapper.NUM_STATE_FEATURES 52 | 53 | def path_features(self, path): 54 | """ Feature subvector for paths. 55 | """ 56 | return [-path_length(path)] 57 | 58 | def state_features(self, sc, i): 59 | """ Feature subvector for states. 60 | """ 61 | state = sc.states[i] 62 | d = mm_dis(state.gps_pos, sc.gps_pos) 63 | return [-0.5 * d * d] 64 | 65 | def call(self, dct): 66 | """ Closure. 67 | """ 68 | (trans1, paths_dct, trans2, sc_dct) = dct 69 | num_paths = len(paths_dct) 70 | vector_size = self.num_path_features+self.num_state_features 71 | empty_paths = [0 for _ in range(self.num_path_features)] 72 | empty_states = [0 for _ in range(self.num_state_features)] 73 | feats_paths = np.zeros((num_paths, vector_size)) 74 | for i in range(num_paths): 75 | path_dct = paths_dct[i] 76 | path = decode_Path(path_dct) 77 | phi = np.array(self.path_features(path) + empty_states) 78 | feats_paths[i, ::] = phi 79 | sc = decode_StateCollection(sc_dct) 80 | num_states = len(sc.states) 81 | feats_states = np.zeros((num_states, vector_size)) 82 | for j in range(len(sc.states)): 83 | phi = np.array(empty_paths + self.state_features(sc, j)) 84 | feats_states[j, ::] = phi 85 | if self.count == 0: 86 | self.feature_trajs.append(feats_states) 87 | else: 88 | # Debugging checks 89 | num_previous_states = len(self.feature_trajs[-1]) 90 | for (u, v) in trans1: 91 | assert u >= 0 92 | assert v >= 0 93 | assert u < num_previous_states, ('trans1', (u, v), u) 94 | assert v < num_paths, ('trans1', (u, v), v) 95 | for (u, v) in trans2: 96 | assert u >= 0 97 | assert v >= 0 98 | assert u < num_paths, ('trans2', (u, v), u) 99 | assert v < num_states, ('trans2', (u, v), v) 100 | self.feature_trajs.append(trans1) 101 | self.feature_trajs.append(feats_paths) 102 | self.feature_trajs.append(trans2) 103 | self.feature_trajs.append(feats_states) 104 | self.count += 1 105 | return None 106 | -------------------------------------------------------------------------------- /mm/path_inference/mapping/FlowAnalysis.py: -------------------------------------------------------------------------------- 1 | ''' 2 | Copyright 2011, 2012 Timothy Hunter 3 | 4 | This library is free software; you can redistribute it and/or 5 | modify it under the terms of the GNU Lesser General Public 6 | License as published by the Free Software Foundation; either 7 | version 2.1 of the License, or (at your option) version 3. 8 | 9 | This library is distributed in the hope that it will be useful, 10 | but WITHOUT ANY WARRANTY; without even the implied warranty of 11 | MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU 12 | Lesser General Public License for more details. 13 | 14 | You should have received a copy of the GNU Lesser General Public 15 | License along with this library. If not, see . 16 | ''' 17 | # pylint: disable=W0105 18 | ''' 19 | Created on Nov 8, 2011 20 | 21 | @author: tjhunter 22 | ''' 23 | from mm.path_inference.json import decode_Path, decode_StateCollection 24 | import collections 25 | 26 | class FlowAnalysis(object): 27 | """ Flow analysis: finds all reachable points from a start point. 28 | """ 29 | def __init__(self): 30 | self.count = 0 31 | self.flow_units = [] 32 | self.reachable = [] 33 | self.num_reachable = [] 34 | self.previous_sc = None # Debugging 35 | def call(self, dct): 36 | """ Calling function, to create a closure. 37 | """ 38 | (trans1, paths_dct, trans2, sc_dct) = dct 39 | sc = decode_StateCollection(sc_dct) 40 | num_paths = len(paths_dct) 41 | num_states = len(sc.states) 42 | paths = [decode_Path(path_dct) for path_dct in paths_dct] 43 | # Due to a bug in the way the closures are used, we nned 44 | # to make sure we are not in the first element. 45 | # Make a dictionary of previous->paths 46 | dic1 = collections.defaultdict(list) 47 | for (u, v) in trans1: 48 | # Debugging: make sure the path corresponds to the previous point 49 | assert u >= 0 50 | assert v >= 0 51 | assert v < num_paths 52 | if self.previous_sc: 53 | assert u < len(self.previous_sc.states) 54 | # Make sure it is proper connectivity 55 | state = self.previous_sc.states[u] 56 | path = paths[v] 57 | assert state.link_id == path.links[0] 58 | assert state.link_id == path.start.link_id 59 | assert state.offset == path.start.offset 60 | dic1[u].append(v) 61 | dic2 = collections.defaultdict(list) 62 | for (v, u) in trans2: 63 | assert u >= 0 64 | assert v >= 0 65 | assert v < num_paths 66 | assert u < num_states 67 | # Check connectivity 68 | state = sc.states[u] 69 | path = paths[v] 70 | if state.link_id != path.links[-1]: 71 | print sc.states 72 | print path 73 | assert state.link_id == path.links[-1], \ 74 | (path.links[-1], state.link_id, v, u, state, path) 75 | assert state.link_id == path.end.link_id, (state.link_id, \ 76 | path.end.link_id) 77 | assert state.offset == path.end.offset, (state.offset, path.end.offset) 78 | dic2[v].append(u) 79 | reachable_paths = set(reduce(lambda l1, l2:l1+l2, \ 80 | [dic1[u] for u in self.reachable], [])) 81 | if not reachable_paths: 82 | print("No reachable paths") 83 | reachable_states = set(reduce(lambda l1, l2:l1+l2, \ 84 | [dic2[u] for u in reachable_paths], [])) 85 | self.num_reachable.append(len(reachable_states)) 86 | if not reachable_states: 87 | print 'break at %i' % self.count 88 | self.reachable = range(num_states) 89 | self.flow_units.append([]) 90 | else: 91 | self.reachable = reachable_states 92 | self.flow_units[-1].append(self.count) 93 | self.count += 1 94 | self.previous_sc = sc 95 | return None 96 | 97 | -------------------------------------------------------------------------------- /mm/path_inference/mapping/HFDecimation.py: -------------------------------------------------------------------------------- 1 | ''' 2 | Copyright 2011, 2012 Timothy Hunter 3 | 4 | This library is free software; you can redistribute it and/or 5 | modify it under the terms of the GNU Lesser General Public 6 | License as published by the Free Software Foundation; either 7 | version 2.1 of the License, or (at your option) version 3. 8 | 9 | This library is distributed in the hope that it will be useful, 10 | but WITHOUT ANY WARRANTY; without even the implied warranty of 11 | MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU 12 | Lesser General Public License for more details. 13 | 14 | You should have received a copy of the GNU Lesser General Public 15 | License along with this library. If not, see . 16 | ''' 17 | # pylint: disable=W0105 18 | ''' 19 | Created on Nov 13, 2011 20 | 21 | @author: tjhunter 22 | ''' 23 | from mm.path_inference.json import encode_Path, decode_StateCollection, \ 24 | decode_Path, encode_StateCollection 25 | from decimation import decimate_point, decimate_path_simple, merge_path_sequence 26 | 27 | class HFDecimation(object): 28 | """ Builds the reference track. 29 | 30 | Sends back the new elements of the trajectory to the caller function. 31 | """ 32 | def __init__(self, decimation_factor, probas, viterbi_idxs): 33 | self.count = 0 34 | # Indexes of the most likely element at each step. 35 | # Invariant: the start point is never null, except at start. 36 | # Invariant: if the end point is not null, the paths, start_trans 37 | # and end_trans are well defined and between the start and end point. 38 | self.most_likely_indexes = [] 39 | self.previous_correspondance = None 40 | self.decimation_factor = decimation_factor 41 | self.viterbi_idxs = viterbi_idxs 42 | self.probas = probas 43 | # SC in decimated format 44 | # Start point of the sequence 45 | self.start_point = None 46 | # index mapping of the start of the sequence. 47 | self.start_mapping = None 48 | # SC in decimate format 49 | # End point of the sequence 50 | self.end_point = None 51 | # Mapping of the end poit of the sequence. 52 | self.end_mapping = None 53 | # Start transition of the current path. 54 | self.start_trans = None 55 | # End transition of the current path. 56 | self.end_trans = None 57 | # Current paths in the transition between start point and end point. 58 | self.paths = None 59 | # Current best index amongst the paths. 60 | self.best_idx = None 61 | 62 | def call(self, dct): 63 | """ Closure. 64 | """ 65 | # All the hard elements are already coded in mapping. 66 | # Here is how the mapping procedure works in high frequency: 67 | # At first, a new point is added to the beginning, with empty paths. 68 | # When a new point (with its paths) arrives, two cases: 69 | # We were right after the start point, then we simply add 70 | # the path and the point. Otherwise, we decimate paths and points, 71 | # and we merge the paths together. 72 | (trans1, paths_dct, trans2, sc_dct) = dct 73 | paths = [decode_Path(path_dct) for path_dct in paths_dct] 74 | del paths_dct 75 | sc = decode_StateCollection(sc_dct) 76 | del sc_dct 77 | # The index of sc 78 | point_idx = 2 * self.count 79 | # The index of the paths 80 | path_idx = 2 * self.count - 1 81 | self.count += 1 82 | (new_decim_sc, new_end_mapping) = \ 83 | decimate_point(sc, self.probas[point_idx], self.viterbi_idxs[point_idx]) 84 | new_most_likely_sc_idx = None 85 | if point_idx >= 0: 86 | assert self.viterbi_idxs[point_idx] in new_end_mapping, \ 87 | (point_idx, new_end_mapping) 88 | new_most_likely_sc_idx = new_end_mapping[self.viterbi_idxs[point_idx]] 89 | # First element?? 90 | if not self.start_point: 91 | self.start_point = new_decim_sc 92 | self.start_mapping = new_end_mapping 93 | assert new_most_likely_sc_idx is not None 94 | self.most_likely_indexes.append(new_most_likely_sc_idx) 95 | return ([], [], [], encode_StateCollection(self.start_point)) 96 | # Try to add a new elment: 97 | if self.paths is None: 98 | assert self.start_mapping is not None 99 | assert self.start_point is not None 100 | # Start a new set of paths 101 | (new_trans1, decimated_paths, new_trans2, paths_mapping) = \ 102 | decimate_path_simple(self.start_mapping, trans1, paths, \ 103 | trans2, new_end_mapping) 104 | assert self.viterbi_idxs[point_idx] in new_end_mapping 105 | assert self.viterbi_idxs[path_idx] in paths_mapping 106 | assert (self.start_mapping[self.viterbi_idxs[path_idx-1]], \ 107 | paths_mapping[self.viterbi_idxs[path_idx]]) in new_trans1 108 | assert (paths_mapping[self.viterbi_idxs[path_idx]], \ 109 | new_end_mapping[self.viterbi_idxs[point_idx]]) in new_trans2 110 | self.end_point = new_decim_sc 111 | self.end_mapping = new_end_mapping 112 | self.start_trans = new_trans1 113 | self.paths = decimated_paths 114 | assert self.paths, self.paths 115 | self.best_idx = paths_mapping[self.viterbi_idxs[path_idx]] 116 | self.end_trans = new_trans2 117 | else: 118 | assert self.start_mapping is not None 119 | assert self.start_point is not None 120 | assert self.start_trans is not None 121 | assert self.end_trans is not None 122 | assert self.paths is not None 123 | # First decimate the paths 124 | (new_trans1, decimated_paths, new_trans2, paths_mapping) = \ 125 | decimate_path_simple(self.end_mapping, trans1, paths, trans2, \ 126 | new_end_mapping) 127 | assert self.viterbi_idxs[path_idx] in paths_mapping 128 | assert self.viterbi_idxs[path_idx-1] in self.end_mapping 129 | assert (self.end_mapping[self.viterbi_idxs[path_idx-1]], \ 130 | paths_mapping[self.viterbi_idxs[path_idx]]) in new_trans1 131 | assert (paths_mapping[self.viterbi_idxs[path_idx]], \ 132 | new_end_mapping[self.viterbi_idxs[point_idx]]) in new_trans2 133 | best_idx2 = paths_mapping[self.viterbi_idxs[path_idx]] 134 | # Merge the paths together 135 | (merged_trans1, merged_paths, merged_trans2, merged_best_idx) = \ 136 | merge_path_sequence(self.start_trans, self.paths, self.end_trans, \ 137 | new_trans1, decimated_paths, new_trans2, \ 138 | self.best_idx, best_idx2) 139 | self.end_point = new_decim_sc 140 | self.end_mapping = new_end_mapping 141 | self.start_trans = merged_trans1 142 | self.paths = merged_paths 143 | self.best_idx = merged_best_idx 144 | self.end_trans = merged_trans2 145 | # Time to send a new element to the output and restart? 146 | if (self.count-1) % self.decimation_factor == 0: 147 | assert self.paths 148 | assert self.end_trans 149 | assert self.start_trans 150 | assert self.best_idx is not None 151 | encoded_paths = [encode_Path(path) for path in self.paths] 152 | print len(encoded_paths), " paths", len(self.end_point.states), " states" 153 | result = (self.start_trans, encoded_paths, \ 154 | self.end_trans, encode_StateCollection(self.end_point)) 155 | # Adding the most likely index of the path and of the next point. 156 | self.most_likely_indexes.append(self.best_idx) 157 | assert new_most_likely_sc_idx is not None 158 | self.most_likely_indexes.append(new_most_likely_sc_idx) 159 | # Restart computations: 160 | self.start_point = self.end_point 161 | self.start_mapping = self.end_mapping 162 | del self.paths 163 | self.start_trans = None 164 | self.end_trans = None 165 | self.paths = None 166 | self.best_idx = None 167 | return result 168 | # Nothing to return for this input, continuing. 169 | return None 170 | -------------------------------------------------------------------------------- /mm/path_inference/mapping/PathCounter.py: -------------------------------------------------------------------------------- 1 | ''' 2 | Copyright 2011, 2012 Timothy Hunter 3 | 4 | This library is free software; you can redistribute it and/or 5 | modify it under the terms of the GNU Lesser General Public 6 | License as published by the Free Software Foundation; either 7 | version 2.1 of the License, or (at your option) version 3. 8 | 9 | This library is distributed in the hope that it will be useful, 10 | but WITHOUT ANY WARRANTY; without even the implied warranty of 11 | MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU 12 | Lesser General Public License for more details. 13 | 14 | You should have received a copy of the GNU Lesser General Public 15 | License along with this library. If not, see . 16 | ''' 17 | # pylint: disable=W0105 18 | ''' 19 | Created on Nov 8, 2011 20 | 21 | @author: tjhunter 22 | ''' 23 | class PathCounter(object): 24 | """ Use it as a closure to count the paths. 25 | """ 26 | def __init__(self): 27 | self.count = 0 28 | self.num_paths = [] 29 | def call(self, dct): 30 | """ Closure call. 31 | """ 32 | (_, paths, _, _) = dct 33 | self.num_paths.append(len(paths)) 34 | self.count += 1 35 | -------------------------------------------------------------------------------- /mm/path_inference/mapping/SparseDecimation.py: -------------------------------------------------------------------------------- 1 | #Copyright 2011, 2012 Timothy Hunter 2 | # 3 | #This library is free software; you can redistribute it and/or 4 | #modify it under the terms of the GNU Lesser General Public 5 | #License as published by the Free Software Foundation; either 6 | #version 2.1 of the License, or (at your option) version 3. 7 | # 8 | #This library is distributed in the hope that it will be useful, 9 | #but WITHOUT ANY WARRANTY; without even the implied warranty of 10 | #MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU 11 | #Lesser General Public License for more details. 12 | # 13 | #You should have received a copy of the GNU Lesser General Public 14 | #License along with this library. If not, see . 15 | ''' 16 | Created on Nov 27, 2011 17 | 18 | @author: tjhunter 19 | ''' 20 | from mm.path_inference.json import encode_Path, decode_StateCollection, \ 21 | decode_Path, encode_StateCollection 22 | from decimation import merge_path 23 | 24 | class SparseDecimation(object): 25 | """ Builds the reference track for some decimation rates higher than one 26 | second. Drops some paths if too many paths are computed between two points, 27 | but ensures that the true path is kept. 28 | 29 | Sends back the new elements of the trajectory to the caller function as JSON 30 | dictionaries. 31 | 32 | Takes a one-second decimated track and builds from it a new track with a higher 33 | decimation rate. 34 | 35 | This class is somewhat simpler than HFDecimation as it does not need to merge 36 | back some paths together, and it does not need to perform point decimation. 37 | """ 38 | def __init__(self, decimation_factor, viterbi_idxs, path_builder): 39 | self.count = 0 40 | self.path_builder = path_builder 41 | # Indexes of the most likely element at each step. 42 | # Invariant: the start point is never null, except at start. 43 | # Invariant: if the end point is not null, the paths, start_trans 44 | # and end_trans are well defined and between the start and end point. 45 | self.most_likely_indexes = [] 46 | self.previous_correspondance = None 47 | self.decimation_factor = decimation_factor 48 | self.viterbi_idxs = viterbi_idxs 49 | # SC in decimated format 50 | # Start point of the sequence 51 | self.start_point = None 52 | # The best path from the start point to the end point. 53 | # It is unique. 54 | self.best_path = None 55 | 56 | def call(self, dct): 57 | """ Closure. 58 | """ 59 | (_, paths_dct, _, sc_dct) = dct 60 | sc = decode_StateCollection(sc_dct) 61 | del sc_dct 62 | # The index of sc 63 | point_idx = 2 * self.count 64 | # The index of the paths 65 | path_idx = 2 * self.count - 1 66 | self.count += 1 67 | new_most_likely_sc_idx = self.viterbi_idxs[point_idx] 68 | # First element?? 69 | if not self.start_point: 70 | self.start_point = sc 71 | assert new_most_likely_sc_idx is not None 72 | self.most_likely_indexes.append(new_most_likely_sc_idx) 73 | return ([], [], [], encode_StateCollection(self.start_point)) 74 | # Only decode the most likely path, we do not need the other paths. 75 | new_best_path = decode_Path(paths_dct[self.viterbi_idxs[path_idx]]) 76 | del paths_dct 77 | # Try to add a new element: 78 | # All this code is much more complicated than it should be now. 79 | if self.best_path is None: 80 | assert self.start_point is not None 81 | self.best_path = new_best_path 82 | else: 83 | assert self.start_point is not None 84 | self.best_path = merge_path(self.best_path, new_best_path) 85 | assert self.best_path.start in self.start_point.states 86 | assert self.best_path.end in sc.states 87 | # Time to send a new element to the output and restart? 88 | if (self.count-1) % self.decimation_factor == 0: 89 | # Time to find all the other paths 90 | (other_trans1, other_paths, other_trans2) = \ 91 | self.path_builder.getPathsBetweenCollections(self.start_point, sc) 92 | # If we have the first path already in, no need to integrate it: 93 | try: 94 | best_path_idx = other_paths.index(self.best_path) 95 | new_trans1 = other_trans1 96 | new_paths = other_paths 97 | new_trans2 = other_trans2 98 | except ValueError: 99 | # We need to append it: 100 | best_path_idx = len(other_paths) 101 | prev_best_idx = self.most_likely_indexes[-1] 102 | new_trans1 = other_trans1 + [(prev_best_idx, best_path_idx)] 103 | new_paths = other_paths + [self.best_path] 104 | new_trans2 = other_trans2 + [(best_path_idx, new_most_likely_sc_idx)] 105 | 106 | encoded_paths = [encode_Path(path) for path in new_paths] 107 | print len(encoded_paths), " paths", len(sc.states), " states", 108 | if len(other_paths) != len(new_paths): 109 | print '(forced insertion)' 110 | else: 111 | print '' 112 | result = (new_trans1, encoded_paths, \ 113 | new_trans2, encode_StateCollection(sc)) 114 | # Adding the most likely index of the path and of the next point. 115 | self.most_likely_indexes.append(best_path_idx) 116 | assert new_most_likely_sc_idx is not None 117 | self.most_likely_indexes.append(new_most_likely_sc_idx) 118 | # Restart computations: 119 | self.start_point = sc 120 | self.best_path = None 121 | return result 122 | # Nothing to return for this input, continuing. 123 | return None 124 | -------------------------------------------------------------------------------- /mm/path_inference/mapping/__init__.py: -------------------------------------------------------------------------------- 1 | ''' 2 | Copyright 2011, 2012 Timothy Hunter 3 | 4 | This library is free software; you can redistribute it and/or 5 | modify it under the terms of the GNU Lesser General Public 6 | License as published by the Free Software Foundation; either 7 | version 2.1 of the License, or (at your option) version 3. 8 | 9 | This library is distributed in the hope that it will be useful, 10 | but WITHOUT ANY WARRANTY; without even the implied warranty of 11 | MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU 12 | Lesser General Public License for more details. 13 | 14 | You should have received a copy of the GNU Lesser General Public 15 | License along with this library. If not, see . 16 | ''' 17 | # pylint: disable=W0105 18 | """ Mapping data package. 19 | """ 20 | # Standard imports 21 | from Counter import Counter 22 | try: 23 | from FancyFeatureMapper import FancyFeatureMapper 24 | from FeatureMapper import FeatureMapper 25 | except ImportError: 26 | pass 27 | from HFDecimation import HFDecimation 28 | from FlowAnalysis import FlowAnalysis 29 | from PathCounter import PathCounter 30 | from SparseDecimation import SparseDecimation 31 | -------------------------------------------------------------------------------- /mm/path_inference/mapping/decimation.py: -------------------------------------------------------------------------------- 1 | ''' 2 | Copyright 2011, 2012 Timothy Hunter 3 | 4 | This library is free software; you can redistribute it and/or 5 | modify it under the terms of the GNU Lesser General Public 6 | License as published by the Free Software Foundation; either 7 | version 2.1 of the License, or (at your option) version 3. 8 | 9 | This library is distributed in the hope that it will be useful, 10 | but WITHOUT ANY WARRANTY; without even the implied warranty of 11 | MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU 12 | Lesser General Public License for more details. 13 | 14 | You should have received a copy of the GNU Lesser General Public 15 | License along with this library. If not, see . 16 | ''' 17 | # pylint: disable=W0105 18 | ''' 19 | Created on Nov 10, 2011 20 | 21 | @author: tjhunter 22 | ''' 23 | from mm.path_inference.structures import Path, StateCollection 24 | from collections import defaultdict 25 | 26 | def decimate_point(sc, proba_vec, best_idx=None): 27 | """ Returns a pair that contains: a new state collection with the decimated 28 | points and a list of indexes that provides a correspondence between the old 29 | points and the new points. 30 | 31 | Parameters: 32 | - best_idx: if specified, the returned state collection will contain the 33 | spot specified by the corresponding index. 34 | 35 | Returns: (new_sc, mapping) with new_sc a new StateCollection and mapping 36 | a mapping from old indexes (in sc) to new indexes (in new_sc) 37 | """ 38 | n = len(sc.states) 39 | assert n == len(proba_vec) 40 | # Build a dictionary that maps a link id to the most likely point on that 41 | # link id. 42 | d = {} 43 | for i in range(n): 44 | state = sc.states[i] 45 | link_id = state.link_id 46 | if link_id not in d: 47 | d[link_id] = i 48 | else: 49 | if proba_vec[d[link_id]] < proba_vec[i]: 50 | d[link_id] = i 51 | # Make sure we store the best index if needed be: 52 | if best_idx is not None: 53 | d[sc.states[best_idx].link_id] = best_idx 54 | new_states = [] 55 | mapping = {} 56 | j = 0 57 | for i in d.values(): 58 | new_states.append(sc.states[i]) 59 | mapping[i] = j 60 | j += 1 61 | new_sc = StateCollection(sc.id, new_states, sc.gps_pos, sc.time) 62 | return (new_sc, mapping) 63 | 64 | def merge_path(path1, path2): 65 | """ Merges two path objects together. 66 | 67 | Assumes the paths are linked together by the same state. 68 | """ 69 | assert path1.end == path2.start, (path1.end, path2.start, path1, path2) 70 | links = path1.links[:-1] + path2.links 71 | latlngs = None 72 | if path1.latlngs and path2.latlngs: 73 | latlngs = path1.latlngs[:-1] + path2.latlngs 74 | return Path(path1.start, links, path2.end, latlngs) 75 | 76 | def decimate_path_simple(start_mapping, trans1, paths, trans2, end_mapping): 77 | """ 78 | Returns (trans1, paths, trans2, paths_mapping) in new mapping 79 | Args: 80 | - start_mapping: dictionary(old index -> new index), mapping of the start point 81 | - end_mapping: dictionary(old index -> new index), mapping for the end point 82 | - trans1: list of (old point index, old path index), correspondance index 83 | - paths: list of paths 84 | - trans2: list of (old path index, old point index), correspondance index 85 | """ 86 | trans1_by_paths = dict([(path_idx, pt_idx) for (pt_idx, path_idx) in trans1]) 87 | trans2_by_paths = dict(trans2) 88 | new_trans1 = [] 89 | new_trans2 = [] 90 | new_paths = [] 91 | paths_mapping = {} 92 | for path_idx in range(len(paths)): 93 | if trans1_by_paths[path_idx] in start_mapping \ 94 | and trans2_by_paths[path_idx] in end_mapping: 95 | p = paths[path_idx] 96 | new_path_idx = len(new_paths) 97 | new_paths.append(p) 98 | paths_mapping[path_idx] = new_path_idx 99 | new_start_pt_idx = start_mapping[trans1_by_paths[path_idx]] 100 | new_trans1.append((new_start_pt_idx, new_path_idx)) 101 | new_trans2.append((new_path_idx, end_mapping[trans2_by_paths[path_idx]])) 102 | return (new_trans1, new_paths, new_trans2, paths_mapping) 103 | 104 | def merge_path_sequence(trans1_a, paths_a, trans2_a, trans1_b, paths_b, \ 105 | trans2_b, best_idx_a, best_idx_b): 106 | """ Merges two paths sequences and preserves the transition information as 107 | well as some specific best index about the sequence. 108 | 109 | Args: 110 | - trans1_a: transitions from point a to path a->b 111 | - paths_a: collection of paths a->b 112 | - trans2_a: transitions from paths a->b to point b 113 | - trans1_b: transitions from point b to paths b->c 114 | - paths_b: collection of paths b->c 115 | - trans2_b: transition from paths b->c to point c 116 | - best_idx_a, best_idx_b: best indexes. 117 | Returns (trans1, paths, trans2, best_idx) 118 | """ 119 | # Aggregate the second paths by their start point: 120 | trans1_b_by_points = defaultdict(list) 121 | for (pt_b_idx, pa_b_idx) in trans1_b: 122 | trans1_b_by_points[pt_b_idx].append(pa_b_idx) 123 | trans2_a_by_paths = dict(trans2_a) 124 | trans2_b_by_paths = dict(trans2_b) 125 | new_trans1 = [] 126 | new_trans2 = [] 127 | new_paths = [] 128 | best_idx = None 129 | for (pt_a_idx, pa_a_idx) in trans1_a: 130 | path_a = paths_a[pa_a_idx] 131 | pt_b_idx = trans2_a_by_paths[pa_a_idx] 132 | if pt_b_idx in trans1_b_by_points: 133 | for pa_b_idx in trans1_b_by_points[pt_b_idx]: 134 | path_b = paths_b[pa_b_idx] 135 | pt_end_idx = trans2_b_by_paths[pa_b_idx] 136 | new_path = merge_path(path_a, path_b) 137 | new_path_idx = len(new_paths) 138 | new_trans1.append((pt_a_idx, new_path_idx)) 139 | new_trans2.append((new_path_idx, pt_end_idx)) 140 | new_paths.append(new_path) 141 | if pa_a_idx == best_idx_a and pa_b_idx == best_idx_b: 142 | best_idx = new_path_idx 143 | assert best_idx is not None 144 | return (new_trans1, new_paths, new_trans2, best_idx) 145 | -------------------------------------------------------------------------------- /mm/path_inference/mapping/decimation_test.py: -------------------------------------------------------------------------------- 1 | ''' 2 | Copyright 2011, 2012 Timothy Hunter 3 | 4 | This library is free software; you can redistribute it and/or 5 | modify it under the terms of the GNU Lesser General Public 6 | License as published by the Free Software Foundation; either 7 | version 2.1 of the License, or (at your option) version 3. 8 | 9 | This library is distributed in the hope that it will be useful, 10 | but WITHOUT ANY WARRANTY; without even the implied warranty of 11 | MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU 12 | Lesser General Public License for more details. 13 | 14 | You should have received a copy of the GNU Lesser General Public 15 | License along with this library. If not, see . 16 | ''' 17 | # pylint: disable=W0105 18 | ''' 19 | Created on Nov 10, 2011 20 | 21 | @author: tjhunter 22 | ''' 23 | 24 | from mm.path_inference.structures import Path, State 25 | from decimation import merge_path, decimate_path_simple, merge_path_sequence 26 | 27 | def test_merge_path_simple(): 28 | """ test_merge_path_simple 29 | """ 30 | s1 = State(1, 1.0, None) 31 | s2 = State(2, 3.0, None) 32 | s3 = State(3, 1.0, None) 33 | p1 = Path(s1, [1, 2], s2) 34 | p2 = Path(s2, [2, 3], s3) 35 | p = merge_path(p1, p2) 36 | assert p.start == s1 37 | assert p.end == s3 38 | assert p.links == [1, 2, 3] 39 | 40 | def test_simple_decimation_1(): 41 | """ test_simple_decimation_1 42 | """ 43 | s1_0 = State(1, 0.0, None) 44 | s1_1 = State(2, 0.0, None) 45 | s1_2 = State(3, 0.0, None) 46 | s2_0 = State(1, 1.0, None) 47 | s2_1 = State(2, 2.0, None) 48 | s2_2 = State(3, 3.0, None) 49 | paths = [Path(s1_0, [1], s2_0), Path(s1_1, [2], s2_1), Path(s1_2, [3], s2_2)] 50 | trans1 = [(0, 0), (1, 1), (2, 2)] 51 | trans2 = [(0, 0), (1, 1), (2, 2)] 52 | start_mapping = {1:0} 53 | end_mapping = {1:0} 54 | (new_trans1, new_paths, new_trans2, paths_mapping) = \ 55 | decimate_path_simple(start_mapping, trans1, paths, trans2, end_mapping) 56 | assert paths_mapping == {1:0}, paths_mapping 57 | assert new_trans1 == [(0, 0)], new_trans1 58 | assert new_trans2 == [(0, 0)], new_trans2 59 | assert len(new_paths) == 1 60 | assert new_paths[0].links == [2] 61 | 62 | def test_simple_decimation_2(): 63 | """ test_simple_decimation_2 64 | """ 65 | s1_0 = State(0, 0.0) 66 | #s1_1 = State(1, 0.0) 67 | s1_2 = State(2, 0.0) 68 | s2_0 = State(0, 1.0) 69 | s2_1 = State(1, 1.0) 70 | s2_2 = State(2, 1.0) 71 | paths = [Path(s1_0, [0, 1], s2_1), \ 72 | Path(s1_2, [2, 1], s2_1), \ 73 | Path(s1_0, [0, 0], s2_0), \ 74 | Path(s1_2, [2, 2], s2_2)] 75 | print paths 76 | trans1 = [(0, 0), (2, 1), (0, 2), (2, 3)] 77 | trans2 = [(0, 1), (1, 1), (2, 0), (3, 2)] 78 | start_mapping = {2:0} 79 | end_mapping = {1:0, 2:1} 80 | (new_trans1, new_paths, new_trans2, paths_mapping) = \ 81 | decimate_path_simple(start_mapping, trans1, paths, trans2, end_mapping) 82 | assert paths_mapping == {1:0, 3:1}, paths_mapping 83 | assert len(new_paths) == 2 84 | assert new_paths[0].links == [2, 1] 85 | assert new_paths[1].links == [2, 2] 86 | assert new_trans1 == [(0, 0), (0, 1)], new_trans1 87 | assert new_trans2 == [(0, 0), (1, 1)], new_trans2 88 | 89 | def test_merge_path_sequence_1(): 90 | """ test_merge_path_sequence_1 91 | """ 92 | sa_0 = State(0, 0.0, None) 93 | sa_1 = State(1, 0.0, None) 94 | sa_2 = State(2, 0.0, None) 95 | sb_0 = State(0, 1.0, None) 96 | sb_1 = State(1, 2.0, None) 97 | sb_2 = State(2, 3.0, None) 98 | sc_0 = State(0, 0.0, None) 99 | sc_1 = State(1, 0.0, None) 100 | sc_2 = State(2, 0.0, None) 101 | paths_a = [Path(sa_0, [0], sb_0), \ 102 | Path(sa_1, [1], sb_1), \ 103 | Path(sa_2, [2], sb_2)] 104 | paths_b = [Path(sb_0, [0], sc_0), \ 105 | Path(sb_1, [1], sc_1), \ 106 | Path(sb_2, [2], sc_2)] 107 | trans1_a = [(0, 0), (1, 1), (2, 2)] 108 | trans1_b = [(0, 0), (1, 1), (2, 2)] 109 | trans2_a = [(0, 0), (1, 1), (2, 2)] 110 | trans2_b = [(0, 0), (1, 1), (2, 2)] 111 | best_idx_a = 1 112 | best_idx_b = 1 113 | (new_trans1, new_paths, new_trans2, new_best_idx) = \ 114 | merge_path_sequence(trans1_a, paths_a, trans2_a, trans1_b, paths_b, \ 115 | trans2_b, best_idx_a, best_idx_b) 116 | assert len(new_paths) == 3 117 | assert new_trans1 == [(0, 0), (1, 1), (2, 2)] 118 | assert new_trans2 == [(0, 0), (1, 1), (2, 2)] 119 | assert new_best_idx == 1 120 | 121 | def test_merge_path_sequence_2(): 122 | """ test_merge_path_sequence_2 123 | """ 124 | sa_0 = State(0, 0.0) 125 | sa_1 = State(1, 0.0) 126 | sb_0 = State(0, 1.0) 127 | sb_1 = State(1, 1.0) 128 | sc_0 = State(0, 2.0) 129 | sc_1 = State(1, 2.0) 130 | paths_a = [Path(sa_0, [0], sb_0), \ 131 | Path(sa_0, [0, 1], sb_1), \ 132 | Path(sa_1, [1, 0], sb_0), \ 133 | Path(sa_1, [1], sb_1)] 134 | paths_b = [Path(sb_0, [0], sc_0), \ 135 | Path(sb_0, [0, 1], sc_1), \ 136 | Path(sb_1, [1, 0], sc_0), \ 137 | Path(sb_1, [1], sc_1)] 138 | trans1_a = [(0, 0), (0, 1), (1, 2), (1, 3)] 139 | trans2_a = [(0, 0), (1, 1), (2, 0), (3, 1)] 140 | trans1_b = [(0, 0), (0, 1), (1, 2), (1, 3)] 141 | trans2_b = [(0, 0), (1, 1), (2, 0), (3, 1)] 142 | best_idx_a = 0 143 | best_idx_b = 1 144 | (new_trans1, new_paths, new_trans2, new_best_idx) = \ 145 | merge_path_sequence(trans1_a, paths_a, trans2_a, trans1_b, paths_b, \ 146 | trans2_b, best_idx_a, best_idx_b) 147 | assert len(new_paths) == 8, len(new_paths) 148 | assert new_trans1 == [(0, 0), (0, 1), (0, 2), (0, 3), (1, 4), (1, 5), (1, 6), (1, 7)], new_trans1 149 | assert new_trans2 == [(0, 0), (1, 1), (2, 0), (3, 1), (4, 0), (5, 1), (6, 0), (7, 1)], new_trans2 150 | assert new_best_idx == 1, new_best_idx 151 | -------------------------------------------------------------------------------- /mm/path_inference/mapping/utils.py: -------------------------------------------------------------------------------- 1 | #Copyright 2011, 2012 Timothy Hunter 2 | # 3 | #This library is free software; you can redistribute it and/or 4 | #modify it under the terms of the GNU Lesser General Public 5 | #License as published by the Free Software Foundation; either 6 | #version 2.1 of the License, or (at your option) version 3. 7 | # 8 | #This library is distributed in the hope that it will be useful, 9 | #but WITHOUT ANY WARRANTY; without even the implied warranty of 10 | #MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU 11 | #Lesser General Public License for more details. 12 | # 13 | #You should have received a copy of the GNU Lesser General Public 14 | #License along with this library. If not, see . 15 | ''' 16 | Created on Nov 8, 2011 17 | 18 | @author: tjhunter 19 | ''' 20 | from mm.path_inference.utils import distance as mm_dis # Name clash with pylab 21 | 22 | def cut_points_by_distance(scs, n=10, max_distance=45, \ 23 | min_distance=1.0, min_length=10): 24 | """ Cuts points into tracks according to some distance criterion. 25 | Arguments: 26 | - scs: a list of StateCollection items corresponding to a run 27 | - n: index shift, > 0. The algorithm will compare the distance between 28 | sc[i] and sc[i+n]. If this distance is found to be greater than 29 | n * max_distance, the trjactory is cut at i. 30 | - max_distance: the maximum distance between two points. Two points 31 | separated by more than this distance are cut. 32 | - min_distance: minimum distance between two point. Below this distance, 33 | the new point is discard for beeing too close. 34 | - min_length: the minimum length of a track. 35 | n: 36 | """ 37 | # Meters, corresponds to 1-second probe data 38 | N = len(scs) 39 | # Compute the n-cut distance first to get an idea of long-terms cuts 40 | dsn = [mm_dis(scs[i].gps_pos, scs[i+n].gps_pos) for i in xrange(N-n)] 41 | # Distance cuts: 42 | d_cuts = [i for i in xrange(N-n) if dsn[i] > n*max_distance] 43 | points = [0] + d_cuts + [N] 44 | res = [] 45 | for (start, end) in zip(points[:-1], points[1:]): 46 | all_points = scs[start:end] 47 | all_points.reverse() 48 | l = [all_points[-1]] 49 | while all_points: 50 | x = all_points.pop() 51 | if mm_dis(x.gps_pos, l[-1].gps_pos) > min_distance: 52 | l.append(x) 53 | if len(l) >= min_length: 54 | res.append(l) 55 | return res 56 | 57 | def remove_spurious_points(scs, n=3, max_distance=45, \ 58 | min_distance=1.0, min_length=10): 59 | """ Removes a number of points that are of no interest or may cause trouble: 60 | - points that correspond to no move (stationary vehicle) 61 | - far points that correspond to GPS errors that make the trajectory jump 62 | Arguments: 63 | scs: a list of StateCollection items corresponding to a run 64 | n: the maximum number of points that may group as a GPS error 65 | max_distance: that maximum distance between points corresponding to a valid 66 | travel. Over this distance, the point is assumed to be disconnected. 67 | min_distance: under this distance, the vehicle is assumed to be stationary 68 | min_length: minimum size of groups of points to correspond to a valid 69 | trajectory chunk. 70 | 71 | Returns: a list of list of valid tracks. 72 | """ 73 | res = [] 74 | # This is a queue that will contain the points 75 | # Each point will be popped out and compared to the head of the 76 | # current track. 77 | all_scs = list(scs) 78 | all_scs.reverse() # Need to reverse, since we will pop() from the start. 79 | current_track = [] 80 | current_junk = [] 81 | while all_scs: 82 | current_sc = all_scs.pop() 83 | if not current_track: 84 | current_track = [current_sc] 85 | else: 86 | last_sc = current_track[-1] 87 | assert last_sc.time < current_sc.time 88 | # Compare the distance 89 | d = mm_dis(last_sc.gps_pos, current_sc.gps_pos) 90 | # If too far, add to the junk 91 | if d < min_distance: 92 | continue 93 | if d > max_distance: 94 | current_junk.append(current_sc) 95 | # If junk if full, discard it, cut the track and start again 96 | if len(current_junk) > n: 97 | current_junk = [] 98 | # If the cut track is long enough, keep it 99 | if len(current_track) >= min_length: 100 | res.append(current_track) 101 | current_track = [] 102 | else: 103 | current_track.append(current_sc) 104 | # And take care of the last track 105 | if len(current_track) >= min_length: 106 | res.append(current_track) 107 | return res 108 | 109 | 110 | def get_track_units(flow_units, path_counts, state_counts, min_path_count=10, \ 111 | min_state_count=1, min_traj_unit_length=5, \ 112 | max_traj_unit_length=3000): 113 | """ Computes the split of the track into trajectories that are 114 | self-coherent (every point is reachable and has a decent number 115 | of states). 116 | flow_units: a list of list of indexes in the track, each list of index is 117 | self-coherent 118 | path_counts: the number of paths between a pair of points. 119 | state_counts: list of integers, counts the number of projections for each 120 | gps point 121 | 122 | The current implementation is a bit loose and does not try to get as many 123 | points as possible. It does not really matter to loose a few points here 124 | and there. 125 | """ 126 | track_length = min(len(state_counts), len(path_counts)) 127 | # The states that are marked as cut states. 128 | black_list = set([i for i in range(track_length) \ 129 | if (state_counts[i] < min_state_count)]) 130 | for i in range(1, track_length): 131 | if path_counts[i] < min_path_count: 132 | black_list.add(i-1) 133 | black_list.add(i) 134 | for flow_unit in flow_units: 135 | if flow_unit[-1] >= track_length: 136 | if flow_unit[0] >= track_length: 137 | break 138 | flow_unit = [i for i in flow_unit if i < track_length] 139 | if not flow_unit: 140 | continue 141 | black_list.add(flow_unit[0]) 142 | black_list.add(flow_unit[-1]) 143 | black_list.add(0) 144 | black_list.add(track_length-1) 145 | # Now we have all the bad points, find all the good ones. 146 | bad_points = list(black_list) 147 | bad_points.sort() 148 | cut_pairs = zip(bad_points[:-1], bad_points[1:]) 149 | res = [] 150 | def chunks(l, n): 151 | for idx in xrange(0, len(l), n): 152 | yield l[idx:idx+n] 153 | 154 | for (x, y) in cut_pairs: 155 | if y-x >= min_traj_unit_length: 156 | for chunk in chunks(range(x, y), max_traj_unit_length): 157 | if len(chunk) >= min_traj_unit_length: 158 | res.append(chunk) 159 | return res 160 | 161 | -------------------------------------------------------------------------------- /mm/path_inference/path_builder.py: -------------------------------------------------------------------------------- 1 | #Copyright 2011, 2012 Timothy Hunter 2 | # 3 | #This library is free software; you can redistribute it and/or 4 | #modify it under the terms of the GNU Lesser General Public 5 | #License as published by the Free Software Foundation; either 6 | #version 2.1 of the License, or (at your option) version 3. 7 | # 8 | #This library is distributed in the hope that it will be useful, 9 | #but WITHOUT ANY WARRANTY; without even the implied warranty of 10 | #MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU 11 | #Lesser General Public License for more details. 12 | # 13 | #You should have received a copy of the GNU Lesser General Public 14 | #License along with this library. If not, see . 15 | ''' 16 | Created on Sep 20, 2011 17 | 18 | @author: tjhunter 19 | ''' 20 | 21 | class PathBuilder(object): 22 | """ Creates candidate paths between states. 23 | 24 | INTERFACE ONLY. 25 | """ 26 | 27 | def getPaths(self, s1, s2): 28 | """ Returns a set of candidate paths between state s1 and state s3. 29 | Arguments: 30 | - s1 : a State object 31 | - s2 : a State object 32 | """ 33 | raise NotImplementedError() 34 | 35 | def getPathsBetweenCollections(self, sc1, sc2): 36 | trans1 = [] 37 | trans2 = [] 38 | paths = [] 39 | n1 = len(sc1.states) 40 | n2 = len(sc2.states) 41 | num_paths = 0 42 | for i1 in range(n1): 43 | for i2 in range(n2): 44 | ps = self.getPaths(sc1.states[i1], sc2.states[i2]) 45 | for path in ps: 46 | trans1.append((i1, num_paths)) 47 | trans2.append((num_paths, i2)) 48 | paths.append(path) 49 | num_paths += 1 50 | return (trans1, paths, trans2) 51 | -------------------------------------------------------------------------------- /mm/path_inference/projector.py: -------------------------------------------------------------------------------- 1 | #Copyright 2011, 2012 Timothy Hunter 2 | # 3 | #This library is free software; you can redistribute it and/or 4 | #modify it under the terms of the GNU Lesser General Public 5 | #License as published by the Free Software Foundation; either 6 | #version 2.1 of the License, or (at your option) version 3. 7 | # 8 | #This library is distributed in the hope that it will be useful, 9 | #but WITHOUT ANY WARRANTY; without even the implied warranty of 10 | #MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU 11 | #Lesser General Public License for more details. 12 | # 13 | #You should have received a copy of the GNU Lesser General Public 14 | #License along with this library. If not, see . 15 | ''' 16 | Created on Sep 20, 2011 17 | 18 | @author: tjhunter 19 | ''' 20 | 21 | class PointProjector(object): 22 | """ Interface that defines how a point is projected on the road network. 23 | """ 24 | 25 | def project(self, gps_pos): 26 | """ (abstract) : takes a GPS position and returns a collection of states. 27 | """ 28 | raise NotImplementedError() 29 | -------------------------------------------------------------------------------- /mm/path_inference/structures.py: -------------------------------------------------------------------------------- 1 | #Copyright 2011, 2012 Timothy Hunter 2 | # 3 | #This library is free software; you can redistribute it and/or 4 | #modify it under the terms of the GNU Lesser General Public 5 | #License as published by the Free Software Foundation; either 6 | #version 2.1 of the License, or (at your option) version 3. 7 | # 8 | #This library is distributed in the hope that it will be useful, 9 | #but WITHOUT ANY WARRANTY; without even the implied warranty of 10 | #MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU 11 | #Lesser General Public License for more details. 12 | # 13 | #You should have received a copy of the GNU Lesser General Public 14 | #License along with this library. If not, see . 15 | ''' 16 | Created on Sep 16, 2011 17 | 18 | @author: tjhunter 19 | ''' 20 | 21 | class LatLng(object): 22 | """ A geolocation representation. 23 | """ 24 | def __init__(self, lat, lng): 25 | self.lat = lat 26 | self.lng = lng 27 | 28 | def __eq__(self, other): 29 | return self.lat == other.lat and self.lng == other.lng 30 | 31 | def __ne__(self, other): 32 | return not self.__eq__(other) 33 | 34 | 35 | class State(object): 36 | """ A state on the road network. 37 | """ 38 | def __init__(self, link_id, offset, pos=None): 39 | """ 40 | - link_id: a string for identifying the link. 41 | - offset: an offset on the link 42 | - pos: a LatLnt position 43 | """ 44 | self.link_id = link_id 45 | self.offset = offset 46 | self.gps_pos = pos 47 | 48 | def __eq__(self, other): 49 | return self.link_id == other.link_id \ 50 | and self.offset == other.offset \ 51 | and self.gps_pos == other.gps_pos 52 | 53 | def __ne__(self, other): 54 | return not self.__eq__(other) 55 | 56 | def __repr__(self): 57 | return "State[%s, %s]" % (str(self.link_id), str(self.offset)) 58 | 59 | class StateCollection(object): 60 | """ 61 | id: a string identifier of the driver 62 | spots: a list of State objects. 63 | gps_pos: the LatLng gps observation. 64 | time: a DateTime object 65 | """ 66 | def __init__(self, v_id, states, gps_pos, time): 67 | self.id = v_id 68 | self.states = states 69 | self.gps_pos = gps_pos 70 | self.time = time 71 | 72 | def __eq__(self, other): 73 | return (isinstance(other, self.__class__) 74 | and self.__dict__ == other.__dict__) 75 | 76 | def __ne__(self, other): 77 | return not self.__eq__(other) 78 | 79 | 80 | class Path(object): 81 | """ A path object: represnts a path on the network. 82 | 83 | start_state: the start state 84 | end_state: the end state 85 | links: a sequence of link ids 86 | states: a list of states (optional, for drawing only) 87 | """ 88 | def __init__(self, start_state, links, end_state, latlngs=None): 89 | self.start = start_state 90 | self.end = end_state 91 | self.links = links 92 | self.latlngs = latlngs 93 | 94 | def __eq__(self, other): 95 | return self.start == other.start \ 96 | and self.end == other.end \ 97 | and self.links == other.links 98 | 99 | def __ne__(self, other): 100 | return not self.__eq__(other) 101 | 102 | def __repr__(self): 103 | return "Path[%s,links=%s,%s]" % (str(self.start), str(self.links), str(self.end)) 104 | 105 | class PathCollection(object): 106 | """ A collection of paths, and some additional informations. 107 | """ 108 | def __init__(self, v_id, paths, start_time, end_time): 109 | self.id = v_id 110 | self.paths = paths 111 | self.start_time = start_time 112 | self.end_time = end_time 113 | -------------------------------------------------------------------------------- /mm/path_inference/utils.py: -------------------------------------------------------------------------------- 1 | #Copyright 2011, 2012 Timothy Hunter 2 | # 3 | #This library is free software; you can redistribute it and/or 4 | #modify it under the terms of the GNU Lesser General Public 5 | #License as published by the Free Software Foundation; either 6 | #version 2.1 of the License, or (at your option) version 3. 7 | # 8 | #This library is distributed in the hope that it will be useful, 9 | #but WITHOUT ANY WARRANTY; without even the implied warranty of 10 | #MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU 11 | #Lesser General Public License for more details. 12 | # 13 | #You should have received a copy of the GNU Lesser General Public 14 | #License along with this library. If not, see . 15 | ''' Utility functions for the path inference. 16 | Created on Sep 23, 2011 17 | 18 | @author: tjhunter 19 | 20 | ''' 21 | import math 22 | 23 | def haversine(lon1, lat1, lon2, lat2): 24 | """ 25 | Calculate the great circle distance between two points 26 | on the earth (specified in decimal degrees) 27 | """ 28 | # convert decimal degrees to radians 29 | lon1, lat1, lon2, lat2 = \ 30 | [math.radians(deg) for deg in [lon1, lat1, lon2, lat2]] 31 | # haversine formula 32 | dlon = lon2 - lon1 33 | dlat = lat2 - lat1 34 | a = math.sin(dlat/2)**2 + math.cos(lat1) * \ 35 | math.cos(lat2) * math.sin(dlon/2)**2 36 | c = 2 * math.atan2(math.sqrt(a), math.sqrt(1-a)) 37 | meters = 6367 * c * 1000 38 | return meters 39 | 40 | def distance(gps1, gps2): 41 | """ Computes the distance (in meters) between two LatLng objects. 42 | """ 43 | return haversine(gps1.lng, gps1.lat, gps2.lng, gps2.lat) 44 | 45 | 46 | --------------------------------------------------------------------------------