def apply_dynamic(self, input, gurobi_model: grb.Model, newthdot, newtheta,
env_input_size, action):
'''
:param costheta: gurobi variable containing the range of costheta values
:param sintheta: gurobi variable containin the range of sintheta values
:param input:
:param gurobi_model:
:param t:
:return:
'''
tau = self.tau # 0.001 # seconds between state updates
# x = input[0]
# x_dot = input[1]
# theta = input[2]
# theta_dot = input[3]
# battery = input[4]
z = gurobi_model.addMVar(shape=(env_input_size, ),
lb=float("-inf"),
name=f"x_prime")
# x_prime = x + tau * x_dot
# x_dot_prime = x_dot + tau * xacc
# theta_prime = theta + tau * theta_dot
# theta_dot_prime = theta_dot + tau * thetaacc
# action_cost = 0
# if action != 0:
# action_cost = 0.5
gurobi_model.addConstr(z[0] == newtheta, name=f"dyna_constr_1")
gurobi_model.addConstr(z[1] == newthdot, name=f"dyna_constr_2")
return z
def build_supply_constraints(
data: InputData, model: grb.Model,
facility_customer_pair_to_column: Dict[Tuple[str, str], grb.Var],
facility_name_to_column: Dict[str, grb.Var]) -> Dict[str, grb.Constr]:
"""
Build constraints s_i y_i - \sum_{j=0}^{m} x_ij >= 0, for all i = 1, ..., n
:param data:
:param model:
:param facility_customer_pair_to_column:
:param facility_name_to_column:
:return:
"""
facility_to_row = dict()
for facility in data.facilities:
lhs = [
-facility_customer_pair_to_column[(facility.name, customer_name)]
for customer_name in facility.transport_cost.keys()
]
facility_var = facility_name_to_column.get(facility.name, 1)
lhs.append(1 * facility.supply * facility_var)
lhs = grb.quicksum(lhs)
name = f'supply_{facility.name}'
row = model.addConstr(lhs >= 0.0, name=name)
facility_to_row[facility.name] = row
return facility_to_row
def apply_dynamic(self, input, gurobi_model: grb.Model, thetaacc, xacc,
env_input_size):
'''
:param costheta: gurobi variable containing the range of costheta values
:param sintheta: gurobi variable containin the range of sintheta values
:param input:
:param gurobi_model:
:param t:
:return:
'''
tau = self.tau # 0.001 # seconds between state updates
x = input[0]
x_dot = input[1]
theta = input[2]
theta_dot = input[3]
z = gurobi_model.addMVar(shape=(env_input_size, ),
lb=float("-inf"),
name=f"x_prime")
x_prime = x + tau * x_dot
x_dot_prime = x_dot + tau * xacc
theta_prime = theta + tau * theta_dot
theta_dot_prime = theta_dot + tau * thetaacc
gurobi_model.addConstr(z[0] == x_prime, name=f"dyna_constr_1")
gurobi_model.addConstr(z[1] == x_dot_prime, name=f"dyna_constr_2")
gurobi_model.addConstr(z[2] == theta_prime, name=f"dyna_constr_3")
gurobi_model.addConstr(z[3] == theta_dot_prime, name=f"dyna_constr_4")
return z
def apply_dynamic(input, gurobi_model: grb.Model, action, env_input_size):
'''
:param input:
:param gurobi_model:
:param action_ego:
:param t:
:return:
lead 100km/h 28m/s
ego 130km/h 36.1 m/s
'''
x_lead = input[0]
x_ego = input[1]
v_ego = input[2]
# gurobi_model.addConstr(action[0] <= 12) # cap the acceleration to +12 m/s*2
# gurobi_model.addConstr(action[0] >= -12) # cap the acceleration to -12 m/s*2
z = gurobi_model.addMVar(shape=(3,), lb=float("-inf"), name=f"x_prime")
dt = .1 # seconds
acceleration = action[0]
a_ego_prime = acceleration
v_ego_prime = v_ego + acceleration * dt
gurobi_model.addConstr(v_ego_prime <= max_speed, name=f"v_constr")
# gurobi_model.addConstr(v_ego_prime >= -max_speed, name=f"v_constr")
# gurobi_model.addConstr(a_lead == 0, name="a_lead_constr")
v_lead_prime = v_lead
x_lead_prime = x_lead + v_lead_prime * dt
x_ego_prime = x_ego + v_ego_prime * dt
gurobi_model.addConstr(z[0] == x_lead_prime, name=f"dyna_constr_1")
gurobi_model.addConstr(z[1] == x_ego_prime, name=f"dyna_constr_2")
gurobi_model.addConstr(z[2] == v_ego_prime, name=f"dyna_constr_3")
return z
def init_model(self, **args):
warm_start = args.get('warm_start', {})
self.model = Model("Knapsack")
self.variable_decision = {"x": {}}
self.description_variable_description = {
"x": {
"shape":
self.knapsack_model.nb_items,
"type":
bool,
"descr":
"dictionary with key the item index \
and value the boolean value corresponding \
to taking the item or not"
}
}
self.description_constraint["weight"] = {
"descr": "sum of weight of used items doesn't exceed max capacity"
}
weight = {}
list_item = self.knapsack_model.list_items
max_capacity = self.knapsack_model.max_capacity
x = {}
for item in list_item:
i = item.index
x[i] = self.model.addVar(vtype=GRB.BINARY,
obj=item.value,
name="x_" + str(i))
if i in warm_start:
x[i].start = warm_start[i]
x[i].varhinstval = warm_start[i]
weight[i] = item.weight
self.variable_decision["x"] = x
self.model.update()
self.constraints_dict["weight"] = self.model.addConstr(
quicksum([weight[i] * x[i] for i in x]) <= max_capacity)
self.model.update()
self.model.setParam("TimeLimit", 200)
self.model.modelSense = GRB.MAXIMIZE
self.model.setParam(GRB.Param.PoolSolutions, 10000)
self.model.setParam("MIPGapAbs", 0.00001)
self.model.setParam("MIPGap", 0.00000001)
def apply_dynamic(input, gurobi_model: grb.Model, action, env_input_size):
'''
in this case ACTION is a variable
:param input:
:param gurobi_model:
:param action_ego:
:param t:
:return:
lead 100km/h 28m/s
ego 130km/h 36.1 m/s
'''
v_lead = 28
max_speed = 36
x_lead = input[0]
x_ego = input[1]
v_ego = input[2]
z = gurobi_model.addMVar(shape=(3,), lb=float("-inf"), name=f"x_prime")
const_acc = 3
dt = .1 # seconds
acceleration = gurobi_model.addMVar(shape=(1,), lb=float("-inf"))
gurobi_model.addConstr(acceleration[0] == (2 * action[0] - 1) * const_acc, name=f"action_constr")
v_ego_prime = v_ego + acceleration * dt
gurobi_model.addConstr(v_ego_prime <= max_speed, name=f"v_constr")
# gurobi_model.addConstr(v_ego_prime >= -max_speed, name=f"v_constr")
# gurobi_model.addConstr(a_lead == 0, name="a_lead_constr")
v_lead_prime = v_lead
x_lead_prime = x_lead + v_lead_prime * dt
x_ego_prime = x_ego + v_ego_prime * dt
gurobi_model.addConstr(z[0] == x_lead_prime, name=f"dyna_constr_1")
gurobi_model.addConstr(z[1] == x_ego_prime, name=f"dyna_constr_2")
gurobi_model.addConstr(z[2] == v_ego_prime, name=f"dyna_constr_3")
return z
def apply_dynamic(input, gurobi_model: grb.Model, action, env_input_size):
'''
:param input:
:param gurobi_model:
:param action_ego:
:param t:
:return:
lead 100km/h 28m/s
ego 130km/h 36.1 m/s
'''
v_lead = 28
max_speed = 36
x_lead = input[0]
x_ego = input[1]
# v_lead = input[2]
v_ego = input[2]
# a_lead = input[4]
# a_ego = input[5]
z = gurobi_model.addMVar(shape=(3, ),
lb=float("-inf"),
name=f"x_prime")
const_acc = 3
dt = .1 # seconds
if action == 0:
acceleration = -const_acc
elif action == 1:
acceleration = const_acc
else:
acceleration = 0
v_ego_prime = v_ego + acceleration * dt
gurobi_model.addConstr(v_ego_prime <= max_speed, name=f"v_constr")
# gurobi_model.addConstr(v_ego_prime >= -max_speed, name=f"v_constr")
# gurobi_model.addConstr(a_lead == 0, name="a_lead_constr")
v_lead_prime = v_lead
x_lead_prime = x_lead + v_lead_prime * dt
x_ego_prime = x_ego + v_ego_prime * dt
# delta_x_prime = (x_lead + (v_lead + (a_lead + 0) * dt) * dt) - (x_ego + (v_ego + (a_ego + acceleration) * dt) * dt)
# delta_v_prime = (v_lead + (a_lead + 0) * dt) - (v_ego + (a_ego + acceleration) * dt)
gurobi_model.addConstr(z[0] == x_lead_prime, name=f"dyna_constr_1")
gurobi_model.addConstr(z[1] == x_ego_prime, name=f"dyna_constr_2")
gurobi_model.addConstr(z[2] == v_ego_prime, name=f"dyna_constr_3")
# gurobi_model.addConstr(z[3] == v_ego_prime, name=f"dyna_constr_4")
# gurobi_model.addConstr(z[4] == 0, name=f"dyna_constr_5") # no change in a_lead
# gurobi_model.addConstr(z[5] == acceleration, name=f"dyna_constr_6")
return z
def build_demand_constraints(data: InputData,
model: grb.Model,
facility_customer_pair_to_column: Dict[Tuple[str, str], grb.Var]) \
-> Dict[str, grb.Constr]:
customer_to_row = dict()
for customer in data.customers:
lhs = grb.quicksum([
facility_customer_pair_to_column[(facility.name, customer.name)]
for facility in data.facilities
])
rhs = customer.demand
name = f'demand_{customer.name}'
row = model.addConstr(lhs >= rhs, name=name)
customer_to_row[customer.name] = row
return customer_to_row
def build_facility_columns(data: InputData,
model: grb.Model) -> Dict[str, grb.Var]:
facility_name_to_column: Dict[str, grb.Var] = dict()
for facility in data.facilities:
name = f'facility_{facility.name}'
lb, obj = (0.0, facility.build_cost) if not facility.exists else (1.0,
0)
var = model.addVar(lb=lb,
ub=1.0,
obj=obj,
vtype=grb.GRB.BINARY,
name=name,
column=None)
facility_name_to_column[facility.name] = var
return facility_name_to_column
def build_transport_columns(
data: InputData, model: grb.Model) -> Dict[Tuple[str, str], grb.Var]:
facility_customer_to_column: Dict[Tuple[str, str], grb.Var] = dict()
lb = 0.0
ub = grb.GRB.INFINITY
for facility in data.facilities:
for customer_name, unit_transport_cost in facility.transport_cost.items(
):
name = f'x_{facility.name}_{customer_name}'
var = model.addVar(lb=lb,
ub=ub,
obj=unit_transport_cost,
vtype=grb.GRB.CONTINUOUS,
name=name,
column=None)
facility_customer_to_column[(facility.name, customer_name)] = var
return facility_customer_to_column
def optimise(self, templates: np.ndarray, gurobi_model: grb.Model,
x_prime: tuple):
results = []
for template in templates:
gurobi_model.update()
gurobi_model.setObjective(
sum((template[i] * x_prime[i])
for i in range(self.env_input_size)), grb.GRB.MAXIMIZE)
gurobi_model.optimize()
# print_model(gurobi_model)
if gurobi_model.status != 2:
return None
result = gurobi_model.ObjVal
results.append(result)
return np.array(results)
def optimise(templates: np.ndarray, gurobi_model: grb.Model, x_prime: tuple):
results = []
for template in templates:
gurobi_model.update()
gurobi_model.setObjective(sum((template[i] * x_prime[i]) for i in range(len(template))), grb.GRB.MAXIMIZE)
gurobi_model.optimize()
# print_model(gurobi_model)
if gurobi_model.status == 5:
result = float("inf")
results.append(result)
continue
if gurobi_model.status == 4 or gurobi_model.status == 3:
return None
assert gurobi_model.status == 2, f"gurobi_model.status=={gurobi_model.status}"
# if gurobi_model.status != 2:
# return None
result = gurobi_model.ObjVal
results.append(result)
return np.array(results)
def generate_nn_guard(gurobi_model: grb.Model,
input,
nn: torch.nn.Sequential,
action_ego=0,
M=1e6):
gurobi_vars = []
gurobi_vars.append(input)
for i, layer in enumerate(nn):
# print(layer)
if type(layer) is torch.nn.Linear:
v = gurobi_model.addMVar(lb=float("-inf"),
shape=(layer.out_features),
name=f"layer_{i}")
weights: np.ndarray = layer.weight.data.numpy()
weights.round(6)
lin_expr = weights @ gurobi_vars[-1]
if layer.bias is not None:
lin_expr = lin_expr + layer.bias.data.numpy()
gurobi_model.addConstr(v == lin_expr,
name=f"linear_constr_{i}")
gurobi_vars.append(v)
elif type(layer) is torch.nn.ReLU:
v = gurobi_model.addMVar(
lb=float("-inf"),
shape=gurobi_vars[-1].shape,
name=f"layer_{i}") # same shape as previous
z = gurobi_model.addMVar(lb=0,
ub=1,
shape=gurobi_vars[-1].shape,
vtype=grb.GRB.INTEGER,
name=f"relu_{i}")
eps = 0 # 1e-9
# gurobi_model.addConstr(v == grb.max_(0, gurobi_vars[-1]))
gurobi_model.addConstr(v >= gurobi_vars[-1],
name=f"relu_constr_1_{i}")
gurobi_model.addConstr(v <= eps + gurobi_vars[-1] + M * z,
name=f"relu_constr_2_{i}")
gurobi_model.addConstr(v >= 0, name=f"relu_constr_3_{i}")
gurobi_model.addConstr(v <= eps + M - M * z,
name=f"relu_constr_4_{i}")
gurobi_vars.append(v)
# gurobi_model.update()
# gurobi_model.optimize()
# assert gurobi_model.status == 2, "LP wasn't optimally solved"
"""
y = Relu(x)
0 <= z <= 1, z is integer
y >= x
y <= x + Mz
y >= 0
y <= M - Mz"""
# gurobi_model.update()
# gurobi_model.optimize()
# assert gurobi_model.status == 2, "LP wasn't optimally solved"
# gurobi_model.setObjective(v[action_ego].sum(), grb.GRB.MAXIMIZE) # maximise the output
last_layer = gurobi_vars[-1]
if action_ego == 0:
gurobi_model.addConstr(last_layer[0] >= last_layer[1],
name="last_layer")
else:
gurobi_model.addConstr(last_layer[1] >= last_layer[0],
name="last_layer")
gurobi_model.update()
gurobi_model.optimize()
# assert gurobi_model.status == 2, f"LP wasn't optimally solved, gurobi status {gurobi_model.status}"
return gurobi_model.status == 2
def generate_angle_milp(gurobi_model: grb.Model, input,
sin_cos_table: List[Tuple]):
"""MILP method
input: theta, thetadot
output: thetadotdot, xdotdot (edited)
l_{theta, i}, l_{thatdot,i}, l_{thetadotdot, i}, l_{xdotdot, i}, u_....
sum_{i=1}^k l_{x,i} - l_{x,i}*z_i <= x <= sum_{i=1}^k u_{x,i} - u_{x,i}*z_i, for each variable x
sum_{i=1}^k l_{theta,i} - l_{theta,i}*z_i <= theta <= sum_{i=1}^k u_{theta,i} - u_{theta,i}*z_i
"""
theta = input[0]
theta_dot = input[1]
k = len(sin_cos_table)
zs = []
newthdot = gurobi_model.addMVar(shape=(1, ),
lb=float("-inf"),
name="newthdot")
newtheta = gurobi_model.addMVar(shape=(1, ),
lb=float("-inf"),
name="newtheta")
for i in range(k):
z = gurobi_model.addMVar(lb=0,
ub=1,
shape=(1, ),
vtype=grb.GRB.INTEGER,
name=f"part_{i}")
zs.append(z)
gurobi_model.addConstr(k - 1 == sum(zs), name=f"const_milp1")
theta_lb = 0
theta_ub = 0
theta_dot_lb = 0
theta_dot_ub = 0
newtheta_lb = 0
newtheta_ub = 0
newthdot_lb = 0
newthdot_ub = 0
for i in range(k):
theta_interval, theta_dot_interval, newth_interval, newthdot_interval = sin_cos_table[
i]
theta_lb += theta_interval[0].inf - theta_interval[0].inf * zs[i]
theta_ub += theta_interval[0].sup - theta_interval[0].sup * zs[i]
theta_dot_lb += theta_dot_interval[
0].inf - theta_dot_interval[0].inf * zs[i]
theta_dot_ub += theta_dot_interval[
0].sup - theta_dot_interval[0].sup * zs[i]
newthdot_lb += newthdot_interval[
0].inf - newthdot_interval[0].inf * zs[i]
newthdot_ub += newthdot_interval[
0].sup - newthdot_interval[0].sup * zs[i]
newtheta_lb += newth_interval[0].inf - newth_interval[0].inf * zs[i]
newtheta_ub += newth_interval[0].sup - newth_interval[0].sup * zs[i]
gurobi_model.update()
gurobi_model.optimize()
assert gurobi_model.status == 2, "LP wasn't optimally solved"
gurobi_model.addConstr(theta >= theta_lb, name=f"theta_guard1")
gurobi_model.addConstr(theta <= theta_ub, name=f"theta_guard2")
gurobi_model.addConstr(theta_dot >= theta_dot_lb,
name=f"theta_dot_guard1")
gurobi_model.addConstr(theta_dot <= theta_dot_ub,
name=f"theta_dot_guard2")
gurobi_model.addConstr(newthdot >= newthdot_lb,
name=f"newthdot_guard1")
gurobi_model.addConstr(newthdot <= newthdot_ub,
name=f"newthdot_guard2")
gurobi_model.addConstr(newtheta >= newtheta_lb,
name=f"newtheta_guard1")
gurobi_model.addConstr(newtheta <= newtheta_ub,
name=f"newtheta_guard2")
gurobi_model.update()
gurobi_model.optimize()
assert gurobi_model.status == 2, "LP wasn't optimally solved"
return newthdot, newtheta
def more_actors_gurobi(data, n_actors, constraints,
first_run=False):
"""Multi-actor labeling using confidence weighted screen space distance.
Args:
first_run (bool):
Short first run for vis only.
n_actors (int):
How many actors to label.
constraints (Dict[str, Dict[int, int]]):
{frame_str => {pose_id => actor_id}}.
first_run (bool):
Is this the very first run (limit runtime for only vis).
Returns:
pose_ids (Dict[int, Dict[int, int]]):
{frame_id => {actor_id => pose_id}}
problem (ActorProblem2):
Labeling problem.
data (PosesWrapper):
Wrapped data for visualization.
"""
# color_norm = cmNormalize(vmin=0, vmax=n_actors+1)
# scalar_map = cm.ScalarMappable(norm=color_norm, cmap='gist_earth')
# colors = [tuple(c * 255. for c in scalar_map.to_rgba(i+1))
# for i in range(n_actors)]
# print(colors)
# raise RuntimeError("")
if isinstance(data, Skeleton):
data = SkeletonPosesWrapper(skeleton=data)
else:
assert isinstance(data, dict), "%s" % type(data)
data = DataPosesWrapper(data=data)
is_conf_normalized = data.is_confidence_normalized()
m = Model('Stealth actors')
w_unary = 1. # positive unary is a bonus
pose_not_present_cost = w_unary * -1000 # negative unary is a penalty
problem = ActorProblem2(n_actors=n_actors,
pose_not_present_unary_cost=pose_not_present_cost,
# positive pairwise is a penalty
pose_not_present_pw_cost=1000. * w_unary)
objective = None
prev_pose_in_2d = None
prev_frame_id = None
for frame_id in data.get_frames():
# try:
# frame_id = int(frame_str.split('_')[1])
# except ValueError:
# print("skipping key %s" % frame_id)
# continue
frame_str = "color_%05d" % frame_id
# if frame_id > 30:
# break
# pose_in = np.array(data[frame_str][u'centered_3d'])
# pose_in_2d = np.array(data[frame_str][u'pose_2d'])
pose_in_2d = data.get_poses_2d(frame_id=frame_id)
# visible = np.array(data[frame_str][u'visible'])
# vis_f = np.array(data[frame_str][u'visible_float'])
vis_f = data.get_confidences(frame_id=frame_id)
assert pose_in_2d.ndim == 3, "no: %s" % repr(pose_in_2d.shape)
problem.add_frame(frame_id,
n_vars=pose_in_2d.shape[0])
# unary
min_count = 0
for pose_id in range(pose_in_2d.shape[0]):
conf = vis_f[pose_id, ...]
if not is_conf_normalized:
conf = get_conf_thresholded(conf, thresh_log_conf=None,
dtype_np=np.float32)
cnt = np.sum(conf > 0.5)
if cnt > conf.shape[0] // 2:
min_count += 1
unary = w_unary * np.sum(conf) / conf.shape[0]
if frame_id == 251:
print("here")
# print("[%s] unary: %s" % (frame_id, unary))
problem.add_unary(frame_id, pose_id, cost=unary)
problem.set_min_count(frame_id, min_count)
# pairwise
if prev_pose_in_2d is not None:
for prev_pose_id in range(prev_pose_in_2d.shape[0]):
prev_pose = prev_pose_in_2d[prev_pose_id, :, :]
for pose_id in range(pose_in_2d.shape[0]):
pose = pose_in_2d[pose_id, :, :]
dist = prev_pose - pose
dist = np.linalg.norm(dist, axis=1)
dist *= prev_vis_f[prev_pose_id, ...] * vis_f[pose_id, ...]
# lg.debug("dist: %s" % repr(dist.shape))
cost = np.sum(dist, axis=0)
# if cost > 200:
# cost = 1e4
# cost /= 1500
# lg.debug("cost: %s" % cost)
problem.add_pw_cost(prev_frame_id, prev_pose_id,
frame_id, pose_id, cost)
prev_pose_in_2d = pose_in_2d
prev_vis_f = vis_f
prev_frame_id = frame_id
gb_vars = m.addVars(problem.get_n_vars(), vtype=GRB.BINARY)
# for lin_id in range(problem.get_n_vars()):
# gb_vars[lin_id].set(problem.get_init_for_lin_id(lin_id))
# unary: we want to maximize confidence
for lin_id, cost in problem._unary.items():
objective -= gb_vars[lin_id] * cost
# pairwise
for (lin_id0, lin_id1), cost in problem._pw.items():
objective += gb_vars[lin_id0] * gb_vars[lin_id1] * cost
# print("NO PAIRWISE!!!")
# a pose can only be labelled once per frame, either
# actor0, or actor1, etc.
for (frame_id, pose_id), lin_ids in problem._constr_p.items():
constr = None
for lin_id in lin_ids:
if constr is None:
constr = gb_vars[lin_id]
else:
constr += gb_vars[lin_id]
# lg.debug("[%d] pose %d can only be %s" % (frame_id, pose_id, lin_ids))
m.addConstr(constr <= 1)
# an actor can only be used once per frame, either
# pose0, or pose1, etc. Note: last actor can be used multiple times,
# it's the "pose not present" label.
for (frame_id, actor_id), lin_ids in problem._constr_a.items():
constr = None
for lin_id in lin_ids:
if constr is None:
constr = gb_vars[lin_id]
else:
constr += gb_vars[lin_id]
# lg.debug("[%d] actor %d can only be %s"
# % (frame_id, actor_id, lin_ids))
m.addConstr(constr == 1)
# maximum number of poses chosen to be visible <= n_actors
# for frame_id, lin_ids in problem._constr_f.items():
# constr = None
# for lin_id in lin_ids:
# if constr is None:
# constr = gb_vars[lin_id]
# else:
# constr += gb_vars[lin_id]
# m.addConstr(constr <= problem._n_actors)
first_constrained = False # type: bool
min_frame_id = min(data.get_frames()) # type: int
assert isinstance(min_frame_id, int)
# anchor first pose as first actor
if constraints and 'labels' in constraints:
for frame_str, labels in constraints['labels'].items():
frame_id = int(frame_str.split('_')[1])
if isinstance(labels, list):
assert len(labels) == n_actors, \
"frame: %d, %s %s" % (frame_id, len(labels), n_actors)
# assert len(set(labels)) == len(labels), \
# "%s: %s" % (set(labels), labels)
labels = {i: v for i, v in enumerate(labels)}
for actor_id, pose_id in labels.items():
pose_id = int(pose_id)
if pose_id < 0:
pose_id = problem._max_pose_ids[frame_id]
lin_id = problem.get_lin_id(frame_id, pose_id, actor_id)
m.addConstr(gb_vars[lin_id] == 1)
if not first_constrained and frame_id == min_frame_id \
and actor_id == 0:
first_constrained = True
if not first_constrained:
m.addConstr(gb_vars[0] == 1)
# m.addConstr(gb_vars[36] == 1)
# m.addConstr(gb_vars[40] == 1)
m.setObjective(objective, GRB.MINIMIZE)
m.Params.timeLimit = 300 if not first_run else 10
# m.solver.callSolver(m)
m.optimize()
pose_ids = defaultdict(dict)
prev_frame_id = None
prev_lin_ids = {}
curr_lin_ids = {}
labelings = defaultdict(dict)
for lin_id, v in enumerate(m.getVars()):
frame_id, pose_id, actor_id = \
problem.get_frame_id_pose_id_actor_id(lin_id)
# print("[%d] %s: %s; pose %d is %sactor %s"
# % (frame_id, v.varName, v.x, pose_id,
# "not " if v.x < 0.5 else "", actor_id))
problem._solution[lin_id] = v.x
if prev_frame_id is not None:
if prev_frame_id != frame_id:
prev_lin_ids = copy.deepcopy(curr_lin_ids)
curr_lin_ids.clear()
# print("[#{f:d}][{l:d}] unary for p{p0:d}, a{a0:d} is {"
# "cost:f}{chosen:s}".format(
# f=frame_id, p0=pose_id, a0=actor_id,
# cost=problem._unary[lin_id], l=lin_id,
# chosen=" <-- chosen" if v.x > 0.5 else ""
# ))
if v.x > 0.5:
curr_lin_ids[lin_id] = {"frame_id": frame_id,
"pose_id": pose_id,
"actor_id": actor_id}
if pose_id == problem._max_pose_ids[frame_id]:
pose_id = -1
if frame_id in pose_ids and actor_id != n_actors:
assert actor_id not in pose_ids[frame_id], "no"
try:
pose_ids[frame_id][actor_id] = pose_id
except KeyError:
pose_ids[frame_id] = {actor_id: pose_id}
labelings[frame_id][pose_id] = actor_id
# print("pw: %s" % problem._pw[lin_id])
# for lin_id0, entries0 in prev_lin_ids.items():
# if (lin_id0, lin_id) in problem._pw:
# print("[#{f:d}] pw {l0:d}(p{p0:d},a{a0:d})"
# "->{l1:d}(p{p1:d},a{a1:d}) is {cost:f}".format(
# l0=lin_id0, l1=lin_id,
# cost=problem._pw[(lin_id0, lin_id)],
# a0=entries0['actor_id'], a1=actor_id,
# f=frame_id, p0=entries0['pose_id'], p1=pose_id
# ))
prev_frame_id = frame_id
# enforce constraints
# if constraints and 'labels' in constraints:
# for frame_str, labels in constraints['labels'].items():
# frame_id = int(frame_str.split('_')[1])
# if isinstance(labels, list):
# labels = {v: i for i, v in enumerate(labels)}
# for pose_id, actor_id in labels.items():
# pose_ids[frame_id][actor_id] = int(pose_id)
try:
for frame_id in labelings:
if frame_id % 5:
continue
print("\"color_%05d\": {%s},"
% (frame_id,
", ".join(["\"%s\": %s" % (key, val)
for key, val in labelings[frame_id].items()])))
except TypeError:
pass
# if we have more, pick the first...
# if len(pose_in.shape) > 2:
# pose_in = pose_in[0, :, :]
# pose_in_2d = pose_in_2d[0, :, :]
# visible = visible[0]
return pose_ids, problem, data
def init_model(self, **kwargs):
greedy_start = kwargs.get("greedy_start", True)
verbose = kwargs.get("verbose", False)
use_cliques = kwargs.get("use_cliques", False)
if greedy_start:
if verbose:
print("Computing greedy solution")
greedy_solver = GreedyColoring(self.coloring_problem,
params_objective_function=self.params_objective_function)
result_store = greedy_solver.solve(strategy=NXGreedyColoringMethod.best, verbose=verbose)
self.start_solution = result_store.get_best_solution_fit()[0]
else:
if verbose:
print("Get dummy solution")
solution = self.coloring_problem.get_dummy_solution()
self.start_solution = solution
nb_colors = self.start_solution.nb_color
color_model = Model("color")
colors_var = {}
range_node = range(self.number_of_nodes)
range_color = range(nb_colors)
for node in self.nodes_name:
for color in range_color:
colors_var[node, color] = color_model.addVar(vtype=GRB.BINARY,
obj=0,
name="x_" + str((node, color)))
one_color_constraints = {}
for n in range_node:
one_color_constraints[n] = color_model.addConstr(quicksum([colors_var[n, c] for c in range_color]) == 1)
color_model.update()
cliques = []
g = self.graph.to_networkx()
if use_cliques:
for c in nx.algorithms.clique.find_cliques(g):
cliques += [c]
cliques = sorted(cliques, key=lambda x: len(x), reverse=True)
else:
cliques = [[e[0], e[1]] for e in g.edges()]
cliques_constraint = {}
index_c = 0
opt = color_model.addVar(vtype=GRB.INTEGER, lb=0, ub=nb_colors, obj=1)
if use_cliques:
for c in cliques[:100]:
cliques_constraint[index_c] = color_model.addConstr(quicksum([(color_i + 1) * colors_var[node, color_i]
for node in c
for color_i in range_color])
>= sum([i + 1 for i in range(len(c))]))
cliques_constraint[(index_c, 1)] = color_model.addConstr(quicksum([colors_var[node, color_i]
for node in c
for color_i in range_color])
<= opt)
index_c += 1
edges = g.edges()
constraints_neighbors = {}
for e in edges:
for c in range_color:
constraints_neighbors[(e[0], e[1], c)] = \
color_model.addConstr(colors_var[e[0], c] + colors_var[e[1], c] <= 1)
for n in range_node:
color_model.addConstr(quicksum([(color_i + 1) * colors_var[n, color_i] for color_i in range_color]) <= opt)
color_model.update()
color_model.modelSense = GRB.MINIMIZE
color_model.setParam(GRB.Param.Threads, 8)
color_model.setParam(GRB.Param.PoolSolutions, 10000)
color_model.setParam(GRB.Param.Method, -1)
color_model.setParam("MIPGapAbs", 0.001)
color_model.setParam("MIPGap", 0.001)
color_model.setParam("Heuristics", 0.01)
self.model = color_model
self.variable_decision = {"colors_var": colors_var}
self.constraints_dict = {"one_color_constraints": one_color_constraints,
"constraints_neighbors": constraints_neighbors}
self.description_variable_description = {"colors_var": {"shape": (self.number_of_nodes, nb_colors),
"type": bool,
"descr": "for each node and each color,"
" a binary indicator"}}
self.description_constraint["one_color_constraints"] = {"descr": "one and only one color "
"should be assignated to a node"}
self.description_constraint["constraints_neighbors"] = {"descr": "no neighbors can have same color"}
def apply_dynamic(input, gurobi_model: grb.Model, action, env_input_size):
'''
:param input:
:param gurobi_model:
:param action_ego:
:param t:
:return:
lead 100km/h 28m/s
ego 130km/h 36.1 m/s
'''
v_lead = 28
max_speed = 36.0
delta_x = input[0]
v_ego = input[1]
z = gurobi_model.addMVar(shape=(env_input_size, ),
lb=float("-inf"),
name=f"x_prime")
const_acc = 3
dt = .1 # seconds
if action == 0:
acceleration = -const_acc
elif action == 1:
acceleration = const_acc
else:
acceleration = 0
v_ego_prime_temp1 = gurobi_model.addVar()
v_ego_prime_temp2 = gurobi_model.addVar()
v_next = v_ego._vararr[0] + acceleration * dt
gurobi_model.addConstr(v_ego_prime_temp1 == v_next, name=f"v_constr")
gurobi_model.addConstr(v_ego_prime_temp2 == grb.min_(
max_speed, v_ego_prime_temp1),
name=f"v_constr")
v_ego_prime = grb.MVar(v_ego_prime_temp2) # convert from Var to MVar
v_lead_prime = v_lead
delta_prime_v = v_lead_prime - v_ego_prime
delta_prime_v_temp = gurobi_model.addMVar(shape=(1, ),
lb=float("-inf"),
name=f"delta_prime_v_temp")
gurobi_model.addConstr(delta_prime_v_temp == delta_prime_v,
name=f"delta_prime_v_constr")
delta_x_prime = delta_x + delta_prime_v_temp * dt
# x_lead_prime = x_lead + v_lead_prime * dt
# x_ego_prime = x_ego + v_ego_prime * dt
gurobi_model.addConstr(z[0] == delta_x_prime, name=f"dyna_constr_1")
gurobi_model.addConstr(z[1] == v_ego_prime, name=f"dyna_constr_3")
return z
def init_model(self, **kwargs):
nb_facilities = self.facility_problem.facility_count
nb_customers = self.facility_problem.customer_count
use_matrix_indicator_heuristic = kwargs.get("use_matrix_indicator_heuristic", True)
if use_matrix_indicator_heuristic:
n_shortest = kwargs.get("n_shortest", 10)
n_cheapest = kwargs.get("n_cheapest", 10)
matrix_fc_indicator, matrix_length = prune_search_space(self.facility_problem,
n_cheapest=n_cheapest,
n_shortest=n_shortest)
else:
matrix_fc_indicator, matrix_length = prune_search_space(self.facility_problem,
n_cheapest=nb_facilities,
n_shortest=nb_facilities)
s = Model("facilities")
x = {}
for f in range(nb_facilities):
for c in range(nb_customers):
if matrix_fc_indicator[f, c] == 0:
x[f, c] = 0
elif matrix_fc_indicator[f, c] == 1:
x[f, c] = 1
elif matrix_fc_indicator[f, c] == 2:
x[f, c] = s.addVar(vtype=GRB.BINARY,
obj=0,
name="x_" + str((f, c)))
facilities = self.facility_problem.facilities
customers = self.facility_problem.customers
used = s.addVars(nb_facilities, vtype=GRB.BINARY, name="y")
constraints_customer = {}
for c in range(nb_customers):
constraints_customer[c] = s.addConstr(quicksum([x[f, c] for f in range(nb_facilities)]) == 1)
# one facility
constraint_capacity = {}
for f in range(nb_facilities):
s.addConstrs(used[f] >= x[f, c] for c in range(nb_customers))
constraint_capacity[f] = s.addConstr(quicksum([x[f, c] * customers[c].demand
for c in range(nb_customers)]) <= facilities[f].capacity)
s.update()
new_obj_f = LinExpr(0.)
new_obj_f += quicksum([facilities[f].setup_cost * used[f] for f in range(nb_facilities)])
new_obj_f += quicksum([matrix_length[f, c] * x[f, c]
for f in range(nb_facilities)
for c in range(nb_customers)])
s.setObjective(new_obj_f)
s.update()
s.modelSense = GRB.MINIMIZE
s.setParam(GRB.Param.Threads, 4)
s.setParam(GRB.Param.PoolSolutions, 10000)
s.setParam(GRB.Param.Method, 1)
s.setParam("MIPGapAbs", 0.00001)
s.setParam("MIPGap", 0.00000001)
self.model = s
self.variable_decision = {"x": x}
self.constraints_dict = {"constraint_customer": constraints_customer,
"constraint_capacity": constraint_capacity}
self.description_variable_description = {"x": {"shape": (nb_facilities, nb_customers),
"type": bool,
"descr": "for each facility/customer indicate"
" if the pair is active, meaning "
"that the customer c is dealt with facility f"}}
self.description_constraint = {"Im lazy."}
print("Initialized")
class LPKnapsackGurobi(SolverDO):
def __init__(self,
knapsack_model: KnapsackModel,
params_objective_function: ParamsObjectiveFunction = None):
self.knapsack_model = knapsack_model
self.model = None
self.variable_decision = {}
self.constraints_dict = {}
self.description_variable_description = {}
self.description_constraint = {}
self.aggreg_sol, self.aggreg_dict, self.params_objective_function = \
build_aggreg_function_and_params_objective(problem=self.knapsack_model,
params_objective_function=params_objective_function)
def init_model(self, **args):
warm_start = args.get('warm_start', {})
self.model = Model("Knapsack")
self.variable_decision = {"x": {}}
self.description_variable_description = {
"x": {
"shape":
self.knapsack_model.nb_items,
"type":
bool,
"descr":
"dictionary with key the item index \
and value the boolean value corresponding \
to taking the item or not"
}
}
self.description_constraint["weight"] = {
"descr": "sum of weight of used items doesn't exceed max capacity"
}
weight = {}
list_item = self.knapsack_model.list_items
max_capacity = self.knapsack_model.max_capacity
x = {}
for item in list_item:
i = item.index
x[i] = self.model.addVar(vtype=GRB.BINARY,
obj=item.value,
name="x_" + str(i))
if i in warm_start:
x[i].start = warm_start[i]
x[i].varhinstval = warm_start[i]
weight[i] = item.weight
self.variable_decision["x"] = x
self.model.update()
self.constraints_dict["weight"] = self.model.addConstr(
quicksum([weight[i] * x[i] for i in x]) <= max_capacity)
self.model.update()
self.model.setParam("TimeLimit", 200)
self.model.modelSense = GRB.MAXIMIZE
self.model.setParam(GRB.Param.PoolSolutions, 10000)
self.model.setParam("MIPGapAbs", 0.00001)
self.model.setParam("MIPGap", 0.00000001)
def retrieve_solutions(self, range_solutions: Iterable[int]):
# nObjectives = S.NumObj
solutions = []
fits = []
# x = S.getVars()
for s in range_solutions:
weight = 0
xs = {}
self.model.params.SolutionNumber = s
obj = self.model.getAttr("ObjVal")
for e in self.variable_decision["x"]:
value = self.variable_decision["x"][e].getAttr('Xn')
if value <= 0.1:
xs[e] = 0
continue
xs[e] = 1
weight += self.knapsack_model.index_to_item[e].weight
solutions += [
KnapsackSolution(problem=self.knapsack_model,
value=obj,
weight=weight,
list_taken=[xs[e] for e in sorted(xs)])
]
fits += [self.aggreg_sol(solutions[-1])]
return ResultStorage(
list_solution_fits=[(s, f) for s, f in zip(solutions, fits)],
mode_optim=self.params_objective_function.sense_function)
def solve(self, parameter_gurobi: ParametersMilp):
self.model.setParam("TimeLimit", parameter_gurobi.TimeLimit)
self.model.modelSense = GRB.MAXIMIZE
self.model.setParam(GRB.Param.PoolSolutions,
parameter_gurobi.PoolSolutions)
self.model.setParam("MIPGapAbs", parameter_gurobi.MIPGapAbs)
self.model.setParam("MIPGap", parameter_gurobi.MIPGap)
print("optimizing...")
self.model.optimize()
nSolutions = self.model.SolCount
nObjectives = self.model.NumObj
objective = self.model.getObjective().getValue()
print('Problem has', nObjectives, 'objectives')
print('Gurobi found', nSolutions, 'solutions')
if parameter_gurobi.retrieve_all_solution:
solutions = self.retrieve_solutions(list(range(nSolutions)))
else:
solutions = self.retrieve_solutions([0])
return solutions
def solve_lns(self, parameter_gurobi: ParametersMilp,
init_solution: KnapsackSolution,
fraction_decision_fixed: float, nb_iteration_max: int):
self.model.setParam("TimeLimit", parameter_gurobi.TimeLimit)
self.model.setParam("OutputFlag", 0)
self.model.modelSense = GRB.MAXIMIZE
self.model.setParam(GRB.Param.PoolSolutions,
parameter_gurobi.PoolSolutions)
self.model.setParam("MIPGapAbs", parameter_gurobi.MIPGapAbs)
self.model.setParam("MIPGap", parameter_gurobi.MIPGap)
current_solution = init_solution
constraints = {}
list_solutions = [current_solution]
list_objective = [current_solution.value]
objective = init_solution.value
for k in trange(nb_iteration_max):
for c in constraints:
self.model.remove(constraints[c])
self.add_init_solution(current_solution)
fixed_variable = set(
random.sample(
self.variable_decision["x"].keys(),
int(fraction_decision_fixed *
len(self.variable_decision["x"]))))
constraints = self.fix_decision(current_solution, fixed_variable)
self.model.optimize()
nSolutions = self.model.SolCount
nObjectives = self.model.NumObj
objective = self.model.getObjective().getValue()
if parameter_gurobi.retrieve_all_solution:
solutions = self.retrieve_solutions(list(range(nSolutions)))
else:
solutions = self.retrieve_solutions([0])
current_solution = solutions[0]
list_solutions += [solutions[0]]
list_objective += [solutions[0].value]
print("Last obj : ", list_objective[-1])
fig, ax = plt.subplots(1)
ax.plot(list_objective)
plt.show()
def add_init_solution(self, init_solution: KnapsackSolution):
for i in self.variable_decision["x"]:
self.variable_decision["x"][i].start = init_solution.list_taken[i]
self.variable_decision["x"][
i].varhintval = init_solution.list_taken[i]
def fix_decision(self, init_solution: KnapsackSolution,
fixed_variable_keys):
constraints = {}
for i in fixed_variable_keys:
constraints[i] = self.model.addConstr(
self.variable_decision["x"][i] == init_solution.list_taken[i])
return constraints
def describe_the_model(self):
return str(self.description_variable_description) + "\n" + str(
self.description_constraint)
def apply_dynamic(input, gurobi_model: grb.Model, env_input_size):
'''
:param input:
:param gurobi_model:
:param t:
:return:
'''
p = input[0]
v = input[1]
dt = 0.1
z = gurobi_model.addMVar(shape=(env_input_size, ),
lb=float("-inf"),
name=f"x_prime")
# pos_max = gurobi_model.addMVar(shape=(1,), lb=float("-inf"), name=f"pos_max")
v_second = v - 9.81 * dt
p_second = p + dt * v_second
gurobi_model.addConstr(z[1] == v_second, name=f"dyna_constr_1")
# gurobi_model.addConstr(pos_max == grb.max_([p_second, 0]), name=f"dyna_constr_2")
# gurobi_model.addConstr(z[1] == p_second, name=f"dyna_constr_2")
max_switch = gurobi_model.addMVar(lb=0,
ub=1,
shape=p_second.shape,
vtype=grb.GRB.INTEGER,
name=f"max_switch")
M = 10e6
# gurobi_model.addConstr(v == grb.max_(0, gurobi_vars[-1]))
gurobi_model.addConstr(z[0] >= p_second)
gurobi_model.addConstr(z[0] <= p_second + M * max_switch)
gurobi_model.addConstr(z[0] >= 0)
gurobi_model.addConstr(z[0] <= M - M * max_switch)
return z
def apply_dynamic(input, gurobi_model: grb.Model, action, env_input_size):
'''
:param input:
:param gurobi_model:
:param t:
:return:
'''
n_actions = 10
K = 1.0
r = 0.3
n_fish = input[0]
quota = (action / n_actions) * K
fish_population = gurobi_model.addMVar(shape=(1, ),
lb=float("-inf"),
name=f"fish_population")
gurobi_model.addConstr(fish_population[0] == (n_fish + 1) * K,
name=f"dyna_constr_1")
fish_population_prime = gurobi_model.addMVar(
shape=(1, ), lb=0, name=f"fish_population_prime") # lb set to 0
gurobi_model.addConstr(fish_population_prime[0] == (fish_population -
quota),
name=f"dyna_constr_2")
# gurobi_model.setObjective(fish_population_prime[0].sum(), grb.GRB.MAXIMIZE)
# gurobi_model.optimize()
# max_fish_population_prime = fish_population_prime[0].X # todo switch to fish_population_prime
# gurobi_model.setObjective(fish_population_prime[0].sum(), grb.GRB.MINIMIZE)
# gurobi_model.optimize()
# min_fish_population_prime = fish_population_prime[0].X
# step_fish_population_prime = 0.1
# split_fish_population_prime = np.arange(min(min_fish_population_prime, 0), min(max_fish_population_prime, 0), step_fish_population_prime)
# split = []
# for fish_pop in split_fish_population_prime:
# lb = fish_pop
# ub = min(fish_pop + step_fish_population_prime, max_fish_population_prime)
# split.append((interval([lb, ub])))
# fish_growth_table = []
# while (len(split)):
# fish_pop_interval = split.pop()
fish_population_post = gurobi_model.addMVar(
shape=(1, ), lb=float("-inf"), name=f"fish_population_post")
growth = r * fish_population_prime # * (1.0 - (fish_population_prime[0] / K))
growth1 = gurobi_model.addMVar(shape=(1, ),
lb=float("-inf"),
name=f"growth1")
gurobi_model.addConstr(growth1 == growth, name=f"dyna_constr_3")
second_growth = (1.0 - (fish_population_prime / K))
growth2 = gurobi_model.addMVar(shape=(1, ),
lb=float("-inf"),
name=f"growth2")
gurobi_model.addConstr(growth2 == second_growth, name=f"dyna_constr_4")
third_growth = growth1 @ growth2
gurobi_model.addConstr(fish_population_post == fish_population_prime +
third_growth,
name=f"dyna_constr_5")
x_prime = gurobi_model.addMVar(shape=(1, ),
lb=float("-inf"),
name=f"x_prime")
gurobi_model.addConstr(x_prime[0] == (fish_population_post / K) - 1,
name=f"dyna_constr_6")
return x_prime
def generate_angle_milp(gurobi_model: grb.Model, input,
sin_cos_table: List[Tuple]):
"""MILP method
input: theta, thetadot
output: thetadotdot, xdotdot (edited)
l_{theta, i}, l_{thatdot,i}, l_{thetadotdot, i}, l_{xdotdot, i}, u_....
sum_{i=1}^k l_{x,i} - l_{x,i}*z_i <= x <= sum_{i=1}^k u_{x,i} - u_{x,i}*z_i, for each variable x
sum_{i=1}^k l_{theta,i} - l_{theta,i}*z_i <= theta <= sum_{i=1}^k u_{theta,i} - u_{theta,i}*z_i
"""
theta = input[2]
theta_dot = input[3]
k = len(sin_cos_table)
zs = []
thetaacc = gurobi_model.addMVar(shape=(1, ),
lb=float("-inf"),
name="thetaacc")
xacc = gurobi_model.addMVar(shape=(1, ), lb=float("-inf"), name="xacc")
for i in range(k):
z = gurobi_model.addMVar(lb=0,
ub=1,
shape=(1, ),
vtype=grb.GRB.INTEGER,
name=f"part_{i}")
zs.append(z)
gurobi_model.addConstr(k - 1 == sum(zs), name=f"const_milp1")
theta_lb = 0
theta_ub = 0
theta_dot_lb = 0
theta_dot_ub = 0
thetaacc_lb = 0
thetaacc_ub = 0
xacc_lb = 0
xacc_ub = 0
for i in range(k):
theta_interval, theta_dot_interval, theta_acc_interval, xacc_interval = sin_cos_table[
i]
theta_lb += theta_interval[0] - theta_interval[0] * zs[i]
theta_ub += theta_interval[1] - theta_interval[1] * zs[i]
theta_dot_lb += theta_dot_interval[
0] - theta_dot_interval[0] * zs[i]
theta_dot_ub += theta_dot_interval[
1] - theta_dot_interval[1] * zs[i]
thetaacc_lb += theta_acc_interval[0] - theta_acc_interval[0] * zs[i]
thetaacc_ub += theta_acc_interval[1] - theta_acc_interval[1] * zs[i]
xacc_lb += xacc_interval[0] - xacc_interval[0] * zs[i]
xacc_ub += xacc_interval[1] - xacc_interval[1] * zs[i]
# eps = 1e-9
gurobi_model.addConstr(theta >= theta_lb, name=f"theta_guard1")
gurobi_model.addConstr(theta <= theta_ub, name=f"theta_guard2")
gurobi_model.addConstr(theta_dot >= theta_dot_lb,
name=f"theta_dot_guard1")
gurobi_model.addConstr(theta_dot <= theta_dot_ub,
name=f"theta_dot_guard2")
gurobi_model.addConstr(thetaacc >= thetaacc_lb,
name=f"thetaacc_guard1")
gurobi_model.addConstr(thetaacc <= thetaacc_ub,
name=f"thetaacc_guard2")
gurobi_model.addConstr(xacc >= xacc_lb, name=f"xacc_guard1")
gurobi_model.addConstr(xacc <= xacc_ub, name=f"xacc_guard2")
gurobi_model.update()
gurobi_model.optimize()
if gurobi_model.status == 4:
gurobi_model.setParam("DualReductions", 0)
gurobi_model.update()
gurobi_model.optimize()
# assert gurobi_model.status == 2, f"LP wasn't optimally solved. gurobi status {gurobi_model.status}"
return thetaacc, xacc
def identify_actors(data):
m = Model('Stealth actors')
problem = ActorProblem()
objective = None
prev_pose_in_2d = None
prev_frame_id = None
for frame_str in sorted(data):
try:
frame_id = int(frame_str.split('_')[1])
except ValueError:
print("skipping key %s" % frame_id)
continue
pose_in = np.array(data[frame_str][u'centered_3d'])
pose_in_2d = np.array(data[frame_str][u'pose_2d'])
visible = np.array(data[frame_str][u'visible'])
assert pose_in_2d.ndim == 3, "no: %s" % repr(pose_in_2d.shape)
problem.add_frame(frame_id, pose_in_2d.shape[0])
if prev_pose_in_2d is not None:
for prev_pose_id in range(prev_pose_in_2d.shape[0]):
prev_pose = prev_pose_in_2d[prev_pose_id, :, :]
for pose_id in range(pose_in_2d.shape[0]):
pose = pose_in_2d[pose_id, :, :]
dist = prev_pose - pose
lg.debug("dist: %s" % repr(dist.shape))
cost = np.sum(np.linalg.norm(dist, axis=1), axis=0)
lg.debug("cost: %s" % cost)
problem.add_cost(prev_frame_id, prev_pose_id, frame_id, pose_id, cost)
prev_pose_in_2d = pose_in_2d
prev_frame_id = frame_id
gb_vars = m.addVars(problem.get_n_vars(), vtype=GRB.BINARY)
for (lin_id0, lin_id1), cost in problem._pw.items():
# lin_id0 = problem.get_lin_id(prev_frame_id, prev_pose_id)
# lin_id1 = problem.get_lin_id(frame_id, pose_id)
objective += gb_vars[lin_id0] * gb_vars[lin_id1] * cost
for frame_id, lin_ids in problem._constr.items():
constr = None
for lin_id in lin_ids:
if constr is None:
constr = gb_vars[lin_id]
else:
constr += gb_vars[lin_id]
m.addConstr(constr == 1)
m.setObjective(objective, GRB.MINIMIZE)
# m.solver.callSolver(m)
m.optimize()
pose_ids = dict()
for lin_id, v in enumerate(m.getVars()):
print(v.varName, v.x)
if v.x > 0.5:
frame_id, pose_id = problem.get_frame_id_pose_id(lin_id)
assert frame_id not in pose_ids, "no"
pose_ids[frame_id] = pose_id
# if we have more, pick the first...
# if len(pose_in.shape) > 2:
# pose_in = pose_in[0, :, :]
# pose_in_2d = pose_in_2d[0, :, :]
# visible = visible[0]
return pose_ids