def testThreeViewAllPairsEstimation(self):
np.random.seed(3)
N = 5
#Defining the external parameters
B_A = (0.1 * e12 + 0.2*e13 + 0.1 *e23 + (3*e1 -1*e2 + 2*e3)*ninf)
R_A = ga_exp(B_A)
B_B = (-0.2 * e12 + -0.1*e13- 0.05 *e23 + (1*e1 +2*e2 - 3*e3)*ninf)
R_B = ga_exp(B_B)
dx = np.random.normal(size=12, scale=0.01) #Starting estimate is slightly off
x0 = MultiViewLineImageMapping.inverserotorconversion([R_A, R_B]) + dx
R_A_start, R_B_start = MultiViewLineImageMapping.rotorconversion(x0)
lines = createRandomLines(N, scale = 2)
for i in range(len(lines)):
lines[i] = Sandwich(lines[i], Translator(e3*10))
lines_img_d_base_real = [projectLineToPlane(line, one) for line in lines] #Real lines A
lines_img_d_A_real = [projectLineToPlane(line, R_A) for line in lines] #Real lines A
lines_img_d_B_real = [projectLineToPlane(line, R_B) for line in lines] #Real lines A
sigma_R_image = 0.00001
sigma_T_image = 0.00001
lines_img_base_d = [perturbeObjectInplane(projectLineToPlane(line, one), sigma_R_image, sigma_T_image) for line in lines]
lines_img_A_d = [perturbeObjectInplane(projectLineToPlane(line, R_A), sigma_R_image, sigma_T_image) for line in lines]
lines_img_B_d = [perturbeObjectInplane(projectLineToPlane(line, R_B), sigma_R_image, sigma_T_image) for line in lines]
#print("No noise cost =", MultiViewLineImageMapping.costfunction(R_A, R_B, lines_img_d_base_real, lines_img_d_A_real, lines_img_d_B_real))
#Computation
lines_imgs_d = [lines_img_A_d, lines_img_B_d]
args = (lines_img_base_d, )
R_list, Nint = minimizeError(args ,MultiViewLineImageMapping, x0 = x0)
R_A_min, R_B_min = R_list
#Output
print("Nint = ", Nint)
print("")
print("R_A_real : ", R_A)
print("R_A_min : ", R_A_min)
print("R_A_start: ", R_A_start)
print("")
print("R_B_real: ", R_B)
print("R_B_min : ", R_B_min)
print("R_B_start: ", R_B_start)
print("")
print("Start cost =", MultiViewLineImageMapping.costfunction(R_A_start, R_B_start, lines_img_base_d, lines_img_A_d, lines_img_B_d))
print("Final cost =", MultiViewLineImageMapping.costfunction(R_A_min, R_B_min, lines_img_base_d, lines_img_A_d, lines_img_B_d))
print("Target cost =", MultiViewLineImageMapping.costfunction(R_A, R_B, lines_img_base_d, lines_img_A_d, lines_img_B_d))
print("No noise cost =", MultiViewLineImageMapping.costfunction(R_A, R_B, lines_img_d_base_real, lines_img_d_A_real, lines_img_d_B_real))
def testLinePointCost(self):
np.random.seed(1)
R_real = ga_exp(createRandomBivector())
R_other = ga_exp(createRandomBivector())
N_points = 3
N_val = 10
assert (all(linePointCostMetric(R_real, R_real, N_val) < 1E-4)
) #TODO: Not as close as I would like it to be
assert (all(linePointCostMetric(R_real, R_real, N_val) >= 0))
assert (all(linePointCostMetric(R_real, R_other, N_val) > 0))
def testImageCostFunction(self):
O1 = up(0)
B = 0.1 * e12 + 0.2*e13 + 0.1 *e23 + (3*e1 -1*e2 + 2*e3)*ninf
R = ga_exp(B)
N = 10
lines = createRandomLines(N, scale = 30)
cPlane1 = (ninf + e3)*I5 #Camera plane 1
cPlane2 = R * cPlane1 * ~R
lines_img_d = [projectLineToPlane(line, R) for line in lines]
assert(sumImageFunction(R, lines, lines_img_d) < 1e-20) #Very small
R_wrong = ga_exp(B * 0.5)
assert(sumImageFunction(R_wrong, lines, lines_img_d) > 1e-5) #not very small
def setUpMultiView(self, N_lines, K_imgs, sigma_R_image = 0.001, sigma_T_image = 0.001):
#Define random rotations for our cameras
R_list = [ga_exp(createRandomBivector()) for _ in range(K_imgs)]
#Create N lines
lines = createRandomLines(N_lines, scale = 2)
for i in range(len(lines)):
lines[i] = Sandwich(lines[i], Translator(e3*4))
#Create our noise free images for comparison
lines_img_base_d_real = [projectLineToPlane(line, one) for line in lines] #Real base lines
lines_imgs_d_real = [[projectLineToPlane(line, R_list[i]) for line in lines] for i in range(K_imgs)]
#Create noisy images
lines_img_base_d = [perturbeObjectInplane(projectLineToPlane(line, one) , sigma_R_image, sigma_T_image) for line in lines]
lines_imgs_d = [[perturbeObjectInplane(projectLineToPlane(line, R_list[i]), sigma_R_image, sigma_T_image) for line in lines] for i in range(K_imgs)]
i = 0
#for j in range(len(lines_imgs_d[i])):
# print((R_list[i]*(-no ^ lines_imgs_d_real[i][j])* ~R_list[i]).normal())
# print(((R_list[i]*(-no)* ~R_list[i])^lines[j]).normal())
# print("")
return R_list, lines, lines_img_base_d, lines_img_base_d_real , lines_imgs_d, lines_imgs_d_real
def testNoisyRotationExtraction(self):
verbose = False
np.random.seed(2)
O1 = up(0)
B = 0.1 * e12 + 0.2*e13 + 0.1 *e23 + 1*(3*e1 -1*e2 + 2*e3)*ninf
R = ga_exp(B)
N = 10
lines = createRandomLines(N, scale = 2)
for i in range(len(lines)):
lines[i] = Sandwich(lines[i], Translator(e3*3))
sigma_R_model = 0.0001
sigma_T_model = 0.0001
lines_perturbed = [perturbeObject(line, sigma_T_model, sigma_R_model) for line in lines] #Model noise
lines_img_d_real = [projectLineToPlane(line, R) for line in lines] #Real lines
sigma_R_image = 0.0001
sigma_T_image = 0.0001
lines_img_d = [perturbeObjectInplane(projectLineToPlane(line, R), sigma_R_image, sigma_T_image) for line in lines]
#using our noisy model and the noisy image of them to estimate R
R_min, Nint = minimizeError((lines_perturbed, lines_img_d), BivectorLineImageMapping, x0 = None)
if verbose:
print("R: ", R)
print("R_min", R_min)
assert(MVEqual(R_min, R, rtol = 1e-2, atol = 1e-2, verbose = False)) #Hard coded values.
#Weird condition. But we hope to find a "better" solution than the true one for the data we see, but worse than the true projection
assert(sumImageFunction(R, lines, lines_img_d) > sumImageFunction(R_min, lines, lines_img_d) > sumImageFunction(R, lines, lines_img_d_real))
def testLineProjection(self):
A, B = createRandomPoints(2)
R = ga_exp(createRandomBivector())
L = createLine(A, B)
L_img = projectLineToPlane(L, R)
A_img = projectPointToPlane(A, R)
B_img = projectPointToPlane(B, R)
L_img_actual = createLine(A_img, B_img)
assert(MVEqual(L_img, L_img_actual))
def testRotationExtraction(self):
np.random.seed(2)
O1 = up(0)
B = 0.1 * e12 + 0.2*e13 + 0.1 *e23 + (3*e1 -1*e2 + 2*e3)*ninf
R = ga_exp(B)
N = 10
lines = createRandomLines(N, scale = 30)
cPlane1 = (ninf + e3)*I5 #Camera plane 1
cPlane2 = R * cPlane1 * ~R
lines_img_d = [projectLineToPlane(line, R) for line in lines]
R_min, Nint = minimizeError((lines, lines_img_d), BivectorLineImageMapping, x0 = None)
assert(MVEqual(R_min, R, rtol = 1e-2, atol = 1e-2, verbose = False)) #Hard coded values.
assert(sumImageFunction(R, lines, lines_img_d) < sumImageFunction(R_min, lines, lines_img_d))
def testExtremeRotationExtraction(self):
verbose = True
np.random.seed(2)
O1 = up(0)
rot_scale = 10
tran_scale = 10
spread_scale = 10
B = 0.1 * e12 + 0.2*e13 + 0.1 *e23 + rot_scale*(3*e1 -1*e2 + 2*e3)*ninf
R = ga_exp(B)
N = 10
lines = createRandomLines(N, scale = spread_scale)
for i in range(len(lines)):
lines[i] = Sandwich(lines[i], Translator(e3*tran_scale))
sigma_R_model = 0.001
sigma_T_model = 0.001
lines_perturbed = [perturbeObject(line, sigma_T_model, sigma_R_model) for line in lines] #Model noise
lines_img_d_real = [projectLineToPlane(line, R) for line in lines] #Real lines
sigma_R_image = 0.001
sigma_T_image = 0.001
lines_img_d = [perturbeObjectInplane(projectLineToPlane(line, R), sigma_R_image, sigma_T_image) for line in lines]
mapping = BivectorLineImageMapping
x0 = mapping.inverserotorconversion(R)
x0[:3] += np.array([0.1, 0.9, -0.17])
R_start = mapping.rotorconversion(x0)
#using our noisy model and the noisy image of them to estimate R
R_min, Nint = minimizeError((lines_perturbed, lines_img_d), mapping, x0 = x0)
if verbose:
print("R: ", R)
print("R_min ", R_min/np.sign(R_min[0]))
print("R_start", R_start/np.sign(R_start[0]))
print("")
print("B: ", B)
print("B_min - B ", ga_log(R_min/np.sign(R_min[0])) - B)
print("B_start - B ", ga_log(R_start/np.sign(R_start[0])) - B)
def benchmarkImageCostFunction():
np.random.seed(123)
B = createRandomBivector()
R_real = ga_exp(B)
N = 10
lines = createRandomLines(N, scale = 2)
for i in range(len(lines)):
lines[i] = Sandwich(lines[i], Translator(e3*3))
sigma_R_model = 0.01
sigma_T_model = 0.01
lines_perturbed = [perturbeObject(line, sigma_T_model, sigma_R_model) for line in lines] #Model noise
lines_img_d_real = [projectLineToPlane(line, R_real) for line in lines] #Real lines
sigma_R_image = 0.01
sigma_T_image = 0.01
lines_img_d = [perturbeObjectInplane(projectLineToPlane(line, R_real), sigma_R_image, sigma_T_image) for line in lines]
traininglinesets = (lines_perturbed, lines_img_d)
benchmarkMinimizeError(R_real, traininglinesets, traininglinesets, fileout = None, mapping = BivectorLineImageMapping)
def benchmarkMinimizeError(R_real,
trainingdata,
validationdata,
N=None,
fileout=None,
mapping=BivectorLineMapping,
verificationfunction=linePointCostMetric):
"""
A function to benchmark the error using a given mapping
"""
#To allow for both writing to std out and a file
if fileout:
outfile = open(fileout, 'a')
def fileprint(string):
outfile.write(string + "\n")
else:
def fileprint(string):
print(string)
t0 = time.time()
costfunction = mapping.costfunction
#Finding the cost if we used the actual rotor used to generate the matrix
x0 = mapping.startValue()
R_start = mapping.rotorconversion(x0)
if N is None:
N = len(trainingdata)
R_min, nit = minimizeError(trainingdata, mapping=mapping, x0=x0)
realtrainingcost = costfunction(R_real, trainingdata)
fileprint("Real training cost is %s" % str(realtrainingcost))
realvalidationcost = costfunction(R_real, validationdata)
fileprint("Real validation cost is %s" % str(realvalidationcost))
fileprint("")
initialtrainingcost = costfunction(R_start, trainingdata)
fileprint("initial training cost %f" % initialtrainingcost)
initialvalidationcost = costfunction(R_start, validationdata)
fileprint("initial validation cost %f" % initialvalidationcost)
fileprint("")
minimumtrainingcost = costfunction(R_min, trainingdata)
fileprint("minimized training cost %f" % minimumtrainingcost)
minimumvalidationcost = costfunction(R_min, validationdata)
fileprint("minimized validation cost = %f" % minimumvalidationcost)
fileprint("")
fileprint("Costfunction invariant point cost %s" %
str(verificationfunction(R_min, R_real, 100)))
R_real_norm = R_real / np.sign(float(R_real(0)))
R_min_norm = R_min / np.sign(float(R_min(0)))
fileprint("")
fileprint("R_real= %s" % str(R_real_norm))
fileprint("R_min = %s" % str(R_min_norm))
B_real = ga_log(R_real_norm)
B_min = ga_log(R_min_norm)
B_diff = B_real - B_min
fileprint("")
fileprint("B_real= %s" % str(B_real))
fileprint("B_min = %s" % str(B_min))
R_diff = ga_exp(B_diff)
diff_cost = rotorAbsCostFunction(R_diff)
fileprint("")
fileprint("B_diff = %s" % str(B_diff))
fileprint("R_min = %s" % str(R_diff))
fileprint("cost(R) = %s" % str(diff_cost))
t_end = time.time()
fileprint("")
fileprint(
"Running time for extracting best rotor for %d line pairs converging after %d iterations is %f s"
% (N, nit, t_end - t0))
fileprint("\n\n")
if fileout:
outfile.close()
return realtrainingcost, minimumvalidationcost, R_min
def perturbeObjectInplane(obj, sigma_T, sigma_R):
B = createRandomNoiseInplane(sigma_R, sigma_T)
M = ga_exp(B) * one
return M * obj * ~M
def perturbeObject(obj, sigma_T, sigma_R):
B = createRandomNoiseBivector(sigma_R, sigma_T)
M = ga_exp(B) * one
return M * obj * ~M
def plotCostFunctionEffect():
print("\nRunning plotCostFunctionEffect")
print("")
np.random.seed(1)
#Test extreme values
np.random.seed(2)
O1 = up(0)
rot_scale = 1
tran_scale = 10
spread_scale = 10
B = 0.1 * e12 + 0.2*e13 - 0.1 *e23 + rot_scale*(3*e1 -1*e2 + 2*e3)*ninf
R = ga_exp(B)
N = 10
lines = createRandomLines(N, scale = spread_scale)
for i in range(len(lines)):
lines[i] = Sandwich(lines[i], Translator(e3*tran_scale))
sigma_R_model = 0.01
sigma_T_model = 0.1
lines_perturbed = [perturbeObject(line, sigma_T_model, sigma_R_model) for line in lines] #Model noise
lines_img_d_real = [projectLineToPlane(line, R) for line in lines] #Real lines
sigma_R_image = 0.002
sigma_T_image = 0.01
lines_img_d = [perturbeObjectInplane(projectLineToPlane(line, R), sigma_R_image, sigma_T_image) for line in lines]
mapping = BivectorLineImageMapping
x0 = mapping.inverserotorconversion(R)
x_test = x0[0]
y_test = x0[3]
N_rot = 50
N_tran = 50
rot_range = 0.4
translation_range = 10
rotation = np.linspace(-rot_range, rot_range, N_rot)
translation = np.linspace(-translation_range, translation_range, N_tran)
ans = np.zeros((N_rot, N_tran))
for i, rot in enumerate(rotation):
for j, tran in enumerate(translation):
x0[0] = x_test + tran
x0[3] = y_test + rot
ans[i, j] = np.log(mapping.costfunction(mapping.rotorconversion(x0), lines_perturbed, lines_img_d, O1))
xv, yv = np.meshgrid(translation, rotation)
fig = plt.figure()
ax = fig.add_subplot(111, projection='3d')
ax.set_xlabel("Translation error")
ax.set_ylabel("Rotation error")
ax.set_zlabel("log(objective function)")
ax.plot_wireframe(xv, yv, ans)
plt.show()
def testPlotProjections(self):
np.random.seed(2)
#A, B = createRandomPoints(2, 100) #Real points
#L = createLine(A, B) #Real line
O1 = up(0)
F1 = up(e3) #Image origin
Q1 = up(e3 + e2) #Defines image rotation
#O1 = up(3*e1 + 4*e2)
#cPlane1 = createRandomPlane(2)
B = 0.1 * e12 + 0.2*e13 + 0.1 *e23 + 1*(3*e1 -1*e2 + 2*e3)*ninf
#x0 = np.array([0.54, 0.85, 0.29, 1*3.1, -1.4 * 1, 1*1.89]) #Close to the real answer
N = 10
R = ga_exp(B)
O2 = R * O1 * ~R #O2
F2 = R * F1 * ~R
Q2 = R * Q1 * ~R
cPlane1 = (ninf + e3)*I5 #Camera plane 1
cPlane2 = R * cPlane1 * ~R
lines = createRandomLines(N, scale = 2)
for i in range(len(lines)):
lines[i] = Sandwich(lines[i], Translator(e3*3))
sigma_R_model = 0.01
sigma_T_model = 0.05
lines_perturbed = [perturbeObject(line, sigma_T_model, sigma_R_model) for line in lines] #Model noise
lines_img_d_real = [projectLineToPlane(line, R) for line in lines] #Real lines
sigma_R_image = 0.0001
sigma_T_image = 0.0001
lines_img_d = [perturbeObjectInplane(projectLineToPlane(line, R), sigma_R_image, sigma_T_image) for line in lines]
print("")
print("Inital cost", sumImageFunction(R, lines_perturbed, lines_img_d))
print("R_real: ", R)
R_min, Nint = minimizeError((lines_perturbed, lines_img_d), BivectorLineImageMapping, x0 = None)
print("R_min: ", R_min)
print("Nint = ", Nint)
print("Final cost= ", sumImageFunction(R_min, lines, lines_img_d))
lines_img_d_min = [projectLineToPlane(line, R_min) for line in lines]
lines_img_d_model = [projectLineToPlane(line, R_min) for line in lines_perturbed]
#Printing
color_print = ['m', 'y', 'k']
N_print = len(color_print)
plot_img = Plot2D()
for i in range(N_print):
Limg = lines_img_d[i]
Limg_min = lines_img_d_min[i]
Limg_real = lines_img_d_real[i]
Limg_model = lines_img_d_model[i]
plot_img.plotLine2D(Limg_min, color = 'g') #Green: estimate of the real line (hidden)
plot_img.plotLine2D(Limg_model, color = 'c') #Cyan: estimate of model line
plot_img.plotLine2D(Limg, color = 'b') #Blue: image (with image noise)
plot_img.plotLine2D(Limg_real, color = color_print[i]) #Other: real line (hidden)
plot = Plot3D()
plot.configure(5)
plot.addPoint(O1, color='r')
plot.addPoint(O2, color='b')
#plot.addPoint(F1, color='r')
plot.addPoint(F2, color='b')
#plot.addPoint(Q1, color='r')
plot.addPoint(Q2, color='b')
for i in range(N_print):
L = lines[i]
L_img = R*lines_img_d_real[i]*~R
L_perturbed = lines_perturbed[i]
plot.addLine(L_perturbed, color = 'c')
plot.addLine(L_img, color = color_print[i])
plot.addLine(L, color = color_print[i])
#plot.addPlane(cPlane1, center = F1, color='r')
plot.addPlane(cPlane2, center = F2, color='b')
plot_img.show(block = False)
plot.show(block = False)
def testLogarithm(self):
verbose = False
if verbose:
print("\nTest Logarithms and exponents")
phi = 0.5 #Rotation amount
P = (e12 + 2 * e23 + 3 * e13).normal() #Rotation Plane
P_n = P * I3
t = 2.73 * e1 + 3.14 * e2 #Translation vector
t_nor = (P_n | t) * P_n #Decomposition into normal component
t_par = t - t_nor #Decomposition into paralel component
assert (t_par + t_nor == t)
if verbose:
print("P = ", P)
print("phi = ", phi)
print("t = ", t)
print("t_nor = ", t_nor)
print("t_par = ", t_par)
print("")
assert (P | t_nor == 0) #Normal to P
assert (P ^ t_nor != 0) #Normal to P
assert (P | t_par != 0) #Parallel to P
assert (P ^ t_par == 0) #Parallel to P
assert (P * t != 0) #Non zero product
R_expected = (np.cos(phi) +
(np.sin(phi) * P)) * (1 + (t_nor * ninf)) + np.sinc(
phi / np.pi) * t_par * ninf
B_expected = phi * P + t * ninf
R_exponential = np.exp(B_expected)
R_actual = ga_exp(B_expected, verbose=verbose)
B_new = ga_log(R_expected, verbose=verbose)
R_ga = ga_exp(B_new)
if verbose:
print("R_old ", R_expected)
print("R_expected ", R_actual)
print("R_exponential", R_exponential)
print("R_ga ", R_ga)
print("B_new ", B_new)
print("B_expected ", B_expected)
#Rotor properties
AssertMVEqual(R_expected * ~R_expected, 1, verbose=verbose)
AssertMVEqual(R_ga * ~R_ga, 1, verbose=verbose)
#Equalities
AssertMVEqual(R_actual, R_expected, verbose=verbose)
AssertMVEqual(R_exponential, R_expected, verbose=verbose)
AssertMVEqual(B_new, B_expected, verbose=verbose)
AssertMVEqual(R_ga, R_expected, verbose=verbose)
N = 100
#Random bivectors to test this as well
for i in range(N):
B = createRandomBivector()
AssertMVEqual(B,
ga_log(ga_exp(B, verbose=verbose), verbose=verbose),
verbose=verbose)
