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 testBenchmarkLinesMinimizeError(self): seed = 21 sigma_T = 0.005 sigma_R = 0.002 N = 10 line1, line2 = createRandomLines(2) R_real = RotorLine2Line(line1, line2) traininglinesets = createNoisyLineSet(R_real, sigma_R, sigma_T, N) validationlinesets = createNoisyLineSet(R_real, sigma_R, sigma_T, N) print( "Training and validation sets created with sig_r = %f and sig_t = %f, N = %d" % (sigma_R, sigma_T, N)) map_list = [BivectorLineMapping] for map_obj in map_list: np.random.seed(seed) print(map_obj.name) benchmarkMinimizeError(R_real, traininglinesets, validationlinesets, fileout=None, mapping=map_obj)
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 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 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 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 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 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 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 plotCostFunctionEffect(): print("\nRunning plotCostFunctionEffect") print("") np.random.seed(1) N_train = 20 line_scale = 10 translation_scale = 10 #Test extreme values sigma_R = 0.01 sigma_T = 1 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) linesets = createNoisyLineSet(R_real, sigma_R, sigma_T, N_train, scale=line_scale) mapping = BivectorWeightedLineMapping mapping.costfunction = sumWeightedLineSquaredErrorCost(1. / translation_scale) x0 = mapping.inverserotorconversion(R_real) 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), linesets)) 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 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 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 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)