def plot(self):
# plot current sdf
# curr_obs_inds = self.sdf.to_inds(self.curr_obs[:,0], self.curr_obs[:,1])
# print 'obs inds', curr_obs_inds
# print 'sdf\n', self.sdf.data()
cmap = np.zeros((256, 3), dtype='uint8')
cmap[:128,0] = 255
cmap[:128,1] = np.linspace(0, 255, 128).astype(int)
cmap[:128,2] = np.linspace(0, 255, 128).astype(int)
cmap[128:,0] = np.linspace(255, 0, 128).astype(int)
cmap[128:,1] = np.linspace(255, 0, 128).astype(int)
cmap[128:,2] = 255
# cmap[:,0] = range(256)
# cmap[:,2] = range(256)[::-1]
colors = np.clip((self.sdf.to_image_fmt()*100 + 128), 0, 255).astype(int)
flatland.show_2d_image(cmap[colors], "sdf")
cv2.moveWindow("sdf", 550, 0)
# plot flow field
# print 'curr flow\n', self.curr_u.data()
self.curr_u.show_as_vector_field("u")
cv2.moveWindow("u", 0, 600)
total, costs = solvers._eval_cost(PIXEL_AREA, self.curr_obs, self.prev_sdf, self.sdf, self.curr_u, ignore_obs=self.curr_obs is None, return_full=True)
print 'total cost', total
print 'individual costs', costs
def eval_cost(self, sdf, u, return_full=False): return solvers._eval_cost(PIXEL_AREA, self.curr_obs, self.prev_sdf, sdf, u, return_full=return_full)
