def testBenchmarkMinimizeErrorPlot(self):
fileprint = False
sigma_R = 0.005
sigma_T = 0.007
N_training = 30
N_plot = 4
line1, line2 = createRandomLines(2)
R_real = RotorLine2Line(line1, line2)
traininglinesets = createNoisyLineSet(R_real, sigma_R, sigma_T,
N_training)
R_min, nit = minimizeError(traininglinesets, RotorLineMapping)
validationlines = createRandomLines(N_plot)
plot = Plot3D()
for line in validationlines:
line_real = R_real * line * ~R_real
line_est = R_min * line * ~R_min
plot.addLine(line_real)
plot.addLine(line_est)
if fileprint:
timestring = time.strftime("%Y%m%d-%H%M%S")
figname = "../benchmarkreports/plot_%s.png" % timestring
plot.save(figname)
def testNoisyRotationExtendedExtraction(self):
verbose = True
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 + 0.1 * E0
R = np.exp(B)
N = 100
lines = createRandomLines(N, scale = 2)
for i in range(len(lines)):
lines[i] = Sandwich(lines[i], Translator(e3*3))
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]
#using our noisy model and the noisy image of them to estimate R
R_min, Nint = minimizeError((lines_perturbed, lines_img_d), ExtendedBivectorLineImageMapping , 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 generate_data():
seed = 31
N = 20
for sigma in [
0.0001, 0.0005, 0.0008, 0.001, 0.005, 0.008, 0.01, 0.05, 0.08, 0.1
]:
#for N in [10, 15, 20, 50, 70, 100, 150, 200]:
for _ in range(10):
line1, line2 = createRandomLines(2)
R_true = RotorLine2Line(line1, line2)
traininglinesets = createNoisyLineSet(R_true, sigma, sigma, N)
try:
R_eduardo = estimate_dual_quaternions(traininglinesets)
Rc_eduardo = bivector_difference_rotor(R_true, R_eduardo)
x0 = BivectorLineMapping.inverserotorconversion(R_eduardo)
R_min, nit_boost = minimizeError(traininglinesets,
mapping=BivectorLineMapping,
x0=None)
Rc_min = bivector_difference_rotor(R_true, R_min)
print("%.4f, %d, %.4e, %.4e, %.4e, %.4e" %
(sigma, N, rotation_cost(Rc_eduardo),
translation_cost(Rc_eduardo), rotation_cost(Rc_min),
translation_cost(Rc_min)))
except:
print()
continue
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 updateLocation(self):
print("Update Location")
for i in range(len(self.lines_imgs)):
args = (self.model_estimate, self.lines_imgs[i])
if (self.R_estimate[i] is None):
x0 = None
else:
x0 = self.mapping.inverserotorconversion(self.R_estimate[i])
R_min, N_int = minimizeError(args, self.mapping, x0 = x0)
self.R_estimate[i] = R_min
print("N_int = ", N_int)
print("Complete: Update location")
def testLinePointError(self):
print("\nWARNING: SLOW")
#Testing how using the distance from certain points on a line as a good external metric of how well we are performing.
np.random.seed(10)
sigma_R = 0.05
sigma_T = 0.07
N = 20
line1, line2 = createRandomLines(2)
R_real = RotorLine2Line(line1, line2)
traininglinesets = createNoisyLineSet(R_real, sigma_R, sigma_T, N)
#Test various minimazation algorithms
R_rotor, nit = minimizeError(traininglinesets, RotorLineMapping)
R_bivector, nit = minimizeError(traininglinesets, BivectorLineMapping)
R_lineproduct, nit = minimizeError(traininglinesets,
LinePropertyBivectorMapping)
R_dummy = RotorLine2Line(
traininglinesets[0][0], traininglinesets[0]
[1]) #comparing it to just taking the first one we find
R_list = [R_rotor, R_bivector, R_lineproduct, R_dummy]
costs = []
N_val = 10
N_points = 4
for R in R_list:
costs.append(linePointCostMetric(R, R_real, N_val=N_val))
print("rotor_pointcost ", costs[0])
print("bivector_pointcost ", costs[1])
print("lineproduct_pointcost ", costs[2])
print("dummy_pointcost ", costs[3])
def testLineAveraging(self):
seed = 1
sigma_T = 0.005
sigma_R = 0.002
N = 100
mapping = BivectorLineMapping
line_start, line_target = createRandomLines(2)
R_real_min, N_int = minimizeError([(line_start, line_target)],
mapping=BivectorLineMapping)
R_real = RotorLine2Line(line_start, line_target)
print("R_real ", R_real)
print("R_real_min", R_real_min)
print("B_real_min", ga_log(R_real_min))
print("B_real ", ga_log(R_real))
print("L_real_min", R_real_min * line_start * ~R_real_min)
traingingdata = [
line_start,
[perturbeObject(line_target, sigma_R, sigma_T) for _ in range(N)]
]
validationdata = [
line_start,
[perturbeObject(line_target, sigma_R, sigma_T) for _ in range(N)]
]
print(
"Training and validation sets created with sig_r = %f and sig_t = %f, N = %d"
% (sigma_R, sigma_T, N))
map_list = [BivectorLineEstimationMapping]
for map_obj in map_list:
np.random.seed(seed)
print(map_obj.name)
realtrainingcost, minimumvalidationcost, R_min = benchmarkMinimizeError(
R_real,
traingingdata,
validationdata,
N=N,
fileout=None,
mapping=map_obj)
print("L_real = ", line_target)
print("L_min = ", R_min * line_start * ~R_min)
print("L_example= ", validationdata[0])
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 calibrate(model, image_model):
lines_ga = []
lines_img_ga = []
dict_lines_ga = model.get_lines_ga()
for name, line in image_model.get_lines_img_ga().items():
lines_img_ga.append(line)
lines_ga.append(dict_lines_ga[name])
t0 = time.clock()
R_min, Nint = minimizeError((lines_ga, lines_img_ga),
BivectorLineImageMapping,
x0=None)
print("t = ", time.clock() - t0, "N_iter = ", Nint)
return R_min
def multiViewEstimation(self, mapping, N, K, scale = None):
np.random.seed(1)
R_list, lines, lines_img_base_d, lines_img_base_d_real , lines_imgs_d, lines_imgs_d_real = self.setUpMultiView(N, K, sigma_R_image = 0.01, sigma_T_image = 0.01)
if scale:
dx = np.random.normal(size=6*K, scale=scale) #Starting estimate is slightly off
x0 = mapping.inverserotorconversion(R_list) + dx #Add the noise
R_list_start = mapping.rotorconversion(x0) #Recover rotor
else:
x0 = None
print("No noise cost =", mapping.costfunction(R_list, lines_img_base_d_real, lines_imgs_d_real, verbose = False))
print("Target cost =", mapping.costfunction(R_list, lines_img_base_d, lines_imgs_d, verbose = False))
print("Start cost =", mapping.costfunction(R_list_start, lines_img_base_d, lines_imgs_d, verbose = False))
#Computation
args = (lines_img_base_d, lines_imgs_d)
R_list_min, Nint = minimizeError(args, mapping, x0 = x0)
#Output
print("Nint = ", Nint)
for i in range(K):
print("")
print("R_real : ", R_list[i])
print("R_min : ", R_list_min[i])
print("R_start: ", R_list_start[i])
print("")
print("Start cost =", mapping.costfunction(R_list_start, lines_img_base_d, lines_imgs_d, verbose = False))
print("Final cost =", mapping.costfunction(R_list_min, lines_img_base_d, lines_imgs_d, verbose = False))
print("Target cost =", mapping.costfunction(R_list, lines_img_base_d, lines_imgs_d, verbose = False))
print("No noise cost =", mapping.costfunction(R_list, lines_img_base_d_real, lines_imgs_d_real, verbose = False))
def averageLines(self, line_start, line_guesses):
mapping = BivectorLineEstimationMapping
args = [line_start, line_guesses]
x0 = np.random.normal(0.01, size=6)
R_min, Nint = minimizeError(args, mapping, x0 = x0)
return R_min * line_start * ~R_min
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 testWeigthingFunction(self):
print("\nRunning testWeigthingFunction")
print("")
#np.random.seed(5)
#Test on some extreme values
sigma_R = 0.01
sigma_T = 0.1
N_train = 100
N_val = 20
line_scale = 10
translation_scale = 100
line1, line2 = createRandomLines(2)
a = createRandomVector(scale=translation_scale)
print("Translated lineA by ", a)
b = createRandomVector(scale=translation_scale)
print("Translated lineB by ", b)
T_a = Translator(a)
T_b = Translator(b)
#Move them far away from the origin
lineA = T_a * line1 * ~T_a
lineB = T_b * line2 * ~T_b
R_real = RotorLine2Line(lineA, lineB)
#x0 = BivectorWeightedLineMapping.inverserotorconversion(R_real)
#x0 += np.array([15, 21, -11, 0.1, -0.1, 0.21])
#R_start = BivectorWeightedLineMapping.
#R_start = BivectorWeightedLineMapping.rotorconversion(x0)
traininglinesets = createNoisyLineSet(R_real,
sigma_R,
sigma_T,
N_train,
scale=line_scale)
validationlinesets = createNoisyLineSet(R_real,
sigma_R,
sigma_T,
N_val,
scale=line_scale)
mapping = BivectorWeightedLineMapping
weightList = [1e-4, 1e-2, 1, 1e2, 1e4]
#mappingList = [BivectorLineMapping, LinePropertyBivectorMapping, BivectorLogCostLineMapping]
plot = Plot3D()
testline = validationlinesets[0][0]
plot.addLine(R_real * testline * ~R_real, color='r')
for weight in weightList:
t0 = time.time()
print("Running %s" % mapping.name)
print("Weight = %e" % weight)
#Test various minimazation algorithms
mapping = BivectorWeightedLineMapping
mapping.costfunction = sumWeightedLineSquaredErrorCost(weight)
R_min, nit = minimizeError(traininglinesets, mapping, x0=None)
#mapping.costfunction = logSumWeightedLineSquaredErrorCost(weight)
#Finding the cost if we used the actual rotor used to generate the matrix
realtrainingcost = mapping.costfunction(R_real, traininglinesets)
print("Real training cost is %s" % str(realtrainingcost))
realvalidationcost = mapping.costfunction(R_real,
validationlinesets)
print("Real validation cost is %s" % str(realvalidationcost))
minimumtrainingcost = mapping.costfunction(R_min, traininglinesets)
print("minimized training cost %f" % minimumtrainingcost)
minimumvalidationcost = mapping.costfunction(
R_min, validationlinesets)
print("minimized validation cost = %f" % minimumvalidationcost)
realpointcost = linePointCostMetric(R_real, R_min, 10)
print(
"Averaged cost for points a: %f, a + m: %f, a + 10m: %f, a + 100m: %f, a + 1000m: %f"
% tuple(realpointcost))
print("")
print("R_real= %s" % str(R_real / np.sign(float(R_real(0)))))
print("R_min = %s" % str(R_min / np.sign(float(R_min(0)))))
#print("R_start = %s" % str(R_start/np.sign(float(R_start(0)))))
B_real = ga_log(R_real / np.sign(float(R_real(0))))
B_min = ga_log(R_min / np.sign(float(R_min(0))))
#B_start = ga_log(R_start/np.sign(float(R_start(0))))
print("")
print("B_real= %s" % str(B_real))
print("B_min = %s" % str(B_min))
#print("B_start = %s" % str(B_start))
print("")
print("C(R(B_real - B_min)) = %f" %
rotorAbsCostFunction(ga_exp(B_min - B_real)))
plot.addLine(R_min * testline * ~R_min, color=mapping.color)
t_end = time.time()
print("")
print(
"Running time for extracting best rotor for %d line pairs is %f s"
% (N_train, t_end - t0))
print("\n\n")
plot.show(False)
def testExtendedLineRotation(self):
print("\nRunning testExtendedLineRotation")
print("")
np.random.seed(1)
#Test extreme values
sigma_R = 0.1
sigma_T = 1
N_train = 100
N_val = 20
line_scale = 40
mapping = ExtendedBivectorMapping
B_real = (0.21381 * e12) + (0.64143 * e13) + (2.73 * e14) + (
2.73 * e15) + (0.42762 * e23) + (3.14 * e24) + (3.14 *
e25) + (0.8 * e45)
R_real = ga_exp_complicated(B_real)
print(R_real)
traininglinesets = createNoisyLineSet(R_real,
sigma_R,
sigma_T,
N_train,
scale=line_scale)
validationlinesets = createNoisyLineSet(R_real,
sigma_R,
sigma_T,
N_val,
scale=line_scale)
plot = Plot3D()
testline = validationlinesets[0][0]
plot.addLine(R_real * testline * ~R_real, color='r')
t0 = time.time()
print("Running %s" % mapping.name)
#Test various minimazation algorithms
R_min, nit = minimizeError(traininglinesets, mapping, x0=None)
costfunction = mapping.costfunction
#Finding the cost if we used the actual rotor used to generate the matrix
realtrainingcost = costfunction(R_real, traininglinesets)
print("Real training cost is %s" % str(realtrainingcost))
realvalidationcost = costfunction(R_real, validationlinesets)
print("Real validation cost is %s" % str(realvalidationcost))
minimumtrainingcost = costfunction(R_min, traininglinesets)
print("minimized training cost %f" % minimumtrainingcost)
minimumvalidationcost = costfunction(R_min, validationlinesets)
print("minimized validation cost = %f" % minimumvalidationcost)
realpointcost = linePointCostMetric(R_real, R_min, 10)
print(
"Averaged cost for points a: %f, a + m: %f, a + 10m: %f, a + 100m: %f, a + 1000m: %f"
% tuple(realpointcost))
print("")
print("R_real= %s" % str(R_real / np.sign(float(R_real(0)))))
print("R_min = %s" % str(R_min / np.sign(float(R_min(0)))))
B_real = ga_log(R_real / np.sign(float(R_real(0))))
B_min = ga_log(R_min / np.sign(float(R_min(0))))
print("B_real= %s" % str(B_real))
print("B_min = %s" % str(B_min))
print("")
print("C(R(B_real - B_min)) = %f" %
rotorAbsCostFunction(ga_exp(B_min - B_real)))
plot.addLine(R_min * testline * ~R_min, color=mapping.color)
t_end = time.time()
print("")
print(
"Running time for extracting best rotor for %d line pairs is %f s"
% (N_train, t_end - t0))
print("\n\n")
plot.show(False)
def testExtremeLineRotation(self):
print("\nRunning testExtremeLineRotation")
print("")
np.random.seed(1)
#Test extreme values
sigma_R = 0.1
sigma_T = 1
N_train = 100
N_val = 20
line_scale = 1000
translation_scale = 1000
line1, line2 = createRandomLines(2)
a = createRandomVector(scale=translation_scale)
print("Translated lineA by ", a)
b = createRandomVector(scale=translation_scale)
print("Translated lineB by ", b)
T_a = Translator(a)
T_b = Translator(b)
#Move them far away from the origin
lineA = T_a * line1 * ~T_a
lineB = T_b * line2 * ~T_b
R_real = RotorLine2Line(lineA, lineB)
traininglinesets = createNoisyLineSet(R_real,
sigma_R,
sigma_T,
N_train,
scale=line_scale)
validationlinesets = createNoisyLineSet(R_real,
sigma_R,
sigma_T,
N_val,
scale=line_scale)
mappingList = [BivectorLineMapping]
x0 = BivectorLineMapping.inverserotorconversion(R_real)
x0 += np.array([0.1, 0.2, -0.1, 1, -0.1, 0.21])
plot = Plot3D()
testline = validationlinesets[0][0]
plot.addLine(R_real * testline * ~R_real, color='r')
for mapping in mappingList:
t0 = time.time()
print("Running %s" % mapping.name)
#Test various minimazation algorithms
R_min, nit = minimizeError(traininglinesets, mapping, x0=x0)
costfunction = mapping.costfunction
#Finding the cost if we used the actual rotor used to generate the matrix
realtrainingcost = costfunction(R_real, traininglinesets)
print("Real training cost is %s" % str(realtrainingcost))
realvalidationcost = costfunction(R_real, validationlinesets)
print("Real validation cost is %s" % str(realvalidationcost))
minimumtrainingcost = costfunction(R_min, traininglinesets)
print("minimized training cost %f" % minimumtrainingcost)
minimumvalidationcost = costfunction(R_min, validationlinesets)
print("minimized validation cost = %f" % minimumvalidationcost)
realpointcost = linePointCostMetric(R_real, R_min, 10)
print(
"Averaged cost for points a: %f, a + m: %f, a + 10m: %f, a + 100m: %f, a + 1000m: %f"
% tuple(realpointcost))
print("")
print("R_real= %s" % str(R_real / np.sign(float(R_real(0)))))
print("R_min = %s" % str(R_min / np.sign(float(R_min(0)))))
B_real = ga_log(R_real / np.sign(float(R_real(0))))
B_min = ga_log(R_min / np.sign(float(R_min(0))))
print("B_real= %s" % str(B_real))
print("B_min = %s" % str(B_min))
print("")
print("C(R(B_real - B_min)) = %f" %
rotorAbsCostFunction(ga_exp(B_min - B_real)))
plot.addLine(R_min * testline * ~R_min, color=mapping.color)
t_end = time.time()
print("")
print(
"Running time for extracting best rotor for %d line pairs is %f s"
% (N_train, t_end - t0))
print("\n\n")
plot.show(False)
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)