image_height = 320
points = np.transpose(
np.array(list(map(lambda x: list(x), list(zip(X, Y, Z, H))))))
spheres = Geometry.generate_spheres(points)
camera = Camera.normalized_camera(0, 0, image_width / 2, image_height / 2)
camera_translated = Camera.normalized_camera(5, 0, image_width / 2,
image_height / 2)
##############
points_persp = camera.apply_perspective_pipeline(points)
(X_orig, Y_orig, Z_orig) = list(Utils.points_into_components(points))
(X_persp, Y_persp, Z_persp) = list(Utils.points_into_components(points_persp))
##############
scene = Scene.Scene(image_width, image_height, spheres, camera)
scene.render()
frame_buffer_image = ImageProcessing.normalize_to_image_space(
scene.frame_buffer)
depth_buffer_image = scene.depth_buffer
cv2.imwrite("framebuffer.png", frame_buffer_image)
cv2.imwrite("depthbuffer.png", depth_buffer_image)
import Numerics.Generator as Generator
import Visualization.Plot3D as Plot3D
import numpy as np
import Numerics.Utils as NumUtils
(X, Y, Z) = Generator.generate_3d_plane(1, 1, -30, 20, 4)
H = np.repeat(1, 20)
points = np.transpose(
np.array(list(map(lambda x: list(x), list(zip(X, Y, Z, H))))))
SE3 = Generator.generate_random_se3(-1, 1, 4)
rotated_points = np.matmul(SE3, points)
(X_new, Y_new, Z_new) = list(NumUtils.points_into_components(rotated_points))
#Plot3D.scatter_plot(X_new,Y_new,Z_new)
Plot3D.scatter_plot([(X, Y, Z), (X_new, Y_new, Z_new)],
['original', 'perturbed'])
import Numerics.Generator as Generator
import Visualization.Plot3D as Plot3D
import numpy as np
import Numerics.Utils as NumUtils
import VisualOdometry.Solver as Solver
from math import pi
N = 100
(X, Y, Z) = Generator.generate_3d_plane(1, 1, -10, N, 4)
H = np.repeat(1, N)
points = np.transpose(
np.array(list(map(lambda x: list(x), list(zip(X, Y, Z, H))))))
SE3 = Generator.generate_random_se3(-10, 10, 4, pi / 2, 0, pi / 2)
rotated_points_gt = np.matmul(SE3, points)
(X_gt, Y_gt, Z_gt) = list(NumUtils.points_into_components(rotated_points_gt))
SE3_est = Solver.solve_SE3(points, rotated_points_gt, 20000, 0.01)
rotated_points_est = np.matmul(SE3_est, points)
(X_est, Y_est,
Z_est) = list(NumUtils.points_into_components(rotated_points_est))
Plot3D.scatter_plot([(X, Y, Z), (X_gt, Y_gt, Z_gt), (X_est, Y_est, Z_est)],
['original', 'ground truth', 'estimate'])
N = 20
(X, Y, Z) = Generator.generate_3d_plane(1, 1, -30, N, 4)
H = np.repeat(1, N)
points = np.transpose(
np.array(list(map(lambda x: list(x), list(zip(X, Y, Z, H))))))
SE3 = Generator.generate_random_se3(-5, 5, pi / 18, pi / 10, 0, pi / 10)
perturbed_points_gt = np.matmul(SE3, points)
camera = Camera.normalized_camera()
points_persp = camera.apply_perspective_pipeline(points)
#SE3_est = Solver.solve_SE3(points,perturbed_points_gt,20000,0.01)
#perturbed_points_est = np.matmul(SE3_est,points)
(X_orig, Y_orig, Z_orig) = list(Utils.points_into_components(points))
(X_new, Y_new, Z_new) = list(Utils.points_into_components(perturbed_points_gt))
#(X_est,Y_est,Z_est) = list(Utils.points_into_components(perturbed_points_est))
(X_persp, Y_persp, Z_persp) = list(Utils.points_into_components(points_persp))
#Plot3D.scatter_plot([(X_orig,Y_orig,Z_orig),(X_new,Y_new,Z_new),(X_est,Y_est,Z_est)],['original','perturbed','estimate'])
Plot3D.scatter_plot_sub([(X_orig, Y_orig, Z_orig)],
[(X_persp, Y_persp, Z_persp)], ['original'],
['projected'])