def calc_camera_angle(self, time):
if len(self.camera_times) == 0:
return 100.0
idx_camera = utils.binary_search(time, self.camera_times)
nxt_idx_camera = idx_camera + 1
if idx_camera == len(self.camera_times)-1:
nxt_idx_camera = idx_camera
if idx_camera > 0:
idx_camera = idx_camera - 1
angle_value = self.camera_angles_predicted[idx_camera]
angle_value_nxt = self.camera_angles_predicted[nxt_idx_camera]
if self.debug:
self.out.write(str(angle_value) + " " + str(angle_value_nxt) + "\n")
self.out.write(str(self.camera_angles_predicted[-1]) + "\n")
self.out.flush()
if abs(angle_value) > 10.0:
return angle_value_nxt
if abs(angle_value_nxt) > 10.0:
return angle_value
angle = utils.interpolate_aprox(time, self.camera_times[idx_camera], \
self.camera_angles_predicted[idx_camera], \
self.camera_times[nxt_idx_camera], \
self.camera_angles_predicted[nxt_idx_camera])
return angle
def calc_camera_coordinates(self, time):
if len(self.camera_times) == 0:
return [0.0, 0.0, 0.0]
idx_camera = utils.binary_search(time, self.camera_times)
nxt_idx_camera = idx_camera + 1
if idx_camera == len(self.camera_times)-1:
nxt_idx_camera = idx_camera
if idx_camera > 0:
idx_camera = idx_camera - 1
#print len(self.camera_times), len(self.camera_coordinates_predicted), idx_camera
coordinates = [utils.interpolate_aprox(time, self.camera_times[idx_camera], \
self.camera_coordinates_predicted[idx_camera][i], \
self.camera_times[nxt_idx_camera], \
self.camera_coordinates_predicted[nxt_idx_camera][i]) for i in range(3)]
return coordinates
def showResults(self, path):
from PIL import Image, ImageFont, ImageDraw
import os
# print self.object_positions
for filename in sorted(os.listdir(path)):
if filename.endswith(".png"):
print filename[:-4]
t = int(filename[:-4])
print t
idx = utils.binary_search(t, self.camera_times)
if idx+1 == len(self.camera_times):
nxtidx = idx
idx = idx - 1
else:
nxtidx = idx + 1
print idx
pos = [utils.interpolate_aprox(t, self.camera_times[idx], \
self.object_positions[idx][i], \
self.camera_times[nxtidx], \
self.object_positions[nxtidx][i]) for i in range(3)]
IMAGE_HEIGHT = 701
IMAGE_WIDTH = 801
BIN = 0.1
row = int(round(math.floor(((((IMAGE_HEIGHT*BIN)/2.0) - pos[0])/(IMAGE_HEIGHT*BIN)) * IMAGE_HEIGHT)))
column = int(round(math.floor(((((IMAGE_WIDTH*BIN)/2.0) - pos[1])/(IMAGE_WIDTH*BIN)) * IMAGE_WIDTH)))
print pos, row, column
source_img = Image.open(path + filename)
draw = ImageDraw.Draw(source_img)
draw.rectangle(((column-10, row-10), (column+10, row+10)))
newimgpath = path + "res/" + filename
source_img.save(newimgpath, "PNG")
def calc_camera_distance(self, time):
#print "--------> Dist_", len(self.camera_times)
if len(self.camera_times) == 0:
return 0.0
idx_camera = utils.binary_search(time, self.camera_times)
nxt_idx_camera = idx_camera + 1
if idx_camera == len(self.camera_times)-1:
nxt_idx_camera = idx_camera
if idx_camera > 0:
idx_camera = idx_camera - 1
#print idx_camera, nxt_idx_camera
distance_val = self.camera_distances_predicted[idx_camera]
distance_val_nxt = self.camera_distances_predicted[nxt_idx_camera]
# print time, distance_val, distance_val_nxt
# print idx_camera
# print self.camera_times
# print self.camera_distances_predicted
# print self.camera_angles_predicted
if abs(distance_val) < eps:
return distance_val_nxt
if abs(distance_val_nxt) < eps:
return distance_val
distance = utils.interpolate_aprox(time, self.camera_times[idx_camera], \
self.camera_distances_predicted[idx_camera], \
self.camera_times[nxt_idx_camera], \
self.camera_distances_predicted[nxt_idx_camera])
return distance
def solve_time(self, time, idx=-1):
real_time = time - int(0.25*1000000000)
real_lidar_time = time #+ int(0.2*1000000000)
if self.debug:
self.out.write(str(real_time) + "\n")
self.out.flush()
#print real_time
# Estimate position
estimated_position = [0.0, 0.0, 0.0]
if not utils.isempty(self.last_position):
estimated_position = self.last_position
# KARMAN-FILTER
if False and not utils.isempty(self.last_velocity):
estimated_position = [estimated_position[i] + self.last_velocity[i]* \
(real_time/MS_IN_SEC - self.last_realtime/MS_IN_SEC) \
for i in range(len(estimated_position))]
#if idx == 350:
# print "------------->", idx
#print estimated_position
# if not utils.isempty(self.last_acceleration):
# estimated_position = [estimated_position[i] + 0.5*self.last_acceleration[i]* \
# math.pow(real_time/MS_IN_SEC - self.last_realtime/MS_IN_SEC, 2) \
# for i in range(len(estimated_position))]
self.mutexCamera.acquire()
try:
camera_angle = self.calc_camera_angle(real_time)
camera_distance = self.calc_camera_distance(real_time)
camera_coordinates = self.calc_camera_coordinates(real_time)
finally:
self.mutexCamera.release()
if self.debug:
self.out.write(str(camera_angle) + " " + str(camera_distance) + "\n")
self.out.flush()
#print camera_angle, camera_distance
if abs(camera_distance) > eps:
camera_coordinates = [camera_distance*math.cos(camera_angle), camera_distance*math.sin(camera_angle), 0.0]
else:
camera_coordinates = [0.0, 0.0, 0.0]
self.mutexRadar.acquire()
try:
prev_radar = self.get_previous_radar(real_time)
nxt_radar = self.get_next_radar(real_time)
prev_radar_time = self.get_previous_radar_time(real_time)
nxt_radar_timer = self.get_next_radar_time(real_time)
finally:
self.mutexRadar.release()
#print real_time
#print nxt_radar
#print prev_radar
# if self.debug:
# self.out.write(str(nxt_radar_timer) + "\n")
# self.out.write(str(nxt_radar) + "\n")
# self.out.write(str(prev_radar_time) + "\n")
# self.out.write(str(prev_radar) + "\n")
# self.out.flush()
radar_coordinates_prev = self.get_closest_radar(prev_radar, estimated_position)
radar_coordinates_next = self.get_closest_radar(nxt_radar, estimated_position)
radar_cam_coordinates_prev = self.get_closest_radar(prev_radar, camera_coordinates)
radar_cam_coordinates_next = self.get_closest_radar(nxt_radar, camera_coordinates)
#print radar_coordinates_prev, radar_coordinates_next
#print radar_cam_coordinates_prev, radar_cam_coordinates_next
radar_coordinates = [0.0, 0.0, 0.0]
radar_cam_coordinates = [0.0, 0.0, 0.0]
if utils.isempty(radar_coordinates_prev):
radar_coordinates = radar_coordinates_next
elif utils.isempty(radar_coordinates_next):
radar_coordinates = radar_coordinates_prev
else:
radar_coordinates = [utils.interpolate_aprox(real_time, prev_radar_time, \
radar_coordinates_prev[i], \
nxt_radar_timer, \
radar_coordinates_next[i]) for i in range(3)]
if utils.isempty(radar_cam_coordinates_prev):
radar_cam_coordinates = radar_cam_coordinates_next
elif utils.isempty(radar_cam_coordinates_next):
radar_cam_coordinates = radar_cam_coordinates_prev
else:
radar_cam_coordinates = [utils.interpolate_aprox(real_time, prev_radar_time, \
radar_cam_coordinates_prev[i], \
nxt_radar_timer, \
radar_cam_coordinates_next[i]) for i in range(3)]
#print "------->", camera_angle, math.atan2(radar_cam_coordinates[1], radar_cam_coordinates[0])
d = utils.distance_2d(radar_cam_coordinates, [0.0, 0.0, 0.0])
radar_cam_coordinates = [d*math.cos(camera_angle), d*math.sin(camera_angle), 0.0]
#print radar_cam_coordinates
#radar_position_prev = self.get_best_radar_position(prev_radar_time, estimated_position)
#radar_position_next = self.get_best_radar_position(nxt_radar_time, estimated_position)
#if len(self.object_positions) == 486:
# print radar_position_prev, radar_position_next, real_time
# print prev_radar_time, nxt_radar_time
#radar_position = []
#if utils.distance_2d(radar_position_prev, radar_position_next) < 10.0:
# radar_position = [utils.interpolate_aprox(real_time, prev_radar_time, \
# radar_position_prev[i], \
# nxt_radar_time, \
# radar_position_next[i]) for i in range(3)]
#else:
# radar_position = self.get_best_radar_position_full([radar_position_prev, radar_position_next], camera_angle, camera_distance, estimated_position)
lidar_position_prev = [0.0, 0.0, 0.0]
lidar_position_next = [0.0, 0.0, 0.0]
self.mutexLidar.acquire()
try:
if len(self.lidar_times) > 0:
prev_lidar_time = self.get_previous_lidar_time(real_lidar_time)
next_lidar_time = self.get_next_lidar_time(real_lidar_time)
guess = estimated_position
if utils.isempty(guess):
guess = camera_coordinates
#if not utils.isempty(estimated_position) and len(self.lidar_times) > 0:
lidar_position_prev = self.get_closest_lidar(prev_lidar_time, guess)
lidar_position_next = self.get_closest_lidar(next_lidar_time, guess)
#elif not utils.isempty(radar_position) and len(self.lidar_times) > 0:
# lidar_position_prev = self.get_closest_lidar(prev_lidar_time, radar_position)
# lidar_position_next = self.get_closest_lidar(next_lidar_time, radar_position)
finally:
self.mutexLidar.release()
if len(self.lidar_times) > 0:
lidar_position = [utils.interpolate_aprox(real_lidar_time, prev_lidar_time, \
lidar_position_prev[i], \
next_lidar_time, \
lidar_position_next[i]) for i in range(3)]
else:
lidar_position = [0.0, 0.0, 0.0]
#if len(self.object_positions) == 201:
# print "------------->"
# print camera_angle, radar_position
# object_position = [0.0, 0.0, 0.0]
# if abs(camera_angle) <1.0e-6 or utils.isempty(radar_position):
# if not utils.isempty(estimated_position) and \
# utils.distance_2d(lidar_position, [0, 0, 0]) < 25.0 and \
# utils.distance_2d(lidar_position, estimated_position) < 5.0:
# object_position = lidar_position[:]
# object_position[1] += 0.4
# object_position[0] -= 0.4
# else:
# radar_position = [0.0, 0.0, 0.0]
# object_position = radar_position[:]
# else:
# if not utils.isempty(lidar_position) or not utils.isempty(radar_position):
# if not utils.isempty(lidar_position) and utils.distance_2d(radar_position, [0, 0, 0]) < 15.0:
# print "Elegir lidar: ", lidar_position
# object_position = lidar_position[:]
# else:
# object_position = radar_position[:]
# object_position[2] = camera_coordinates[2]
# object_position[1] += 0.4
# print real_time
# print radar_position, camera_angle
# print object_position, lidar_position, estimated_position
# print prev_lidar_time, next_lidar_time
# print lidar_position_prev, lidar_position_next
hasLidar = not utils.isempty(lidar_position)
hasRadar = not utils.isempty(radar_coordinates)
hasRadarCam = not utils.isempty(radar_cam_coordinates)
if self.debug:
self.out.write("Lidar: " + str(hasLidar) + " " + str(lidar_position) + "\n")
self.out.write("Radar: " + str(hasRadar) + " " + str(radar_coordinates) + "\n")
self.out.write("RadarCam: " + str(hasRadarCam) + " " + str(radar_cam_coordinates) + "\n")
self.out.write("Camera: " + str(camera_coordinates) + "\n")
self.out.write("EstPos: " + str(estimated_position) + "\n")
self.out.flush()
object_position = [0.0, 0.0, 0.0]
if utils.isempty(estimated_position) or not hasLidar:
if not utils.isempty(estimated_position):
distanceRadar = utils.distance_ang(estimated_position, radar_coordinates)
distanceRadarCam = utils.distance_ang(estimated_position, radar_cam_coordinates)
if abs(distanceRadar - distanceRadarCam) > 15:
if hasRadarCam and utils.distance_ang(radar_cam_coordinates, camera_coordinates) < 5:
object_position = radar_cam_coordinates
else:
object_position = camera_coordinates
else:
if distanceRadar < distanceRadarCam:
object_position = radar_coordinates
else:
object_position = radar_cam_coordinates
else:
if hasRadarCam and utils.distance_ang(radar_cam_coordinates, camera_coordinates) < 5:
object_position = radar_cam_coordinates
else:
object_position = camera_coordinates
else:
distanceLidar = utils.distance_ang(camera_coordinates, lidar_position)
distanceRadar = utils.distance_ang(estimated_position, radar_coordinates)
distanceRadarCam = utils.distance_ang(estimated_position, radar_cam_coordinates)
if (abs(distanceLidar) < 5 or not hasRadarCam) \
and (utils.distance_ang([0.0, 0.0, 0.0], lidar_position) < 20.0 or utils.distance_ang(radar_cam_coordinates, lidar_position) < 5.0):
#print distanceLidar
if abs(distanceLidar) > 5:
if hasRadarCam and utils.distance_ang(radar_cam_coordinates, camera_coordinates) < 5:
object_position = radar_cam_coordinates
else:
object_position = camera_coordinates
else:
object_position = lidar_position
if self.type == "Car":
object_position = lidar_position
else:
if distanceRadar < distanceRadarCam:
object_position = radar_coordinates
else:
object_position = radar_cam_coordinates
if hasRadarCam and utils.distance_ang(radar_cam_coordinates, camera_coordinates) < 5:
object_position = radar_cam_coordinates
else:
object_position = camera_coordinates
#if utils.isempty(object_position) and not utils.isempty(camera_coordinates):
# object_position = camera_coordinates
if self.debug:
self.out.write("Result: " + str(object_position) + "\n")
self.out.flush()
#print "Result: ", object_position
#object_position[1] = abs(object_position[1]) - 2
# if not utils.isempty(object_position):
# self.x, self.P = kalman_xy(self.x, self.P, object_position[:2], self.R)
# tmp = (self.x[:2]).tolist()
# tmp = [tmp[0][0], tmp[1][0], 0.0]
# self.object_positions.append(tmp)
# else:
# self.object_positions.append(object_position)
d = utils.distance_2d(object_position, [0,.0, 0.0, 0.0])
if type == "Car":
object_position[2] = -0.00 - 0.0095*d
else:
object_position[2] = -0.70 - 0.0095*d
#object_position[1] = abs(object_position[1]) - 2
self.object_positions.append(object_position)
#print real_time, self.last_realtime
#print estimated_position, object_position
#print "-->", self.get_previous_lidar(real_time)
velocity = [0.0, 0.0, 0.0]
if not utils.isempty(object_position) and not utils.isempty(self.last_position):
velocity = [np.divide(np.sum([object_position[i], -self.last_position[i]], dtype=np.float64), np.sum( [real_time/MS_IN_SEC, -self.last_realtime/MS_IN_SEC], dtype=np.float64)) \
for i in range(len(object_position))]
acceleration = [0.0, 0.0, 0.0]
if not utils.isempty(velocity) and not utils.isempty(self.last_velocity):
acceleration = [(velocity[i] - self.last_velocity[i]) / (real_time/MS_IN_SEC - self.last_realtime/MS_IN_SEC) \
for i in range(len(velocity))]
self.object_velocity.append(velocity)
self.object_acceleration.append(acceleration)
self.last_position = object_position
self.last_velocity = velocity
self.last_acceleration = acceleration
self.last_realtime = real_time
