def km_control(self, action_weight=np.ones(4), action_attacker=np.zeros(4)):
"""
将输出量加上km
:param action_weight:
:param action_attacker:
:return:
"""
# 权重归一化
action_weight = action_weight / sum(action_weight)
# print(action_weight, action_attacker)
# 传感器随机误差
SSerror = np.random.randn(4) * self.sensor_error
# 更新前车原始数据
self.v_cal_raw = self.v_head * np.ones(4) + action_weight * (SSerror + action_attacker)
km1 = kalman_filter(0.001, 1)
km2 = kalman_filter(0.001, 1)
km3 = kalman_filter(0.001, 1)
km4 = kalman_filter(0.001, 1)
self.v_cal_raw_km[0] = km1.kalman(self.v_cal_raw[0])
self.v_cal_raw_km[1] = km2.kalman(self.v_cal_raw[1])
self.v_cal_raw_km[2] = km3.kalman(self.v_cal_raw[2])
self.v_cal_raw_km[3] = km4.kalman(self.v_cal_raw[3])
# print(self.v_cal_raw)
# 前车的估计车速 公式7
self.v_cal = np.sum(self.v_cal_raw_km)/4
# 控制结果 公式1
self.action_car = self.lam * (self.v_cal - self.v)
def save_plot(job_file_name='job.json',
data_file_name='data1.json',
title='1',
navigator_titles=[]):
with open(job_file_name) as data_file:
cd = json.load(data_file) #constants dictionary
with open(data_file_name) as data_file2:
dict_list = json.load(data_file2) #dictionary tuple
for i in range(len(dict_list) - 1):
diff_dict = dict_list[i + 38]
kalman_dict = kalman_filter(diff_dict)
navigator_title = str(navigator_titles[i]) + title
fig, ax = plt.subplots()
plot(kalman_dict, navigator_title, ax=ax)
def save_detection_plot(job_file_name='job.json',
data_file_name='data1.json',
navigator_titles=('2', '3', 'carde1', 'carde2',
'carde2')):
with open(job_file_name) as data_file:
cd = json.load(data_file) #constants dictionary
with open(data_file_name) as data_file2:
dict_list = json.load(data_file2) #dictionary tuple
for i in range(len(dict_list)):
diff_dict = dict_list[i]
kalman_dict = kalman_filter(diff_dict)
#navigator = navigator_titles[i]
################################something should be done about these titles
title = '; amplitude = ' + str(cd['amplitude'])
detection_plot(kalman_dict, title)
def processButtonClick(self):
stock_ticker = stockVar.get()
start_mm = startMonthVar.get()
start_dd = startDateVar.get()
start_yy = startYearVar.get()
end_mm = endMonthVar.get()
end_dd = endDayVar.get()
end_yy = endYearVar.get()
if stock_ticker == 'STOCK' or start_mm == 'START MM' or start_dd == 'START DD' or start_yy == 'START YYYY' or end_mm == 'END MM' or end_dd == 'END DD' or end_yy == 'END YYYY':
tkMessageBox.showinfo(
"ERROR", "You need to choose a value for each option")
start = datetime.datetime(int(start_yy), int(start_mm), int(start_dd))
end = datetime.datetime(int(end_yy), int(end_mm), int(end_dd))
stock = data.DataReader(stock_ticker, 'google', start, end)
close_price = stock.get('Close')
kalman_filtered = kalman.kalman_filter(stock)
self.draw(
np.array(close_price.axes)[0], np.array(close_price),
kalman_filtered, stock_ticker)
def calculate_differences(num_trials, x_init, y_init, a_x, a_y, dt):
""" Simply subtracts values calculated from a kalman filter and theoretical values. This tells us how impactful the kalman filter was at a certain point in time. The
difference is calculated in the x as well as the y dimension. """
filtered, _ = kalman.kalman_filter(num_trials, x_init, y_init, a_x,
a_y) # returns x, y, and v_x, v_y
true_vals = generate_data.generate_true_values(num_trials,
dt,
x_init,
y_init,
a_x,
a_y,
x_y_only=True)
differences = np.zeros((2, num_trials))
for i in range(0, num_trials):
diff_x = true_vals[0, i] - filtered[0, i]
differences[0, i] = diff_x
diff_y = true_vals[1, i] - filtered[1, i]
differences[1, i] = diff_y
return differences
Dd = Dc
[nx, nu] = Bd.shape # number of states and number or inputs
ny = np.shape(Cc)[0]
# Kalman filter extended matrices
Bd_kal = np.hstack([Bd, np.eye(nx)])
Dd_kal = np.hstack([Dd, np.zeros((ny, nx))])
Q_kal = np.diag([10,10,10,10])#np.eye(nx) * 100
R_kal = np.eye(ny) * 1
#Bd_kal = np.hstack([Bd, Bd])
#Dd_kal = np.hstack([Dd, Dd])
#Q_kal = np.eye(nu) * 1
#R_kal = np.eye(ny) * 1
L,P,W = kalman_filter(Ad, Bd_kal, Cd, Dd_kal, Q_kal, R_kal)
# Simulate in closed loop
[nx, nu] = Bd.shape # number of states and number or inputs
len_sim = 100 # simulation length (s)
nsim = int(len_sim/Ts) # simulation length(timesteps)
x_vec = np.zeros((nsim, nx))
y_vec = np.zeros((nsim, ny))
x_est_vec = np.zeros((nsim, nx))
u_vec = np.zeros((nsim, nu))
t_vec = np.arange(0, nsim) * Ts
time_start = time.time()
x_step = x0
def grab_and_cal(info):
global ref_nodes
global pos, k_pos, acc_pos
global ax
global count, iter_time
dist = {}
rough_split = info.split('[')
if len(rough_split) <= 2:
return -1
dis_list = rough_split[1].split(']')[0].split(',')
if len(dis_list) < 4:
return -1
id_list = rough_split[2].split(']')[0].split(',')
if len(id_list) < 4:
return -1
if (len(dis_list) != len(id_list)) | (len(dis_list) < 4):
return -1
base = ""
for i in range(0, len(dis_list)):
id_list[i] = id_list[i].strip('"')
if id_list[i] in ref_nodes:
dist[id_list[i]] = (float)(dis_list[i].strip('"'))
if base == "":
base = id_list[i]
if len(dist) < 4:
return -1
# print(dist)
A = np.array([0, 0, 0])
B = np.array([0])
for i in dist:
if i == base:
continue
A = np.vstack((A, ref_nodes[i] - ref_nodes[base]))
B = np.vstack((
B, dist[i]**2 - dist[base]**2 -
np.dot(ref_nodes[i]**2 - ref_nodes[base]**2, np.array([1, 1, 1]))))
A = A[1:len(dist)]
B = B[1:len(dist)] * (-0.5)
AT = np.transpose(A)
B = np.dot(AT, B)
rev = np.linalg.inv(np.dot(AT, A))
pos = np.dot(rev, B)
it_count = 0
while it_count < iter_time:
pos = nt_iter(pos)
k_pos = kalman.kalman_filter(pos)
acc_pos += pos
count += 1
# print(acc_pos[2])
return 1
#Kalman filtering the returned data
if not any(init_settings_kalman) or options.config != "":
cfg=ConfigParser.ConfigParser()
invalid=False
cfg.read(options.config)
init_settings_kalman=ConfigSectionMap(cfg,"sectionOne")
#init_settings_kalman["noise_var_x"]=init_settings_kalman["noise_var_x"]/(sampling_rate*interpolation)
elif not any(init_settings_kalman) and options.config == "":
sys.exit("no valid configuration found")
if options.algorithm == "chan" or options.algorithm == "both":
init_settings_kalman['algorithm']='chan'
kalman=kalman_filter(init_settings_kalman)
estimated_positions["chan"]=chan94_algorithm.localize(receivers_steps[0],ref_receiver,np.round(basemap(bbox[2],bbox[3])))
measurement=estimated_positions["chan"]["coordinates"]#+np.array([-20,-20])
xk_1 = np.hstack((np.array(list(estimated_positions["chan"]["coordinates"])),np.zeros(kalman.get_state_size()-2))) #init state
kalman_states = xk_1
Pk_1=kalman.get_init_cov()
for i in range(len(receivers_steps)):
start_time=time.time()
signal_strength=[]
if options.reference_selection in ["Min-signal-power","Max-signal-power"]:
for idx in range(len(receivers_steps[i])):
signal_strength.append(np.sum(np.square(np.abs(receiver.samples))))
#pdb.set_trace()
if options.reference_selection == "Min-signal-power":
ref_receiver = np.argmin(signal_strength)
old_list = dic["diff_list{0}".format(0)]
new_list = []
new_dic = {}
for tup in old_list:
new_tup = (tup[0], tup[1] + disp_size, tup[2], tup[3], tup[4])
new_list.append(new_tup)
new_dic["diff_list{0}".format(0)] = new_list
return new_dic
#if __name__ == "__main__":
for i in range(1):
with open('data0.json') as data_file2:
dict_list = json.load(data_file2)
new_dict = {}
print len(dict_list)
kalman_dict0 = disp(dict_list[137], 200)
kalman_dict1 = disp(dict_list[369], 100)
kalman_dict2 = disp(dict_list[495], 100)
kalman_dict3 = dict_list[789]
new_dict["diff_list0"] = kalman_dict0["diff_list{0}".format(0)]
new_dict["diff_list1"] = kalman_dict1["diff_list{0}".format(0)]
new_dict["diff_list2"] = kalman_dict2["diff_list{0}".format(0)]
new_dict["diff_list3"] = kalman_dict3["diff_list{0}".format(0)]
kalman_dict = kalman_filter(new_dict)
title1 = 'Demonstration of Large Final Sweeps Casting' + str(i)
title2 = 'Demonstration of different navigation strategies'
plot(kalman_dict, title2)
def debug(opt):
os.environ['CUDA_VISIBLE_DEVICES'] = opt.gpus_str
Detector = detector_factory[opt.task]
detector = Detector(opt)
path_dataset = "/home/guillermo/Escritorio/analisis_puntos"
path_save = os.path.join(path_dataset, 'debug_images')
if not os.path.exists(path_save):
os.makedirs(path_save)
image_names = dm.carga_imagenes(os.path.join(path_dataset, 'rgb'))
image_names_d = dm.carga_imagenes(os.path.join(path_dataset, 'd'))
image_names_mask = dm.carga_imagenes(os.path.join(path_dataset, 'mask'))
pred = []
target = []
img_nm = []
img_idx = []
media = []
q1 = []
q3 = []
puntos = []
puntos_silla = []
modes = []
medians = []
i = 0
percentage_print = 0
k_filter = kalman.kalman_filter()
for image_name in image_names:
#img_rgb = cv2.imread(image_name,-1)
#img_rgb_cal = calibrate_images(img_rgb.copy(), True, opt)
ret = detector.run(image_name)
im = image_name
imd = image_names_d[i]
imm = image_names_mask[i]
i = i + 1
percentage = int(i * 100 / len(image_names))
#para sacar por pantalla el progreso
if percentage >= percentage_print:
string = "["
for x in range(int(100 / 2.5)):
if x <= int(percentage / 2.5):
string = string + "|"
else:
string = string + "-"
string = string + "] "
print(string + str(percentage) + '%')
percentage_print = percentage_print + 2.5
p, t, m, q, qq, punt, mod, med, punt_silla = eval_total(
ret, im, imd, imm, i, opt, path_save, k_filter)
for pp in p:
pred.append(pp)
for tt in t:
target.append(tt)
img_nm.append(im)
img_idx.append(i)
for mm in m:
media.append(mm)
for q11 in q:
q1.append(q11)
for q33 in qq:
q3.append(q33)
for punto in punt:
puntos.append(punto)
for mo in mod:
modes.append(mo)
for mmm in med:
medians.append(mmm)
for p_silla in punt_silla:
puntos_silla.append(p_silla)
print("Analisis completado...")
data = {
'Prediction': pred,
'Target': target,
'Mediana': medians,
'Moda': modes,
'Media': media,
'Q1': q1,
'Q3': q3,
'Img_index': img_idx,
'Img_name': img_nm
}
df = pd.DataFrame(data)
df.to_excel(os.path.join(path_dataset, 'debug_data.xlsx'))
writer = pd.ExcelWriter(os.path.join(path_dataset, 'point_data.xlsx'),
engine='xlsxwriter')
cont = 0
for info in puntos:
info = info * 0.001
real = puntos_silla[cont] * 0.001
data_points = {img_nm[cont]: info}
plt.hist(info, bins=100)
plt.hist(real, bins=100)
plt.axvline(x=target[cont], color='brown')
plt.axvline(x=media[cont], color='red')
plt.axvline(x=medians[cont], color='yellow')
plt.axvline(x=modes[cont], color='green')
plt.axvline(x=(modes[cont] + medians[cont]) / 2, color='fuchsia')
plt.savefig(os.path.join(path_save, str(cont + 1) + '_hist.png'))
plt.close()
df = pd.DataFrame(data_points)
df.to_excel(writer, sheet_name=str(cont))
cont = cont + 1
writer.save()
print("Archivo creado")
from plot import plot_trace_points, plot_trace_path
from preprocess import smooth_spline, smooth_regress
from kalman import kalman_filter
import pandas as pd
from ggplot import ggplot, aes, geom_point, geom_line
if __name__ == '__main__':
df = pd.DataFrame(
data={
't': [1, 2, 3, 4, 5],
'lat': [10, 12, 10, 9, 8],
'lon': [100, 99, 98, 95, 97]
})
df2 = smooth_spline(df, 0.1)
df3 = smooth_regress(df, 0.1, 4)
#df4 = smooth_regress(df, 0.1, 3)
df4 = kalman_filter(df, 0.1)
p = ggplot(aes(x='lat', y='lon'),
data=pd.DataFrame(columns=('lat', 'lon'), data={}))
p += plot_trace_points(df, color='black')
p += plot_trace_path(df2, color='red')
p += plot_trace_path(df3, color='green')
p += plot_trace_path(df4, color='blue')
p.save('test.png')
from plot import plot_trace_points, plot_trace_path
from preprocess import smooth_spline, smooth_regress
from kalman import kalman_filter
import pandas as pd
from ggplot import ggplot, aes, geom_point, geom_line
if __name__ == '__main__':
df = pd.DataFrame(data = { 't': [1, 2, 3, 4, 5], 'lat': [10, 12, 10, 9, 8], 'lon': [100, 99, 98, 95, 97] })
df2 = smooth_spline(df, 0.1)
df3 = smooth_regress(df, 0.1, 4)
#df4 = smooth_regress(df, 0.1, 3)
df4 = kalman_filter(df, 0.1)
p = ggplot(aes(x='lat', y='lon'), data=pd.DataFrame(columns=('lat', 'lon'), data={}))
p += plot_trace_points(df, color='black')
p += plot_trace_path(df2, color='red')
p += plot_trace_path(df3, color='green')
p += plot_trace_path(df4, color='blue')
p.save('test.png')
def run(modelPath,
nnFramesize=(64, 48),
save=False,
folder='webcam',
showHeatmap=False,
liveFeed=True,
displayScale=1,
USE_KALMAN=True,
filestream=None,
largeDisplay=False,
heatmapThresh=0.75,
runAlgorithm=True):
# Import the trained neural network
print("Loading saved neural network from " + modelPath + '.pb')
tensorflowNet = cv2.dnn.readNetFromTensorflow(modelPath + '.pb')
print("Neural network sucessfully loaded")
# Prepare the output folder
if save:
if os.path.exists(folder): # only rm if it exists
shutil.rmtree(folder, ignore_errors=True)
if not os.path.exists(folder):
os.mkdir(folder)
# Initialize COM and size history vectors
history_rawCOM = np.array([[], []])
history_kalmanCOM = np.array([[], []])
history_area = []
history_kalmanArea = []
# Initialize the video stream from the camera
if filestream == None:
webcam = wcam.videoStream()
print("Video stream started")
print("Press P key to pause/play")
print("Press ESC key to quit")
else:
print("Reading image sequence from " + filestream)
frameset = vu.pull_sequence(filestream)
print("Image sequence shape:")
print(frameset.shape)
# Continuously pull and display frames from the camera until stopped
i = 0
start_time = datetime.now()
prev_time = start_time
while True:
if filestream == None:
# Pull from camera
frame = webcam.grabFrame() # grab a frame
else:
# Break off and exit if past end of sequence
if i >= frameset.shape[0]:
break
# Pull from file
frame = np.repeat(frameset[i, :, :, :], 3, axis=2)
if i == 0:
print("Raw frame shape: " + str(frame.shape))
curr_time = datetime.now()
# If statement around all the processing. Always true - only set to false
# for latency comparison tests.
if runAlgorithm:
# Massage frame to be the right size and colorset#
nnFrame = cv2.resize(frame, nnFramesize)
nnFrame = nnFrame[:, :, 0] * float(1.0 / 255.0)
nnFrame = np.squeeze(nnFrame)
# Initialize Kalman filter as motionless at center of image if this
# is the first frame. Uncertainty is the size of the image.
if i == 0:
kalmanFilter = kalman.kalman_filter(
nnFrame.shape[0] / 2,
nnFrame.shape[1] / 2, # x,y
0,
0, # vx, vy
nnFrame.shape[0] / 2,
nnFrame.shape[1] / 2, # sigX, sigY
0,
0) # sigVX, sigVY
kalmanFilterArea = kalman1D.kalman_filter(
600, 0.0, 1000, 0.0001) # x,vx, sigX, sigVX
# Initialize useful arrays for later
allZeros = np.zeros_like(nnFrame)
all255 = np.ones_like(nnFrame) * 255
# Execute a forward pass of the neural network on the frame to get a
# heatmap of target likelihood
# This is now by far the limiting temporal factor - without it the
# framerate is in the 300Hz range, with it framerate is in the 50Hz range.
tensorflowNet.setInput(nnFrame)
heatmap = tensorflowNet.forward()
heatmap = np.squeeze(heatmap)
# Optionally resize everything to be larger for display. Has the
# drawback of increased latency
scale = 1
if largeDisplay:
scale = 4
bigShape = (int(heatmap.shape[1] * scale),
int(heatmap.shape[0] * scale))
heatmap = cv2.resize(heatmap, bigShape)
nnFrame = cv2.resize(frame[:, :, 0], bigShape) * float(
1.0 / 255.0)
#print("Heatmap big shape (%d,%d)" % heatmap.shape[:2])
#print("Frame big shape (%d,%d)" % nnFrame.shape[:2])
if i == 0:
# Initialize useful arrays for later
allZeros = np.zeros_like(nnFrame)
all255 = np.ones_like(nnFrame) * 255
# Overlay heatmap contours onto the image (speed negligible)
#print(np.min(heatmap), np.max(heatmap))
overlaidNN = vu.overlay_heatmap(heatmap,
nnFrame,
heatThreshold=0.75,
scale=scale)
heatmap = heatmap * 255.0 # scale appropriately
# Calculate the estimated size of the quadcopter as the area of the
# thresholded region
area = np.sum(heatmap > 255.0 * heatmapThresh)
history_area.append(area)
# Find the center of mass for this frame
heatmapCOM = vu.find_centerOfMass(heatmap,
minThresh=heatmapThresh * 255)
targetVisible = True
if heatmapCOM == [None, None]:
targetVisible = False
print("Target not found")
heatmapCOM = [heatmap.shape[0] / 2, heatmap.shape[1] / 2
] # default to center of image
print("Frame %04d" % i)
print(" Pre-kalman: %02d, %02d" %
(heatmapCOM[0], heatmapCOM[1]))
print(" Pre-kalman area: %g" % area)
history_rawCOM = np.append(history_rawCOM,
np.expand_dims(heatmapCOM, 1),
axis=1)
# Apply Kalman filter to the COM centroid measurement if desired
if USE_KALMAN:
kalmanFilter.project((curr_time - prev_time).total_seconds())
kalmanFilterArea.project(
(curr_time - prev_time).total_seconds())
# Only update the kalman filter when we actually detect the target
if targetVisible:
# Massage the heatmap and calculate average per-pixel energy
heatmapClip = np.maximum(np.minimum(heatmap, all255),
allZeros) # range is 0 to 255
heatmapClip = heatmapClip.astype(np.float32)
heatmapClip[heatmapClip < 255.0 * heatmapThresh] = 0.0
heatmapMeanEnergy = np.mean(
heatmapClip) / 255.0 # range is 0 to 1
print(" Heatmap mean energy: %g" % heatmapMeanEnergy)
# Calculate the measurement error as the inverse of total heatmap
# energy becuase more energy = better localization accuracy
# 0.08 here is an empirically determined factor to get the
# uncertanties in the expected range. 0.01 just prevents infinities.
sigX = 0.08 * (1.0 / scale) * 0.5 * nnFrame.shape[0] / (
heatmapMeanEnergy + 0.01)
sigY = 0.08 * (1.0 / scale) * 0.5 * nnFrame.shape[1] / (
heatmapMeanEnergy + 0.01)
print(" (sigX, sigY) = (%g,%g)" % (sigX, sigY))
# Update the kalman filter with the measured COM and measurement error
kalmanFilter.update(heatmapCOM, [[sigX, 0], [0, sigY]])
# Update the kalman filter with the measured area
sigArea = 40.0 / (heatmapMeanEnergy + 0.0001)
kalmanFilterArea.update(area, sigArea)
print(" sigArea = %g" % sigArea)
# Pull the COM from the kalman vector state
heatmapCOM = kalmanFilter.stateVector[:2]
areaFrac = kalmanFilterArea.stateVector[0]
areaRate = kalmanFilterArea.stateVector[1]
# endif USE_KALMAN
# Overlay the target location
overlaidNN = vu.overlay_point(overlaidNN, heatmapCOM, color='g')
print(" Post-kalman: %02d, %02d" %
(heatmapCOM[0], heatmapCOM[1]))
print(" Post-kalman area: %f" % areaFrac)
print(" Post-kalman area rate: %f" % areaRate)
print('')
history_kalmanCOM = np.append(history_kalmanCOM,
np.expand_dims(heatmapCOM, 1),
axis=1)
history_kalmanArea.append(areaFrac)
if showHeatmap:
# Display the heatmap and the image side-by-side
heatmap = np.minimum(heatmap, np.ones(heatmap.shape) * 255)
heatmap = np.maximum(heatmap, np.zeros(heatmap.shape))
heatmap = heatmap.astype(
np.uint8) # map to uint8 to match nnFrame
heatmapColor = np.repeat(heatmap[:, :, np.newaxis], 3, axis=2)
displayThis = np.concatenate([overlaidNN, heatmapColor],
axis=0)
else:
# Just display image with outlines and COM
displayThis = overlaidNN
# end if runAlgorithm
# keypress control
key = cv2.waitKey(1) & 0xFF
# Display video feed live if desired
if liveFeed:
if key == ord('p'): # pause
while True:
key2 = cv2.waitKey(1) or 0xff
if runAlgorithm:
cv2.imshow('frame',
displayThis) # show last grabbed frame
if key2 == ord('p'): # resume
break
# end if paused
# Enlarge display for ease of viewing if desired
if displayScale != 1:
pairFactor = 1
if showHeatmap:
pairFactor = 2
pair = cv2.resize(displayThis,
(nnFramesize[0] * displayScale,
nnFramesize[1] * displayScale * pairFactor))
# end if displayScale != 1
# Show the current frame with neural net mask
if runAlgorithm:
cv2.imshow('frame', displayThis)
# end if liveFeed
if runAlgorithm:
# Save each frame if desired
if save:
# Save raw frame
filestr = "frameRaw_%04d.jpg" % i
fullpath = os.path.join(folder, filestr)
cv2.imwrite(fullpath, nnFrame * 255.0)
# Save displayed frame (with heatmap overlay)
filestr = "frameDisplay_%04d.jpg" % i
fullpath = os.path.join(folder, filestr)
cv2.imwrite(fullpath, displayThis)
# end if runAlgorithm
# Always show first frame so that exit key works
if i == 1:
cv2.imshow('frame', frame)
# End on Esc keypress
if key == 27:
break
# Increment frame loop counter
i += 1
# Save off time for use by kalman filter
prev_time = curr_time
# end while True
# Calculate framerate statistics
end_time = datetime.now()
timeElapsed = (end_time - start_time).total_seconds()
print("%d frames captured in %g seconds: framerate = %g Hz" %
(i, timeElapsed, i / timeElapsed))
if runAlgorithm:
# Print directory frames are saved to if they are being saved
if save:
print("Wrote image pairs to " + folder + ' with shape ' +
str(displayThis.shape))
# Make a final display of the snail trail of the COM
xCoords = history_rawCOM[1, :]
yCoords = heatmap.shape[0] - history_rawCOM[
0, :] # invert y axis for consistency
plt.plot(xCoords, yCoords, 'k+', markersize=4, label="raw")
xCoordsKalman = history_kalmanCOM[1, :]
yCoordsKalman = heatmap.shape[0] - history_kalmanCOM[
0, :] # invert y axis for consistency
#plt.plot(xCoordsKalman[0::10], yCoordsKalman[0::10], 'go', label="Kalman filtered") # plot every 10 points
plt.plot(xCoordsKalman,
yCoordsKalman,
'go',
markersize=3,
label="Kalman filtered")
plt.axis('equal')
plt.xlim([0, nnFrame.shape[1]])
plt.ylim([0, nnFrame.shape[0]])
plt.xlabel('Horizontal pixels')
plt.ylabel('Vertical pixels')
plt.title('Quadrotor centroid history')
plt.legend()
plt.savefig("snailTrail.png")
print("Wrote snail trail to snailTrail.png")
plt.show()
#print(history_rawCOM.shape)
# Make a final display of the quadcopter area through time
totalArea = heatmap.shape[0] * heatmap.shape[1]
plt.plot(np.arange(len(history_area)),
np.array(history_area) / totalArea,
'k+',
markersize=4,
label="raw")
plt.plot(np.arange(len(history_area)),
np.array(history_kalmanArea) / totalArea,
'g',
markersize=3,
label="Kalman filtered")
plt.xlim([0, len(history_area)])
plt.ylim([0.1, 0.4])
plt.xlabel('Frame number')
plt.ylabel('Fractional Area')
plt.title('Measured Quadcopter size vs time')
plt.legend()
plt.savefig("areaVsTime.png")
print("Wrote area vs time plot to areaVsTime.png")
plt.show()
# end if runAlgorithm
# Cleanup
print("Cleaning up")
if filestream == None:
webcam.stop()
cv2.destroyAllWindows()
print("Done")
# -*- coding: utf-8 -*-
import cv2
import numpy as np
import time
import math
from kalman import kalman_filter
#Global variables to store measurements
kalman_obj = []
kalman_future = kalman_filter()
contour_pointx = []
contour_pointy = []
measured_pointx = []
measured_pointy = []
position_x = 0
position_y = 0
#Run in cmd window as "python xyz.py"
if __name__ == '__main__':
#Getting video parameters/morphology options #####################################
print("Enter video name: ")
vid_name = str(input())
velocity_estimates.append(np.sum(np.square(v)))
f.close()
if options.redo_kalman:
#Kalman filtering the returned data
cfg=ConfigParser.ConfigParser()
cfg.read(options.config)
init_kalman = ConfigSectionMap(cfg,"sectionOne")
if chan_x :
init_kalman['algorithm']='chan'
kalman = kalman_filter(init_kalman)
measurements = np.column_stack((chan_x,chan_y))
xk_1 = np.array(np.hstack((measurements[0,:],np.zeros(kalman.get_state_size()-2))))#init state
kalman_states = xk_1
Pk_1 = kalman.get_init_cov()
for i in range(len(measurements)):
xk_1,Pk_1 = kalman.kalman_fltr(measurements[i-1,:],Pk_1,xk_1,"chan")
if i > 0:
kalman_states = np.vstack((kalman_states,xk_1))
chan_x_kalman.append(xk_1[0] )
chan_y_kalman.append(xk_1[1] )
if grid_x:
init_kalman['algorithm']='grid_based'
def debug(opt):
os.environ['CUDA_VISIBLE_DEVICES'] = opt.gpus_str
Detector = detector_factory[opt.task]
detector = Detector(opt)
if opt.dataset_name == "external":
path_dataset = "/media/guillermo/60F9-DB6E/external"
else:
path_dataset = os.path.join(os.path.dirname(os.getcwd()), 'data')
path_dataset = os.path.join(path_dataset, opt.dataset_name)
path_test = os.path.join(path_dataset, 'images_test')
path_save = os.path.join(path_dataset, 'debug_images')
if not os.path.exists(path_save):
os.makedirs(path_save)
image_names = dm.carga_imagenes(os.path.join(path_test, 'rgb'))
image_names_d = dm.carga_imagenes(os.path.join(path_test, 'd'))
pred = []
target = []
img_nm = []
img_idx = []
media = []
q1 = []
q3 = []
i = 0
percentage_print = 0
k_filter = kalman.kalman_filter()
for image_name in image_names:
#img_rgb = cv2.imread(image_name,-1)
#img_rgb_cal = calibrate_images(img_rgb.copy(), True, opt)
ret = detector.run(image_name)
im = image_name
imd = image_names_d[i]
i = i + 1
percentage = int(i * 100 / len(image_names))
#para sacar por pantalla el progreso
if percentage >= percentage_print:
string = "["
for x in range(int(100 / 2.5)):
if x <= int(percentage / 2.5):
string = string + "|"
else:
string = string + "-"
string = string + "] "
print(string + str(percentage) + '%')
percentage_print = percentage_print + 2.5
p, t, m, q, qq = eval_total(ret, im, imd, i, opt, path_save, k_filter)
for pp in p:
pred.append(pp)
for tt in t:
target.append(tt)
img_nm.append(im)
img_idx.append(i)
for mm in m:
media.append(mm)
for q11 in q:
q1.append(q11)
for q33 in qq:
q3.append(q33)
print("Analisis completado...")
data = {
'Prediction': pred,
'Target': target,
'Media': media,
'Q1': q1,
'Q3': q3,
'Img_index': img_idx,
'Img_name': img_nm
}
df = pd.DataFrame(data)
df.to_excel(os.path.join(path_dataset, 'debug_data.xlsx'))
print("Archivo creado")