print('[INF] Kernel parameters')
for row in goalsData.kernelsX:
for ker in row:
ker.print_parameters()
"""
"""********** Testing ***********"""
gi, gj, pathId = 0, 6, 5
# Get the ground truth path
path = trajMat[gi][gj][pathId]
pathX, pathY, pathT = path
# Total path length
pathSize = len(pathX)
# Prediction of single paths with single goals
gp = singleGP(gi,gj,goalsData,'Trautman')
# Divides the trajectory in part_num parts and infer the posterior over the remaining part
part_num = 3
for i in range(1,part_num-1):
p = plotter(imgGCS)
p.plot_scene_structure(goalsData)
# Data we will suppose known
knownN = int((i+1)*(pathSize/part_num))
observations = observed_data(path,knownN)
"""Single goal prediction test"""
# Update the GP with (real) observations
start = time.process_time()
likelihood = gp.update(observations)
stop = time.process_time()
# Perform prediction
nParameters = 4
# Read the kernel parameters from file
goalsData.kernelsX = read_and_set_parameters(
"parameters/linearpriorcombined20x20_x.txt", nParameters)
goalsData.kernelsY = read_and_set_parameters(
"parameters/linearpriorcombined20x20_y.txt", nParameters)
# Sampling 3 trajectories between all the pairs of goals
allPaths = []
for i in range(goalsData.goals_n):
for j in range(i, goalsData.goals_n):
if (i != j) and len(
trajMat[i]
[j]) > 0 and goalsData.kernelsX[i][j].optimized is True:
path = trajMat[i][j][0]
pathX, pathY, pathT = path
pathL = trajectory_arclength(path)
observations, __ = observed_data([pathX, pathY, pathL, pathT], 2)
if observations is None:
continue
# The basic element here is this object, that will do the regression work
gp = singleGP(i, j, goalsData)
likelihood = gp.update(observations)
# Generate samples
predictedXY, varXY = gp.predict_path(compute_sqRoot=True)
paths = gp.sample_paths(3)
allPaths = allPaths + paths
plot_path_samples(img, allPaths)
startG, endG = 0, 7
pathId = np.random.randint(0, len(trajMat[startG][endG]))
# Kernels for this pair of goals
kernelX = goalsData.kernelsX[startG][endG]
kernelY = goalsData.kernelsY[startG][endG]
# Get the ground truth path
path = trajMat[startG][endG][pathId]
# Get the path data
pathX, pathY, pathT = path
pathL = trajectory_arclength(path)
pathS = trajectory_speeds(path)
# Total path length
pathSize = len(pathX)
# Test function: prediction of single trajectories with a single goal
gp = singleGP(startG, endG, goalsData)
# For different sub-parts of the trajectory
for knownN in range(10, pathSize - 1, 10):
fig, ax = plt.subplots(4, 1)
ax[0].set_ylabel('x')
ax[0].set_xlabel('l')
ax[1].set_ylabel('y')
ax[1].set_xlabel('l')
ax[2].set_ylabel('v')
ax[2].set_xlabel('l')
ax[3].set_ylabel('t')
ax[3].set_xlabel('l')
p = plotter()
p.set_background(imgGCS)
p.plot_scene_structure(goalsData)
gi, gj = 0, 7
pathId = np.random.randint(0, len(trajMat[gi][gj]))
# Get the ground truth path
if goalsData.kernelsX[gi][gj].optimized is not True:
print("[INF] This pair of goals have not optimized parameters. Aborting.")
sys.exit()
path = trajMat[gi][gj][pathId]
pathX, pathY, pathT = path
pathL = trajectory_arclength(path)
# Total path length
pathSize = len(pathX)
# Prediction of single paths with single goals
gp = singleGP(gi, gj, goalsData)
# Divides the trajectory in part_num parts and infer the posterior over the remaining part
part_num = 10
for i in range(1, part_num - 1):
p = plotter()
print('--------------------------')
if coordinates == 'img':
p.set_background(imgGCS)
p.plot_scene_structure(goalsData)
# Data we will suppose known
knownN = int((i + 1) * (pathSize / part_num))
observations, ground_truth = observed_data([pathX, pathY, pathL, pathT],
knownN)
"""Single goal prediction test"""
print('[INF] Updating likelihoods')