def train(dataset_name):
# Read parameters
with open('config/real_lidar_cut.yaml') as file:
yaml_data = yaml.load(file, Loader=yaml.FullLoader)
fn_train, fn_out, hinge_resolution, gamma, q_resolution, t_steps = yaml_data['fn_train'], yaml_data['fn_out'], yaml_data['hinge_resolution'], yaml_data['gamma'], yaml_data['q_resolution'], yaml_data['t_steps']
fn_train = os.getcwd() + fn_train
fn_out = os.getcwd() + fn_out
# Read data
for framei in range(t_steps-1):
# Load data
print('\nReading ' + fn_train + '.csv...'.format(framei))
#df = pd.read_csv(fn_train + '.csv'.format(framei), delimiter=',').values[:, :]
df = dataset_name
df = df[df[:,0] == framei, 1:]
print(df,'df is printing')
# Determine the lidar area
delta = 1
area_min, area_max = df[:, :2].min(axis=0), df[:, :2].max(axis=0)
area_max_min = [area_min[0] - delta, area_max[0] + delta, area_min[1] - delta, area_max[1] + delta]
print('Cell area = {}'.format(area_max_min))
# Prepare data
df = pt.tensor(df, dtype=pt.float32)
X = df[:, :2]
y = df[:, 2].reshape(-1, 1)
print(' Data shape={}'.format(X.shape))
# Define the model
bhm_mdl = BHM2D_PYTORCH(gamma=gamma, grid=None, cell_resolution=hinge_resolution, cell_max_min=area_max_min, X=X, nIter=1)
# Fit the model
t1 = time.time()
bhm_mdl.fit(X, y)
t2 = time.time()
trainTime = np.round(t2-t1, 3)
print(' Training time={} s'.format(trainTime))
# Query the model
xx, yy= np.meshgrid(np.arange(area_max_min[0], area_max_min[1] + q_resolution, q_resolution),
np.arange(area_max_min[2], area_max_min[3] + q_resolution, q_resolution))
grid = np.hstack((xx.ravel()[:, np.newaxis], yy.ravel()[:, np.newaxis]))
Xq = pt.tensor(grid, dtype=pt.float32)
t1 = time.time()
yq = bhm_mdl.predict(Xq)
t2 = time.time()
queryTime = np.round(t2-t1, 3)
print(' Query time={} s'.format(queryTime))
print("yq is",yq)
print("Xq is",Xq)
#update chart
#run chart
# Plot frame i
plt.close('all')
plt.figure(figsize=(3,3))
plt.scatter(Xq[:, 0], Xq[:, 1], c=yq, cmap='jet', s=20, vmin=0, vmax=1, edgecolors='')
plt.colorbar()
plt.title('frame{}'.format(framei))
fig = plt.figure()
ax = fig.add_axes([0.1,0.1,0.4,0.8])
plot = ax.scatter #sys.exit()
#print(' Plotted.')
#np.save(fn_out+'_frame{}'.format(framei), np.hstack((Xq,yq[:,None])),allow_pickle=True, fix_imports=True) #return data, don't save
#break
#data = np
#return data #return yq,xq, plot yq and xq
#print(data)
#bhm =[]
bhm = np.hstack((Xq,yq[:,None]))
print('bhm made')
return bhm
def train_data(filename, frames, out_path, condition):
# Settings
dtype = pt.float32
device = pt.device("cpu")
# Global variables and hyperparameters
QUERY_RESOLUTION = 0.5
QUERY_RESOLUTION_FULLMAP = 2*QUERY_RESOLUTION
all_parameters = np.asarray([])
# Read the train and test files
fn_train, cell_resolution, cell_max_min, max_distance, _, _, gamma = load_parameters(filename)
print('\nReading '+fn_train)
dt = np.load(fn_train)
Xt = dt['X_train']
yt = dt['Y_train'].reshape(-1, 1)
# Combine x and y data in one big matrix
g = np.hstack((Xt, yt))
# Learn parameters for each frame
for framei in frames:
print('\nReading frame {}'.format(framei))
# Filter by frame
layer = (g[:,0] == framei)
buffer_datapoints = g[layer, :]
# Extract X and y data from memory buffer for plotting and training
f = pt.tensor(buffer_datapoints, dtype=pt.float32)
X = f[:, 1:3]
y = f[:, 3].reshape(-1, 1)
# Create kernels for learning based on datapoints max and min
frame_limit = _frame_grid(X, cell_resolution[0])
xx, yy = np.meshgrid(np.arange(frame_limit[0], frame_limit[1], cell_resolution[0]), \
np.arange(frame_limit[2], frame_limit[3], cell_resolution[1]))
model_grid = np.hstack((xx.ravel()[:, np.newaxis], yy.ravel()[:, np.newaxis]))
# Graph LiDAR points
ones_ = np.where(buffer_datapoints[:,3]==1)
hit_pts = buffer_datapoints[ones_]
zeros_ = np.where(buffer_datapoints[:,3]==0)
free_pts = buffer_datapoints[zeros_]
# IMAGE 1: LiDAR Hit Points and Free Points
fig, axs = pl.subplots(1, 4, figsize=(18, 3.5))
axs[0].plot(free_pts[:,1], free_pts[:,2],'b.',ms=1.0)
axs[0].plot(hit_pts[:,1], hit_pts[:,2], 'r.',ms=1.0)
axs[0].set_title('LiDAR Points')
fig.suptitle('Frame {}'.format(framei))
# Learn parameters for the current frame
totalTime = 0
bhm_mdl = BHM2D_PYTORCH(gamma=gamma, grid=model_grid, cell_resolution=cell_resolution, cell_max_min=cell_max_min, X=X, nIter=1)
t1 = time.time()
mu, sig = bhm_mdl.fit(X, y)
t2 = time.time()
totalTime += (t2-t1)
# Record mu, sig parameters for kernels in format: time, x1, x2, mu, sig
mu = mu.numpy().reshape(-1,1)
sig = sig.numpy().reshape(-1,1)
current_parameters = np.hstack((model_grid, np.hstack((mu,sig))))
frame_header = np.full((current_parameters.shape[0], 1), framei)
current_parameters = np.hstack((frame_header, current_parameters))
all_parameters = np.append(all_parameters.reshape(-1, 5), current_parameters, axis=0)
# Query the model
xx, yy= np.meshgrid(np.arange(frame_limit[0]-cell_resolution[0], frame_limit[1], QUERY_RESOLUTION),
np.arange(frame_limit[2]-cell_resolution[1], frame_limit[3], QUERY_RESOLUTION))
query_grid = np.hstack((xx.ravel()[:, np.newaxis], yy.ravel()[:, np.newaxis]))
Xq = pt.tensor(query_grid, dtype=pt.float32)
yq = bhm_mdl.predict(Xq)
mean, zq = bhm_mdl.predictSampling(Xq)
print(' Total training time={} s'.format(np.round(totalTime, 2)))
# IMAGE 2: Instantaneous MEAN map
axs[1].scatter(Xq[:, 0], Xq[:, 1], c=yq, cmap='jet', s=5, vmin=0, vmax=1, edgecolors='')
pcm =axs[1].get_children()[0] #get the mappable, the 1st and the 2nd are the x and y axes
pl.colorbar(pcm, ax=axs[1])
axs[1].set_title('Instantaneous map (mean)')
# IMAGE 3: Instantaneous VARIANCE map
axs[2].scatter(Xq[:, 0], Xq[:, 1], c=zq, cmap='jet', s=5, vmin=0, vmax=0.51, edgecolors='')
pcm =axs[2].get_children()[0] #get the mappable, the 1st and the 2nd are the x and y axes
pl.colorbar(pcm, ax=axs[2])
axs[2].set_title('Instantaneous map (variance)')
current_data = np.hstack((Xq, yq.reshape(-1,1)))
current_data = np.hstack((current_data, zq.reshape(-1,1)))
np.save(out_path + 'mean_var_frame{}'.format(framei), current_data)
# Add in entire map
all_data = _save_new_weights(all_parameters, condition)
kernel_grid = all_data[:, 0:2]
kernel_weights = all_data[:, 2:4]
# Create the model
bhm_mdl = BHM2D_PYTORCH(gamma=gamma, grid=kernel_grid, cell_resolution=cell_resolution, cell_max_min=cell_max_min, X=None, nIter=1, mu_sig = kernel_weights)
# Query the model
xx, yy= np.meshgrid(np.arange(cell_max_min[0], cell_max_min[1], QUERY_RESOLUTION_FULLMAP),
np.arange(cell_max_min[2], cell_max_min[3], QUERY_RESOLUTION_FULLMAP))
all_query_grid = np.hstack((xx.ravel()[:, np.newaxis], yy.ravel()[:, np.newaxis]))
Xq_all = pt.tensor(all_query_grid, dtype=dtype)
print("Grid for entire map initialized")
t5 = time.time()
all_yq = bhm_mdl.predict(Xq_all)
t6 = time.time()
globalPredTime = (t6-t5)
print(' Total global predict time={} s'.format(np.round(globalPredTime, 2)))
print("\nPlotting for frame {}".format(framei))
# IMAGE 4 MEAN MAP OF TOWN
axs[3].scatter(Xq_all[:, 0], Xq_all[:, 1], c=all_yq, cmap='jet', s=5, vmin=0, vmax=1, edgecolors='')
pcm =axs[3].get_children()[0] #get the mappable, the 1st and the 2nd are the x and y axes
pl.colorbar(pcm, ax=axs[3])
axs[3].set_xlim(cell_max_min[0], cell_max_min[1])
axs[3].set_ylim(cell_max_min[2], cell_max_min[3])
axs[3].set_title('Map (mean) of the town')
# Save the figure
pl.savefig(os.path.abspath(out_path + 'frame{}.png'.format(framei)))
pl.close('all')
np.save(out_path + 'all_parameters_frame{}_to_{}'.format(FRAMES[0], FRAMES[-1]),all_parameters)
return all_parameters
def bhm_train(df, config_name, predict=True):
print(" |_training started")
# Read parameters
with open(config_name) as file:
yaml_data = yaml.load(file, Loader=yaml.FullLoader)
fn_train, fn_out, hinge_resolution, gamma, q_resolution, t_steps = yaml_data['fn_train'], yaml_data['fn_out'], \
yaml_data['hinge_resolution'], yaml_data[
'gamma'], yaml_data['q_resolution'], \
yaml_data['t_steps']
fn_train = os.getcwd() + fn_train
fn_out = os.getcwd() + fn_out
df = df[:, 1:]
# Determine the lidar area
delta = 1
area_min, area_max = df[:, :2].min(axis=0), df[:, :2].max(axis=0)
area_max_min = [
area_min[0] - delta, area_max[0] + delta, area_min[1] - delta,
area_max[1] + delta
]
print(' |_cell area = {}'.format(area_max_min))
# Prepare data
df = pt.tensor(df, dtype=pt.float32)
X = df[:, :2]
y = df[:, 2].reshape(-1, 1)
print(' |_data shape={}'.format(X.shape))
# Define the model
bhm_mdl = BHM2D_PYTORCH(gamma=gamma,
grid=None,
cell_resolution=hinge_resolution,
cell_max_min=area_max_min,
X=X,
nIter=1)
# Fit the model
t1 = time.time()
bhm_mdl.fit(X, y)
t2 = time.time()
trainTime = np.round(t2 - t1, 3)
print(' |_training time={} s'.format(trainTime))
if predict is True:
# Query the model
xx, yy = np.meshgrid(
np.arange(area_max_min[0], area_max_min[1] + q_resolution,
q_resolution),
np.arange(area_max_min[2], area_max_min[3] + q_resolution,
q_resolution))
grid = np.hstack((xx.ravel()[:, np.newaxis], yy.ravel()[:,
np.newaxis]))
Xq = pt.tensor(grid, dtype=pt.float32)
t1 = time.time()
yq = bhm_mdl.predict(Xq)
t2 = time.time()
queryTime = np.round(t2 - t1, 3)
print(' |_query time={} s'.format(queryTime))
return Xq, yq
else:
return None, None
def train(dataset_name):
# Read parameters
with open('config/' + dataset_name + '.yaml') as file:
yaml_data = yaml.load(file, Loader=yaml.FullLoader)
fn_train, fn_out, hinge_resolution, gamma, q_resolution, t_steps = yaml_data[
'fn_train'], yaml_data['fn_out'], yaml_data[
'hinge_resolution'], yaml_data['gamma'], yaml_data[
'q_resolution'], yaml_data['t_steps']
fn_train = os.getcwd() + fn_train
fn_out = os.getcwd() + fn_out
# Read data
for framei in range(t_steps):
# Load data
print('\nReading ' + fn_train + '.csv...'.format(framei))
df = pd.read_csv(fn_train + '.csv'.format(framei),
delimiter=',').values[:, :]
df = df[df[:, 0] == framei, 1:]
# Determine the lidar area
delta = 1
area_min, area_max = df[:, :2].min(axis=0), df[:, :2].max(axis=0)
area_max_min = [
area_min[0] - delta, area_max[0] + delta, area_min[1] - delta,
area_max[1] + delta
]
print('Cell area = {}'.format(area_max_min))
# Prepare data
df = pt.tensor(df, dtype=pt.float32)
X = df[:, :2]
y = df[:, 2].reshape(-1, 1)
print(' Data shape={}'.format(X.shape))
# Define the model
bhm_mdl = BHM2D_PYTORCH(gamma=gamma,
grid=None,
cell_resolution=hinge_resolution,
cell_max_min=area_max_min,
X=X,
nIter=1)
# Fit the model
t1 = time.time()
bhm_mdl.fit(X, y)
t2 = time.time()
trainTime = np.round(t2 - t1, 3)
print(' Training time={} s'.format(trainTime))
# Query the model
xx, yy = np.meshgrid(
np.arange(area_max_min[0], area_max_min[1] + q_resolution,
q_resolution),
np.arange(area_max_min[2], area_max_min[3] + q_resolution,
q_resolution))
grid = np.hstack((xx.ravel()[:, np.newaxis], yy.ravel()[:,
np.newaxis]))
Xq = pt.tensor(grid, dtype=pt.float32)
t1 = time.time()
yq = bhm_mdl.predict(Xq)
t2 = time.time()
queryTime = np.round(t2 - t1, 3)
print(' Query time={} s'.format(queryTime))
# Plot frame i
pl.close('all')
pl.figure(figsize=(3, 3))
pl.scatter(Xq[:, 0],
Xq[:, 1],
c=yq,
cmap='jet',
s=20,
vmin=0,
vmax=1,
edgecolors='')
pl.colorbar()
pl.title('frame{}'.format(framei))
pl.savefig(fn_out + '_frame{}.png'.format(framei))
#sys.exit()
print(' Plotted.')
sys.exit()
X = g[:, :2]
y = g[:, 3].reshape(-1, 1)
# if pt.cuda.is_available():
# X = X.cuda()
# y = y.cuda()
toPlot = []
totalTime = 0
for segi in range(len(cell_max_min_segments)):
print(' Mapping segment {} of {}...'.format(
segi + 1, len(cell_max_min_segments)))
cell_max_min = cell_max_min_segments[segi]
bhm_mdl = BHM2D_PYTORCH(gamma=gamma,
grid=None,
cell_resolution=cell_resolution,
cell_max_min=cell_max_min,
X=X,
nIter=1)
t1 = time.time()
bhm_mdl.fit(X, y)
t2 = time.time()
totalTime += (t2 - t1)
# query the model
q_resolution = 0.5
xx, yy = np.meshgrid(
np.arange(cell_max_min[0], cell_max_min[1] - 1, q_resolution),
np.arange(cell_max_min[2], cell_max_min[3] - 1, q_resolution))
grid = np.hstack((xx.ravel()[:, np.newaxis], yy.ravel()[:,
np.newaxis]))