def getLinearVels(bagFile,
front_encoders,
back_encoders,
front_cmds,
back_cmds,
imu,
gps,
cmd_vels):
threshold = 0.1
gps_x = [x.pose.pose.position.x for x in gps_msgs]
gps_y = [y.pose.pose.position.y for y in gps_msgs]
time = [t.header.stamp.secs+t.header.stamp.nsecs/10.0**9 for t in gps_msgs]
gps_x = [x[i] for i in range(0,len(gps_x),2)]
gps_y = [x[i] for i in range(0,len(gps_y),2)]
time = [x[i] for i in range(0,len(time),2)]
print time
gps_time = movingAverage.movingAverage(\
[t.header.stamp.secs+t.header.stamp.nsecs/10.0**9
for t in gps_msgs],1)
gpsVels = velocity.getLinearGPSVelocities(gps_x,gps_y,time)
cmd_vels_time = numpy.linspace(imu_time[0],imu_time[-1],len(cmd_vel_msgs))
cmd_vels_x = [t.linear.x for t in cmd_vel_msgs]
cmdVels,GpsVels = sync.matchInput(cmd_vels_time,cmd_vels_x,gps_time,gpsVels,threshold)
return cmdVels,GpsVels
def alignLifetimeHist(data, n=15):
"""
Align lifetime histograms of different channels. Calculate moving averaged
histograms, caculate derivative and find peak. Then shift everything
so that the peaks are aligned.
===========================================================================
Input Meaning
---------- ---------------------------------------------------------------
data data object with microtime histograms
n width of the window for the calculation of the moving average
===========================================================================
Output Meaning
---------- ---------------------------------------------------------------
data same data object as input, but with histograms aligned in time
===========================================================================
"""
histList = [i for i in list(data.__dict__.keys()) if i.startswith('hist')]
for hist in histList:
# get histogram
histD = getattr(data, hist)
# calculate moving average
IAv = movingAverage(histD[:, 1], n=n)
# calculate derivative
derivative = IAv[1:] - IAv[0:-1]
# find maximum of derivative
maxInd = np.argmax(derivative)
# shift histogram
histD[:, 1] = np.roll(histD[:, 1], -maxInd)
# store shifted histogram in data object
setattr(data, hist, histD)
return data
if Select_GSR: #select on
for g in range (len(DataGS)):
#Try a changer?
try :
xG += [float(DataGS[g][0])]
except:
xG += [xG[-1]]
try :
#yG += [float(DataGS[g][1])]
queueY.append(float(DataGS[g][1]))
if len(queueY) > queueLen :
queueY.pop(0)
#yG += [lowpass.lowPass(queueY, 50, 50)]
yG += [movingAverage.movingAverage(queueY)]
except: #si bug et n'arrive pas a convertir string
yG += [yG[-1]]
if xG[-1] > xmaxG:
xG.pop(0)
yG.pop(0)
plt.plot(xG,yG,'m', axes=axplot,label='GSR')
#Resize le plot
if xG[-1] > xmaxG:
plt.axis([xG[0],xG[-1],yminG,ymaxG])
else:
plt.axis([xminG,xmaxG,yminG,ymaxG])
#Get the data
DataReader = ser.getData()
DataGS = DataReader[0]
DataACC = DataReader[1]
for g in range (len(DataGS)):
try :
#yG += [float(DataGS[g][1])]
queueY.append(float(DataGS[g][1]))
if len(queueY) > queueLen :
queueY.pop(0)
#yG += [lowpass.lowPass(queueY, 50, 50)]
y = movingAverage.movingAverage(queueY)
print y
socket.envoyerMsg(str(y))
#socket.envoyerMsg(str(0.1))
except: #si bug et n'arrive pas a convertir string
pass
#Accelerometer data??
"""
for a in range(len(DataACC)):
yA1 += [float(DataACC[a][1])]
yA2 += [float(DataACC[a][2])]
yA3 += [float(DataACC[a][3])]
xA += [float(DataACC[a][0])]
def detect(opt, save_img=False):
global bird_image
out, source, weights, view_img, save_txt, imgsz = \
opt.output, opt.source, opt.weights, opt.view_img, opt.save_txt, opt.img_size
webcam = source == '0' or source.startswith('rtsp') or source.startswith(
'http') or source.endswith('.txt')
# initialize the ROI frame
cv2.namedWindow("image")
cv2.setMouseCallback("image", get_mouse_points)
# Initialize
device = select_device(opt.device)
if os.path.exists(out):
shutil.rmtree(out) # delete output folder
os.makedirs(out) # make new output folder
half = device.type != 'cpu' # half precision only supported on CUDA
# Load model
model = torch.load(weights,
map_location=device)['model'].float() # load to FP32
model.to(device).eval()
if half:
model.half() # to FP16
# initialize deepsort
cfg = get_config()
cfg.merge_from_file(opt.config_deepsort)
deepsort = DeepSort(cfg.DEEPSORT.REID_CKPT,
max_dist=cfg.DEEPSORT.MAX_DIST,
min_confidence=cfg.DEEPSORT.MIN_CONFIDENCE,
nms_max_overlap=cfg.DEEPSORT.NMS_MAX_OVERLAP,
max_iou_distance=cfg.DEEPSORT.MAX_IOU_DISTANCE,
max_age=cfg.DEEPSORT.MAX_AGE,
n_init=cfg.DEEPSORT.N_INIT,
nn_budget=cfg.DEEPSORT.NN_BUDGET,
use_cuda=True)
# Set Dataloader
vid_path, vid_writer = None, None
if webcam:
view_img = True
cudnn.benchmark = True # set True to speed up constant image size inference
dataset = LoadStreams(source, img_size=imgsz)
else:
view_img = True
save_img = True
dataset = LoadImages(source, img_size=imgsz)
# Get names and colors
names = model.module.names if hasattr(model, 'module') else model.names
#initialize moving average window
movingAverageUpdater = movingAverage.movingAverage(5)
# Run inference
t0 = time.time()
img = torch.zeros((1, 3, imgsz, imgsz), device=device) # init img
_ = model(img.half() if half else img
) if device.type != 'cpu' else None # run once
save_path = str(Path(out))
txt_path = str(Path(out)) + '/results.txt'
d = DynamicUpdate()
d.on_launch()
risk_factors = []
frame_nums = []
count = 0
for frame_idx, (path, img, im0s, vid_cap) in enumerate(dataset):
if (frame_idx == 0):
while True:
image = im0s
cv2.imshow("image", image)
cv2.waitKey(1)
if len(mouse_pts) == 7:
cv2.destroyWindow("image")
break
four_points = mouse_pts
# Get perspective, M is the transformation matrix for bird's eye view
M, Minv = get_camera_perspective(image, four_points[0:4])
# Last two points in getMousePoints... this will be the threshold distance between points
threshold_pts = src = np.float32(np.array([four_points[4:]]))
# Convert distance to bird's eye view
warped_threshold_pts = cv2.perspectiveTransform(threshold_pts,
M)[0]
# Get distance in pixels
threshold_pixel_dist = np.sqrt(
(warped_threshold_pts[0][0] - warped_threshold_pts[1][0])**2 +
(warped_threshold_pts[0][1] - warped_threshold_pts[1][1])**2)
# Draw the ROI on the output images
ROI_pts = np.array([
four_points[0], four_points[1], four_points[3], four_points[2]
], np.int32)
# initialize birdeye view video writer
frame_h, frame_w, _ = image.shape
bevw = birdeye_video_writer.birdeye_video_writer(
frame_h, frame_w, M, threshold_pixel_dist)
else:
break
t = time.time()
for frame_idx, (path, img, im0s, vid_cap) in enumerate(dataset):
print("Loop time: ", time.time() - t)
t = time.time()
img = torch.from_numpy(img).to(device)
img = img.half() if half else img.float() # uint8 to fp16/32
img /= 255.0 # 0 - 255 to 0.0 - 1.0
if img.ndimension() == 3:
img = img.unsqueeze(0)
cv2.polylines(im0s, [ROI_pts], True, (0, 255, 255), thickness=4)
# Inferenc
tOther = time.time()
t1 = time_synchronized()
pred = model(img, augment=opt.augment)[0]
# Apply NMS
pred = non_max_suppression(pred,
opt.conf_thres,
opt.iou_thres,
classes=opt.classes,
agnostic=opt.agnostic_nms)
t2 = time_synchronized()
print("Non max suppression and inference: ", time.time() - tOther)
print("Pre detection time: ", time.time() - t)
# Process detections
for i, det in enumerate(pred): # detections per image
if webcam: # batch_size >= 1
p, s, im0 = path[i], '%g: ' % i, im0s[i].copy()
else:
p, s, im0 = path, '', im0s
s += '%gx%g ' % img.shape[2:] # print string
save_path = str(Path(out) / Path(p).name)
if det is not None and len(det):
# Rescale boxes from img_size to im0 size
det[:, :4] = scale_coords(img.shape[2:], det[:, :4],
im0.shape).round()
bbox_xywh = []
bbox_xyxy = []
confs = []
ROI_polygon = Polygon(ROI_pts)
# Adapt detections to deep sort input format
for *xyxy, conf, cls in det:
img_h, img_w, _ = im0.shape
x_c, y_c, bbox_w, bbox_h = bbox_rel(img_w, img_h, *xyxy)
obj = [x_c, y_c, bbox_w, bbox_h]
confs.append([conf.item()])
bbox_xyxy.append(xyxy)
bbox_xywh.append(obj)
xywhs = torch.Tensor(bbox_xywh)
confss = torch.Tensor(confs)
# Pass detections to deepsort
deepsortTime = time.time()
#outputs = deepsort.update(xywhs, confss, im0)
print("Deepsort function call: ", (time.time() - deepsortTime))
outputs = bbox_xyxy
# draw boxes for visualization
if len(outputs) > 0:
# filter deepsort output
outputs_in_ROI, ids_in_ROI = remove_points_outside_ROI(
bbox_xyxy, ROI_polygon)
center_coords_in_ROI = xywh_to_center_coords(
outputs_in_ROI)
warped_pts = birdeye_transformer.transform_center_coords_to_birdeye(
center_coords_in_ROI, M)
clusters = DBSCAN(eps=threshold_pixel_dist,
min_samples=1).fit(warped_pts)
print(clusters.labels_)
draw_boxes(im0, outputs_in_ROI, clusters.labels_)
risk_dict = Counter(clusters.labels_)
bird_image = bevw.create_birdeye_frame(
warped_pts, clusters.labels_, risk_dict)
# movingAverageUpdater.updatePoints(warped_pts, ids_in_ROI)
#
# gettingAvgTime = time.time()
# movingAveragePairs = movingAverageUpdater.getCurrentAverage()
#
# movingAverageIds = [id for id, x_coord, y_coord in movingAveragePairs]
# movingAveragePts = [(x_coord, y_coord) for id, x_coord, y_coord in movingAveragePairs]
# embded the bird image to the video
# otherStuff = time.time()
# if(len(movingAveragePairs) > 0):
# movingAvgClusters = DBSCAN(eps=threshold_pixel_dist, min_samples=1).fit(movingAveragePts)
# movingAvgClustersLables = movingAvgClusters.labels_
# risk_dict = Counter(movingAvgClustersLables)
# bird_image = bevw.create_birdeye_frame(movingAveragePts, movingAvgClustersLables, risk_dict)
# bird_image = resize(bird_image, 20)
# bv_height, bv_width, _ = bird_image.shape
# frame_x_center, frame_y_center = frame_w //2, frame_h//2
# x_offset = 20
#
# im0[ frame_y_center-bv_height//2:frame_y_center+bv_height//2, \
# x_offset:bv_width+x_offset ] = bird_image
# else:
# risk_dict = Counter(clusters.labels_)
# bird_image = bevw.create_birdeye_frame(warped_pts, clusters.labels_, risk_dict)
bird_image = resize(bird_image, 20)
bv_height, bv_width, _ = bird_image.shape
frame_x_center, frame_y_center = frame_w // 2, frame_h // 2
x_offset = 20
im0[frame_y_center - bv_height // 2:frame_y_center + bv_height // 2, \
x_offset:bv_width + x_offset] = bird_image
# print("Other stuff: ", time.time() - otherStuff)
#write the risk graph
risk_factors += [compute_frame_rf(risk_dict)]
frame_nums += [frame_idx]
graphTime = time.time()
if (frame_idx > 100):
count += 1
frame_nums.pop(0)
risk_factors.pop(0)
if frame_idx % 10 == 0:
d.on_running(frame_nums, risk_factors, count,
count + 100)
print("Graph Time: ", time.time() - graphTime)
# Write MOT compliant results to file
if save_txt and len(outputs_in_ROI) != 0:
for j, output in enumerate(outputs_in_ROI):
bbox_left = output[0]
bbox_top = output[1]
bbox_w = output[2]
bbox_h = output[3]
identity = output[-1]
with open(txt_path, 'a') as f:
f.write(('%g ' * 10 + '\n') %
(frame_idx, identity, bbox_left, bbox_top,
bbox_w, bbox_h, -1, -1, -1,
-1)) # label format
# Stream results
if view_img:
# cv2.imshow("bird_image", bird_image)
cv2.imshow(p, im0)
if cv2.waitKey(1) == ord('q'): # q to quit
raise StopIteration
# Save results (image with detections)
if save_img:
if dataset.mode == 'images':
cv2.imwrite(save_path, bird_image)
cv2.imwrite(save_path, im0)
else:
if vid_path != save_path: # new video
vid_path = save_path
if isinstance(vid_writer, cv2.VideoWriter):
vid_writer.release(
) # release previous video writer
fps = vid_cap.get(cv2.CAP_PROP_FPS)
w = int(vid_cap.get(cv2.CAP_PROP_FRAME_WIDTH))
h = int(vid_cap.get(cv2.CAP_PROP_FRAME_HEIGHT))
vid_writer = cv2.VideoWriter(
save_path, cv2.VideoWriter_fourcc(*opt.fourcc),
fps, (w, h))
vid_writer.write(im0)
if save_txt or save_img:
print('Results saved to %s' % os.getcwd() + os.sep + out)
if platform == 'darwin': # MacOS
os.system('open ' + save_path)
print('Done. (%.3fs)' % (time.time() - t0))
imu = "/imu/data"
gyros = "/imu/integrated_gyros_stamped"
cmd_vels = "/Arduino_RC"
bag = rosbag.Bag(bagFile)
front_encoder_msgs = [msg for topic,msg,t in bag.read_messages(topics=[front_encoders])]
back_encoder_msgs = [msg for topic,msg,t in bag.read_messages(topics=[back_encoders])]
front_cmd_msgs = [msg for topic,msg,t in bag.read_messages(topics=[front_cmds])]
back_cmd_msgs = [msg for topic,msg,t in bag.read_messages(topics=[back_cmds])]
imu_msgs = [msg for topic,msg,t in bag.read_messages(topics=[imu])]
imu_time = [t.header.stamp.secs+t.header.stamp.nsecs*10**-9 for t in imu_msgs]
gyros_msgs = [msg for topic,msg,t in bag.read_messages(topics=[gyros])]
gyros_time = [t.header.stamp.secs+t.header.stamp.nsecs*10**-9 for t in gyros_msgs]
#imu_avg_time = movingAverage.exponentialMovingAverage(imu_time,alpha=0.5)
imu_avg_time = imu_time
gyros_avg_time = movingAverage.movingAverage(gyros_time,1)
cmd_vel_msgs = [msg for topic,msg,t in bag.read_messages(topics=[cmd_vels])]
cmd_vels_time = numpy.linspace(imu_time[0],imu_time[-1],len(cmd_vel_msgs))
front_cmd_time = [t.header.stamp.secs+t.header.stamp.nsecs/10.0**9
for t in front_cmd_msgs]
back_cmd_time = [t.header.stamp.secs+t.header.stamp.nsecs/10.0**9
for t in back_cmd_msgs]
# front_cmd_time = numpy.linspace(imu_time[0],imu_time[-1],len(front_cmd_msgs))
# back_cmd_time = numpy.linspace(imu_time[0],imu_time[-1],len(back_cmd_msgs))
orig_cmd_z = [t.twist.angular.z for t in front_cmd_msgs]
imu_z = [t.angular_velocity.z for t in imu_msgs]
def plot(bagFile,
front_encoders,
back_encoders,
front_cmds,
back_cmds,
imu,
gps,
cmd_vels):
bag = rosbag.Bag(bagFile)
front_encoder_msgs= [msg for topic,msg,t in bag.read_messages(topics=[front_encoders])]
back_encoder_msgs = [msg for topic,msg,t in bag.read_messages(topics=[back_encoders])]
front_cmd_msgs = [msg for topic,msg,t in bag.read_messages(topics=[front_cmds])]
back_cmd_msgs = [msg for topic,msg,t in bag.read_messages(topics=[back_cmds])]
imu_msgs = [msg for topic,msg,t in bag.read_messages(topics=[imu])]
gps_msgs = [msg for topic,msg,t in bag.read_messages(topics=[gps])]
cmd_vel_msgs = [msg for topic,msg,t in bag.read_messages(topics=[cmd_vels])]
gps_x = [x.pose.pose.position.x for x in gps_msgs]
gps_y = [y.pose.pose.position.y for y in gps_msgs]
time = [t.header.stamp.secs+t.header.stamp.nsecs/10.0**9 for t in gps_msgs]
gpsTimes = time[:-1]
# gps_x = movingAverage.exponentialMovingAverage(\
# [x.pose.pose.position.x for x in gps_msgs],alpha=0.5)
# gps_y = movingAverage.exponentialMovingAverage(\
# [y.pose.pose.position.y for y in gps_msgs],alpha=0.5)
# time = movingAverage.exponentialMovingAverage(\
# [t.header.stamp.secs+t.header.stamp.nsecs/10.0**9 for t in gps_msgs],alpha=0.5)
# gpsTimes = movingAverage.movingAverage(\
# [t.header.stamp.secs+t.header.stamp.nsecs/10.0**9
# for t in gps_msgs],1)
# gpsVels = movingAverage.exponentialMovingAverage(\
# velocity.getLinearGPSVelocities(gps_x,gps_y,time),alpha=0.5)
gpsVels = velocity.getLinearGPSVelocities(gps_x,gps_y,time)
imu_time = [t.header.stamp.secs+t.header.stamp.nsecs*10**-9 for t in imu_msgs]
imu_z = [t.angular_velocity.z for t in imu_msgs]
print "IMU velocity mean",numpy.mean(imu_z)
roboteq_cmd_z = [t.twist.angular.z for t in front_cmd_msgs]
front_time = movingAverage.movingAverage(\
[t.header.stamp.secs+t.header.stamp.nsecs/10.0**9
for t in front_encoder_msgs],1)
back_time = movingAverage.movingAverage(\
[t.header.stamp.secs+t.header.stamp.nsecs/10.0**9
for t in back_encoder_msgs],1)
frontVels,backVels = velocity.getLinearEncoderVelocities(\
front_encoder_msgs,back_encoder_msgs)
cmd_vels_time = numpy.linspace(imu_time[0],imu_time[-1],len(cmd_vel_msgs))
cmd_vels_time_ = cmd_vels_time
# frontVels = movingAverage.exponentialMovingAverage(
# frontVels,alpha=0.5)
# backVels = movingAverage.exponentialMovingAverage(
# backVels,alpha=0.5)
front_cmd_time = [t.header.stamp.secs+t.header.stamp.nsecs/10.0**9
for t in front_cmd_msgs]
front_cmd_vels = [t.twist.linear.x for t in front_cmd_msgs]
# frontVels = [-x for x in frontVels]
# backVels = [-x for x in backVels]
frontVels_ = frontVels
backVels_ = backVels
#front_cmd_vels = [-x for x in front_cmd_vels]
cmd_vels_z = [t.angular.z for t in cmd_vel_msgs]
cmd_vels_z_ = cmd_vels_z
#Preliminary analysis
f1 = figure(1)
plot(gps_x,gps_y,'ob')
xlabel("x")
ylabel("y")
title("Experiment positions")
f1.show()
f2 = figure(2)
p1,=plot(front_time,frontVels,'-g')
p2,=plot(back_time,backVels,'-b')
p3,=plot(gpsTimes,gpsVels,'-k')
p4,=plot(front_cmd_time,front_cmd_vels,':r')
xlabel("Time")
ylabel("m/s")
title("Experiment velocities")
legend([p1,p2,p3,p4],["front encoder velocity",
"back encoder velocity",
"gps velocity",
"cmd velocity"])
f2.show()
#Linear velocity error analysis
threshold = 0.05
gpsFrontVels,frontVels = sync.matchInput(gpsTimes,gpsVels,front_time,frontVels,threshold)
gpsBackVels,backVels = sync.matchInput(gpsTimes,gpsVels,back_time,backVels,threshold)
frontError = [gpsFrontVels[i]-frontVels[i] for i in xrange(0,len(frontVels))]
backError = [gpsBackVels[i]-backVels[i] for i in xrange(0,len(backVels))]
print "Encoder mean",numpy.mean(frontError)
print "Encoder std dev",numpy.std(frontError)
f3 = figure(3)
plot(frontVels,gpsFrontVels,'ob')
plot(backVels,gpsBackVels,'ob')
xlabel("Encoder Velocities (m/s)")
ylabel("GPS Velocities (m/s)")
title("Experiment gps encoder velocities")
f3.show()
f4 = figure(4)
p1,=plot(frontVels,frontError,'.')
p2,=plot(backVels,backError,'.')
p3,=plot(gpsFrontVels,frontError,'.')
p4,=plot(gpsBackVels,backError,'.')
xlabel("Velocity (m/s)")
ylabel("Error (m/s)")
title("Experiment gps encoder error")
legend([p1,p2,p3,p4],["front encoder velocity",
"back encoder velocity",
"front gps velocity",
"back gps velocity"])
f4.show()
threshold = 0.01
frontVels,cmdVels = sync.matchInput(front_time,frontVels_,front_cmd_time,front_cmd_vels,threshold)
f5 = figure(5)
plot(cmdVels,frontVels,'ob')
xlabel("Command Velocities (m/s)")
ylabel("Encoder Velocities (m/s)")
title("Experiment command encoder velocities")
f5.show()
f6 = figure(6)
p1,=plot(imu_time,imu_z,'-r')
p2,=plot(front_cmd_time,roboteq_cmd_z,'-b')
p3,=plot(cmd_vels_time,cmd_vels_z,'-g')
xlabel("Time")
ylabel("rad/s")
title("Angular velocity experiment")
legend([p1,p2,p3],["imu","roboteq cmd vel","arduino cmd vel"])
f6.show()
#print len(cmd_vels_time),len(imu_time)
#Error analysis
cmdVels,ImuVels = sync.matchInput(cmd_vels_time,cmd_vels_z,imu_time,imu_z,threshold)
f7 = figure(7)
plot(cmdVels,ImuVels,'ob')
xlabel("Command Velocities (rad/s)")
ylabel("IMU Velocities (rad/s)")
title("Experiment cmd imu velocities")
xlim([-.75,.75])
ylim([-.75,.75])
f7.show()
back_encoder_msgs = [msg for topic,msg,t in bag.read_messages(topics=[back_encoders])]
front_cmd_msgs = [msg for topic,msg,t in bag.read_messages(topics=[front_cmds])]
back_cmd_msgs = [msg for topic,msg,t in bag.read_messages(topics=[back_cmds])]
imu_msgs = [msg for topic,msg,t in bag.read_messages(topics=[imu])]
gps_msgs = [msg for topic,msg,t in bag.read_messages(topics=[gps])]
ekf_msgs = [msg for topic,msg,t in bag.read_messages(topics=[ekf])]
gps_x = [x.pose.pose.position.x for x in gps_msgs]
gps_y = [y.pose.pose.position.y for y in gps_msgs]
time = [t.header.stamp.secs+t.header.stamp.nsecs/10.0**9 for t in gps_msgs]
ekf_x = [x.x for x in ekf_msgs]
ekf_y = [y.y for y in ekf_msgs]
gpsTime = movingAverage.movingAverage(\
[t.header.stamp.secs+t.header.stamp.nsecs/10.0**9
for t in gps_msgs],1)
gpsVels = velocity.getLinearGPSVelocities(gps_x,gps_y,time)
front_time = movingAverage.movingAverage(\
[t.header.stamp.secs+t.header.stamp.nsecs/10.0**9
for t in front_encoder_msgs],1)
back_time = movingAverage.movingAverage(\
[t.header.stamp.secs+t.header.stamp.nsecs/10.0**9
for t in back_encoder_msgs],1)
plot(gps_x,gps_y,'-ob')
plot(ekf_x,ekf_y,'-or')
xlim([-16,2])
ylim([-10,0])
