def main():
noise = True
if not noise:
tri_orig = np.loadtxt('../files/obstacles_map.txt')
tri_alt = np.loadtxt('../files/obstacles_laser.txt')
else:
m = np.array([0, 0])
c = np.array([[1, 0], [0, 1]])
tri_orig = np.loadtxt('../files/obstacles_map.txt')
tri_alt = np.loadtxt('../files/obstacles_laser.txt')
tri_alt = tri_alt + \
np.random.multivariate_normal(m, c, len(tri_alt)) * 0.1
true_rotation = np.pi / 4
true_translation = np.array([0, 0])
tri_alt = support.rotate(tri_alt, true_rotation)
tri_alt = support.translate(tri_alt, true_translation)
tri_orig_centered = support.center(tri_orig)
tri_alt_centered = support.center(tri_alt)
support.plot_double(tri_orig_centered, tri_alt_centered, "Original")
start = time.time()
map_tree = spatial.cKDTree(tri_orig)
num_points = 5
results = []
while True:
map_partition = support.partition_space(tri_orig, map_tree, num_points)
for pt in map_partition:
tri_alt_trans = support.translate(tri_alt_centered, pt)
orig_dists = support.measure_distance(tri_orig_centered)
alt_dists = support.measure_distance(tri_alt_trans)
# FFT
orig_fft, orig_angle = support.compute_fft(orig_dists)
alt_fft, alt_angle = support.compute_fft(alt_dists)
ps = orig_angle - alt_angle
results.append(ps[0])
break
print "Estimated PS:", min(results)
print "Real PS:", true_rotation
end = time.time()
print "Time:", str(end - start)
plt.show()
def main():
noise = True
if not noise:
tri_orig = np.loadtxt('../files/obstacles_map.txt')
tri_alt = np.loadtxt('../files/obstacles_laser.txt')
else:
m = np.array([0, 0])
c = np.array([[1, 0], [0, 1]])
tri_orig = np.loadtxt('../files/obstacles_map.txt')
tri_alt = np.loadtxt('../files/obstacles_laser.txt')
tri_alt = tri_alt + \
np.random.multivariate_normal(m, c, len(tri_alt)) * 0.1
true_rotation = np.pi / 4
true_translation = np.array([0, 0])
tri_alt = support.rotate(tri_alt, true_rotation)
tri_alt = support.translate(tri_alt, true_translation)
tri_orig_centered = support.center(tri_orig)
tri_alt_centered = support.center(tri_alt)
support.plot_double(tri_orig_centered, tri_alt_centered, "Original")
start = time.time()
map_tree = spatial.cKDTree(tri_orig)
num_points = 5
results = []
while True:
map_partition = support.partition_space(tri_orig, map_tree, num_points)
for pt in map_partition:
tri_alt_trans = support.translate(tri_alt_centered, pt)
orig_dists = support.measure_distance(tri_orig_centered)
alt_dists = support.measure_distance(tri_alt_trans)
# FFT
orig_fft, orig_angle = support.compute_fft(orig_dists)
alt_fft, alt_angle = support.compute_fft(alt_dists)
ps = orig_angle - alt_angle
results.append(ps[0])
break
print "Estimated PS:", min(results)
print "Real PS:", true_rotation
end = time.time()
print "Time:", str(end - start)
plt.show()
def canBeSubSorou(aSorou, anotherSorou):
"""
Returns true if anotherSorou can be rotated so that it is a subSorou of aSorou, and false otherwise
"""
output = False
aSorouRelOrder = support.findRelativeOrder(aSorou)
anotherSorouRelORder = support.findRelativeOrder(anotherSorou)
crossRelativeOrder = support.lcm(aSorouRelOrder, anotherSorouRelORder)
for index in range(crossRelativeOrder):
rotatedSorou = support.rotate(anotherSorou, crossRelativeOrder, index)
if support.isSubSorou(aSorou, rotatedSorou):
output = True
break
return output
def makeSubtractedSorouList(aSorou, aSorouList):
"""
For each sorou in a sorou list, find all ways in which aSorou can be subtracted from it
Returns the list of all possible results from the subractions
"""
output = []
firstRelOrder = support.findRelativeOrder(aSorou)
for sorou in aSorouList:
totalRelOrder = support.lcm(firstRelOrder,
support.findRelativeOrder(sorou))
for index in range(totalRelOrder):
rotated = support.rotate(sorou, totalRelOrder, index)
temp = support.subtractSorous(rotated, aSorou)
if len(temp) == len(rotated) - len(aSorou) and shouldAdd(
temp, output):
output.append(temp)
return output
def main():
tri = True
noise = True
if not tri:
if not noise:
tri_orig = np.loadtxt('../files/obstacles_laser.txt')
tri_alt = copy.deepcopy(tri_orig)
else:
m = np.array([0, 0])
c = np.array([[1, 0], [0, 1]])
tri_orig = np.loadtxt('../files/obstacles_laser.txt')
tri_alt = copy.deepcopy(tri_orig) + \
np.random.multivariate_normal(m, c, len(tri_orig)) * 0.1
else:
if not noise:
tri_orig = support.create_triangle()
tri_alt = copy.deepcopy(tri_orig)
else:
m = np.array([0, 0])
c = np.array([[1, 0], [0, 1]])
tri_orig = support.create_triangle()
tri_alt = copy.deepcopy(tri_orig) + \
np.random.multivariate_normal(m, c, len(tri_orig)) * 0.5
true_rotation = np.pi
true_translation = np.array([0, 0])
tri_alt = support.rotate(tri_alt, true_rotation)
tri_alt = support.translate(tri_alt, true_translation)
tri_orig_centered = support.center(tri_orig)
tri_alt_centered = support.center(tri_alt)
support.plot_double(tri_orig_centered, tri_alt_centered, "Original")
start = time.time()
orig_dists = support.measure_distance(tri_orig_centered)
alt_dists = support.measure_distance(tri_alt_centered)
support.plot_double(orig_dists, alt_dists, "D(alpha)")
# FFT
orig_abs, orig_angle = support.compute_fft(orig_dists)
alt_abs, alt_angle = support.compute_fft(alt_dists)
# support.plot_double(orig_fft_cte, alt_fft_cte, "FT Constant Terms")
ps = orig_angle - alt_angle
abs_diff = orig_abs - alt_abs
weight_n = [1 / n if n != 0 else 1 for n in xrange(len(ps))]
weight_n_sq = [1 / np.square(n) if n != 0 else 1 for n in xrange(len(ps))]
print "PS[0]:", ps[0], "\t\t\tABS[0]:", abs_diff[0]
print "---------------------------------------------------------------"
print "PS[1]:", ps[1], "\t\tABS[0]:", abs_diff[1]
print "PS[2]:", ps[2], "\t\tABS[0]:", abs_diff[2]
print "PS[3]:", ps[3], "\t\tABS[0]:", abs_diff[3]
print "PS[4]:", ps[4], "\t\tABS[0]:", abs_diff[4]
print "PS[5]:", ps[5], "\t\tABS[0]:", abs_diff[5]
print "PS[6]:", ps[6], "\t\tABS[0]:", abs_diff[6]
print "PS[7]:", ps[7], "\t\tABS[0]:", abs_diff[7]
print "PS[8]:", ps[8], "\t\tABS[0]:", abs_diff[8]
print "PS[9]:", ps[9], "\t\tABS[0]:", abs_diff[9]
print "PS[10]:", ps[10], "\t\tABS[0]:", abs_diff[10]
print "---------------------------------------------------------------"
print "PS Sum:", np.sum(ps), "\t\tABS Sum:", np.sum(abs_diff)
print "PS Sum W(1 / n):", np.sum(np.dot(ps, weight_n)), \
"\t\tABS Sum W(1 / n):", np.sum(np.dot(abs_diff, weight_n))
print "PS Sum W(1 / sq(n)):", np.sum(np.dot(ps, weight_n_sq)), \
"\t\tABS Sum W(1 / sq(n)):", np.sum(np.dot(abs_diff, weight_n_sq))
print "---------------------------------------------------------------"
print "Real PS:", true_rotation
end = time.time()
tri_alt_corrected = support.rotate(tri_alt, -ps[1])
support.plot_double(tri_orig, tri_alt_corrected, "Corrected Shape")
print "\n\nTime:", str(end - start)
plt.show()
def main():
tri = True
noise = False
plots = False
if not tri:
if not noise:
tri_orig = np.loadtxt('../files/obstacles_laser.txt')
tri_cpy = copy.deepcopy(tri_orig)
else:
m = np.array([0, 0])
c = np.array([[1, 0], [0, 1]])
tri_orig = np.loadtxt('../files/obstacles_laser.txt')
tri_cpy = copy.deepcopy(tri_orig) + \
np.random.multivariate_normal(m, c, len(tri_orig)) * 1.5
else:
if not noise:
tri_orig = support.create_triangle()
tri_cpy = copy.deepcopy(tri_orig)
else:
m = np.array([0, 0])
c = np.array([[1, 0], [0, 1]])
tri_orig = support.create_triangle()
tri_cpy = copy.deepcopy(tri_orig) + \
np.random.multivariate_normal(m, c, len(tri_orig)) * 0.05
true_rotation = 0
true_translation = np.array([0, 0])
tri_rot = copy.deepcopy(support.rotate(tri_cpy, true_rotation))
tri_alt = copy.deepcopy(support.translate(tri_rot, true_translation))
if plots:
support.plot_double(tri_orig, tri_alt, "Original")
# tri_orig_fun = support.function_from_pc(tri_orig)
# tri_alt_fun = support.function_from_pc(tri_alt)
x, y, tri_orig_fun, tri_alt_fun = support.get_2_2d_functions(tri_orig, tri_alt)
# tri_orig_fun, tri_alt_fun = support.zero_pad(tri_orig_fun, tri_alt_fun)
if plots:
# support.plot_contour(x, y, tri_orig_fun)
# support.plot_contour(x, y, tri_alt_fun)
support.plot_contour_single(tri_orig_fun)
support.plot_contour_single(tri_alt_fun)
start = time.time()
# Function FFT
orig_fft = support.compute_fft2d(tri_orig_fun, plot=plots,
title="Original")
alt_fft = support.compute_fft2d(tri_alt_fun, plot=plots,
title="Alternative")
orig_f, orig_log_base = support.logpolar(tri_orig_fun)
alt_f, alt_log_base = support.logpolar(tri_alt_fun)
orig_lp_fft = support.compute_fft2d(orig_f)
alt_lp_fft = support.compute_fft2d(alt_f)
print "Orig FFT, LP:", orig_fft.shape, orig_f.shape, orig_log_base
print "Alt FFT, LP:", alt_fft.shape, alt_f.shape, alt_log_base
# Pointcloud FFT
pc_orig_fft = support.compute_fft2d(tri_orig)
pc_alt_fft = support.compute_fft2d(tri_alt)
# Function-to-Pointcloud ratios
rx = 1 / 100.0 # np.max(tri_orig[:, 0]) / tri_orig_fun.shape[0]
ry = 1 / 100.0 # np.max(tri_orig[:, 1]) / tri_orig_fun.shape[1]
# Cross-correlation
# corr = signal.correlate2d(tri_orig_fun, tri_alt_fun, mode='same')
# print "Corr;", np.unravel_index(np.argmax(corr), corr.shape)
# Translation estimation
et = support.phase_correlation(orig_fft, alt_fft, rx, ry)
er = 0
# support.numeric_translation(orig_fft, alt_fft, tri_orig_fun)
# Rotation estimation
orig_abs = np.abs(orig_fft)
alt_abs = np.abs(alt_fft)
oxy = np.where(orig_abs >= 0)
oxy = np.vstack((oxy[1], oxy[0])).T * rx
axy = np.where(alt_abs >= 0)
axy = np.vstack((axy[1], axy[0])).T * rx
print "O:", oxy.shape
print "A:", axy.shape
r = support.kabsch(oxy, axy)
print r
er = np.arccos(r.item(0, 1))
print "\nTrue Translation:", true_translation
print "True Rotation:", true_rotation
print "\nEstimated Translation:", et
print "Estimated Rotation:", er
print "\nTranslation Error:", np.abs(true_translation - et)
print "Rotation Error:", np.abs(true_rotation - er)
end = time.time()
print "\n\nTime:", str(end - start)
plt.show()
def genAllSorousOfMinVanType(aMinVanType, aTypeSorouMap, aPermutationsMap):
aTypeStr = support.typeToLatexString(aMinVanType)
if aTypeStr in aTypeSorouMap.keys() and aTypeSorouMap[aTypeStr] != []:
return aTypeSorouMap[aTypeStr]
outputKeys = set()
output = []
subTypes = aMinVanType[0][2:]
subSorous = []
for subType in subTypes:
subSorou = genAllSorousOfType(subType, aMinVanType[0][1],
aTypeSorouMap, aPermutationsMap)
subSorous.append(subSorou)
support.tPrint("Generating SubType Permutations")
subTypePermunations = findSubTypePermutations(len(subTypes),
aMinVanType[0][0],
aPermutationsMap)
support.tPrint("{} SubType Permutations Generated".format(
len(subTypePermunations)))
subSorouPermutations = []
for permutation in subTypePermunations:
temp = []
for index in permutation:
if index == 0:
temp.append([False])
else:
temp.append(subSorous[index - 1])
newSubSorouPermutations = [p for p in itertools.product(*temp)]
for item in newSubSorouPermutations:
if item not in subSorouPermutations:
subSorouPermutations.append(item)
support.tPrint("{} subSorouPermutations Generated".format(
len(subSorouPermutations)))
for i, permutation in enumerate(subSorouPermutations):
support.tPrint([
"New subSorou permutation", '\t', i + 1, " of ",
len(subSorouPermutations)
])
newSorou = genBaseSorou(aMinVanType)
for index in range(aMinVanType[0][0]):
if permutation[index]:
rotationIndicies = findRotationIndicies(
permutation[index], newSorou[index][0])
for rotationIndex in rotationIndicies:
workingSubSorou = support.rotate(permutation[index],
rotationIndex[0],
rotationIndex[1])
if support.isSubSorou(workingSubSorou, newSorou[index][0]):
temp = support.subtractSorous(newSorou[index][0],
workingSubSorou)
newSorou[index].append(temp)
working = selectFromNewSorou(newSorou)
support.tPrint(
["All potential subsorours generated for this permutation"])
for item in working:
if len(item) == support.findWeightofType(aMinVanType):
sortedItem = support.ssort(item)
itemKey = support.sorouToLatexString(sortedItem)
if itemKey not in outputKeys:
print(item)
outputKeys.add(itemKey)
output.append(item)
# if shouldAdd(item, output):
# output.append(item)
# print(item)
if output == []:
print("ERROR: NO SOROUS GENERATED FOR {}".format(
support.typeToLatexString(aMinVanType)))
return output
def genBaseSorou(aMinVanType):
working = aMinVanType[0]
baseSorou = []
for index in range(working[0]):
baseSorou.append([support.rotate(working[1], working[0], index)])
return baseSorou
def main():
tri = True
noise = False
plots = False
if not tri:
if not noise:
tri_orig = np.loadtxt('../files/obstacles_laser.txt')
tri_cpy = copy.deepcopy(tri_orig)
else:
m = np.array([0, 0])
c = np.array([[1, 0], [0, 1]])
tri_orig = np.loadtxt('../files/obstacles_laser.txt')
tri_cpy = copy.deepcopy(tri_orig) + \
np.random.multivariate_normal(m, c, len(tri_orig)) * 1.5
else:
if not noise:
tri_orig = support.create_triangle()
tri_cpy = copy.deepcopy(tri_orig)
else:
m = np.array([0, 0])
c = np.array([[1, 0], [0, 1]])
tri_orig = support.create_triangle()
tri_cpy = copy.deepcopy(tri_orig) + \
np.random.multivariate_normal(m, c, len(tri_orig)) * 0.05
true_rotation = 0
true_translation = np.array([0, 0])
tri_rot = copy.deepcopy(support.rotate(tri_cpy, true_rotation))
tri_alt = copy.deepcopy(support.translate(tri_rot, true_translation))
if plots:
support.plot_double(tri_orig, tri_alt, "Original")
# tri_orig_fun = support.function_from_pc(tri_orig)
# tri_alt_fun = support.function_from_pc(tri_alt)
x, y, tri_orig_fun, tri_alt_fun = support.get_2_2d_functions(
tri_orig, tri_alt)
# tri_orig_fun, tri_alt_fun = support.zero_pad(tri_orig_fun, tri_alt_fun)
if plots:
# support.plot_contour(x, y, tri_orig_fun)
# support.plot_contour(x, y, tri_alt_fun)
support.plot_contour_single(tri_orig_fun)
support.plot_contour_single(tri_alt_fun)
start = time.time()
# Function FFT
orig_fft = support.compute_fft2d(tri_orig_fun,
plot=plots,
title="Original")
alt_fft = support.compute_fft2d(tri_alt_fun,
plot=plots,
title="Alternative")
orig_f, orig_log_base = support.logpolar(tri_orig_fun)
alt_f, alt_log_base = support.logpolar(tri_alt_fun)
orig_lp_fft = support.compute_fft2d(orig_f)
alt_lp_fft = support.compute_fft2d(alt_f)
print "Orig FFT, LP:", orig_fft.shape, orig_f.shape, orig_log_base
print "Alt FFT, LP:", alt_fft.shape, alt_f.shape, alt_log_base
# Pointcloud FFT
pc_orig_fft = support.compute_fft2d(tri_orig)
pc_alt_fft = support.compute_fft2d(tri_alt)
# Function-to-Pointcloud ratios
rx = 1 / 100.0 # np.max(tri_orig[:, 0]) / tri_orig_fun.shape[0]
ry = 1 / 100.0 # np.max(tri_orig[:, 1]) / tri_orig_fun.shape[1]
# Cross-correlation
# corr = signal.correlate2d(tri_orig_fun, tri_alt_fun, mode='same')
# print "Corr;", np.unravel_index(np.argmax(corr), corr.shape)
# Translation estimation
et = support.phase_correlation(orig_fft, alt_fft, rx, ry)
er = 0
# support.numeric_translation(orig_fft, alt_fft, tri_orig_fun)
# Rotation estimation
orig_abs = np.abs(orig_fft)
alt_abs = np.abs(alt_fft)
oxy = np.where(orig_abs >= 0)
oxy = np.vstack((oxy[1], oxy[0])).T * rx
axy = np.where(alt_abs >= 0)
axy = np.vstack((axy[1], axy[0])).T * rx
print "O:", oxy.shape
print "A:", axy.shape
r = support.kabsch(oxy, axy)
print r
er = np.arccos(r.item(0, 1))
print "\nTrue Translation:", true_translation
print "True Rotation:", true_rotation
print "\nEstimated Translation:", et
print "Estimated Rotation:", er
print "\nTranslation Error:", np.abs(true_translation - et)
print "Rotation Error:", np.abs(true_rotation - er)
end = time.time()
print "\n\nTime:", str(end - start)
plt.show()