uz_assignments/assignment5/solution.py

import numpy as np
import numpy.typing as npt
from matplotlib import pyplot as plt
import cv2
import uz_framework.image as uz_image
import uz_framework.text as uz_text

def one_a() -> None:
	"""
	d = (f * px)/pz + f(T - px)/pz => d = (f * T) / pz
	"""

def one_b() -> None:
	"""
	Write a script that computes the disparity for a range of values of pz. Plot the values
	to a figure and set the appropriate units to axes. Use the following parameters of
	the system: focal length is f = 2.5mm and stereo system baseline is T = 12cm
	"""

	pz_values = np.arange(0.001, 0.1, 0.001)
	f = 0.0025 
	T = 0.12

	d = (f * T) / pz_values

	plt.plot(pz_values, d)

	plt.xlabel('pz [m]')
	plt.ylabel('d [m]')
	plt.title('Disparity for a range of values of pz')
	plt.show()

def one_c() -> None:
	"""
	In order to get a better grasp on the idea of distance and disparity, you will calculate
	the numbers for a specific case. We will take the parameters from a specification of
	a commercial stereo camera Bumblebee2 manufactured by the company PointGray:
	f = 2.5mm, T = 12cm, whose image sensor has a resolution of 648×488 pixels that
	are square and the width of a pixel is 7.4µm. We assume that there is no empty
	space between pixels and that both cameras are completely parallel and equal. Lets
	say that we use this system to observe a (point) object that is detected at pixel 550
	in x axis in the left camera and at the pixel 300 in the right camera. How far is the
	object (in meters) in this case? How far is the object if the object is detected at
	pixel 540 in the right camera? Solve this task analytically and bring your solution
	to the presentation of the exercise

	z1 = 0,16216m
	z2 = 0.402m	
	"""

def one_d() -> None:
	"""
	Write a script that calculates the disparity for an image pair. Use
	the images in the directory disparity. Since the images were pre-processed we can
	limit the search for the most similar pixel to the same row in the other image. Since
	just the image intensity carries too little information, we will instead compare small
	"""
	
	left_image  = uz_image.imread_gray('/home/gasperspagnolo/Documents/faks_git/uz_assignments/assignment5/data/disparity/office_left.png', uz_image.ImageType.float64)
	right_image = uz_image.imread_gray('/home/gasperspagnolo/Documents/faks_git/uz_assignments/assignment5/data/disparity/office_right.png', uz_image.ImageType.float64)

	d = uz_image.get_disparity(left_image, right_image)

	plt.imshow(d, cmap='gray')
	plt.show()


def two_b() -> None:
	"""
	mplement a function fundamental_matrix that is given a set of (at least) eight
	pairs of points from two images and computes the fundamental matrix using the
	eight-point algorithm.
	As the eight-point algorithm can be numerically unstable, it is usually not executed
	directly on given pairs of points. Instead, the input is first normalized by centering
	them to their centroid and scaling their positions so that the average distance to the
	centroid is √2. To achieve this, you can use the function normalize_points from
	the supplementary material.
	Extend the function fundamental_matrix so that it first normalizes the input point-
	set of the left camera (we get transformed points and the transformation matrix T1)
	and then transform the input point set of the right camera (we get the transformed
	points and the transformation matrix T2). Using the transformed points the algo-
	rithm computes fundamental matrix ˆF, then transforms it into the original space
	using both transformation matrices F = TT
	2 ˆFT1.
	Test your function for fundamental matrix estimation using ten correspondence
	pairs that you load from the file house_points.txt. The columns are formatted as
	follows: x1, y1, x2, y2, i.e. the first column contains the x-coordinates of the points for
	the first image etc. Compute the fundamental matrix F and for each point in each
	image calculate the corresponding epipolar line in the other image. You can draw the
	epipolar lines using draw_epiline from the supplementary material. According to
	epipolar geometry the corresponding epipolar line should pass through the point. As
	a testing reference the correct fundamental matrix is included in the supplementary
	material in file house_fundamental.txt
	"""

	points = np.loadtxt('./data/epipolar/house_points.txt')
	left_image  = uz_image.imread_gray('./data/epipolar/house1.jpg', uz_image.ImageType.float64)
	right_image = uz_image.imread_gray('./data/epipolar/house2.jpg', uz_image.ImageType.float64) 

	p1 = points[:, :2]
	p2 = points[:, 2:]

	lines_p1, lines_p2 = uz_image.get_epipolar_lines(p1, p2)

	plt.imshow(left_image, cmap='gray')
	for line, point in zip(lines_p1, p1):
		uz_image.draw_epiline(line, left_image.shape[0], left_image.shape[1])
		plt.plot(point[0], point[1], 'r', marker='o', markersize=10)
	
	plt.show()
	

	plt.imshow(right_image, cmap='gray')
	for line, point in zip(lines_p2, p2):
		uz_image.draw_epiline(line, right_image.shape[0], right_image.shape[1])
		plt.plot(point[0], point[1], 'r', marker='o', markersize=10)

	plt.show()

def two_c() -> None:
	"""
	We use the reprojection error as a quantitative measure of the quality of the estimated fundamental matrix.
	Write a function reprojection_error that calculates the reprojection error of a
	fundamental matrix F given two matching points. For each point, the function
	should calculate the corresponding epipolar line from the point’s match in the other	
	image, then calculate the perpendicular distance between the point and the line
	where a, b and c are the parameters of the epipolar line. Finally, the function should
	return the average of the two distances.
	Write a script that performs two tests: (1) compute the reprojection error for points
	p1 = [85, 233]T
	in the left image-plane and p2 = [67, 219]T
	in right image-plane using
	the fundamental matrix (the error should be approximately 0.15 pixels). (2) Load
	the points from the file house_points.txt and compute the average of symmetric
	reprojection errors for all pairs of points. If your calculation is correct, the average
	error should be approximately 0.33 pixels.
	"""
	points = np.loadtxt('./data/epipolar/house_points.txt')
	p1 = points[:, :2]
	p2 = points[:, 2:]
	F = uz_image.fundamential_matrix(p1, p2)
	print('Reprojection error for house points', uz_image.reprojection_error(F, np.array([[85, 233], [85, 233]]), np.array([[67, 219], [67, 219]])))
	print('Reprojection error for house points', uz_image.reprojection_error(F, p1, p2))

def three_a() -> None:
	"""
	Implement the function triangulate that accepts a set of correspondence pointsand a 
	pair of calibration matrices as an input and returns the triangulated 3Dpoints. 
	Test the triangulation on the ten points from the filehouse_points.txt.Visualize the result usingplt.plotorplt.scatter.
	Also plot the index of thepoint in 3D space (useplt.text) so the results will be easier to interpret. 
	Plot thepoints interactively, so that you can rotate the visualization.	
	"""
	points = np.loadtxt('./data/epipolar/house_points.txt')
	p1 = points[:, :2]
	p2 = points[:, 2:]

	projection_matrix1 = np.loadtxt('./data/epipolar/house1_camera.txt')
	projection_matrix2 = np.loadtxt('./data/epipolar/house2_camera.txt')

	triangulated_points = uz_image.triangulate(p1, p2, projection_matrix1, projection_matrix2)

	transformation_matrix = np.array([[-1, 0, 0], [0, 0, -1], [0, 1, 0]])
	
	triangulated_points = np.dot(transformation_matrix, triangulated_points.T).T
	
	fig, axs = plt.subplots(1, 3)

	axs[0].imshow(uz_image.imread_gray('./data/epipolar/house1.jpg', uz_image.ImageType.float64), cmap='gray')
	axs[0].scatter(p1[:, 0], p1[:, 1], c='r', marker='o', s=10)
	axs[0].set_title('Left image')

	axs[1].imshow(uz_image.imread_gray('./data/epipolar/house2.jpg', uz_image.ImageType.float64), cmap='gray')
	axs[1].scatter(p2[:, 0], p2[:, 1], c='r', marker='o', s=10)
	axs[1].set_title('Right image')

	axs[2] = fig.add_subplot(111, projection='3d')
	axs[2].scatter(triangulated_points[:, 0], triangulated_points[:, 1], triangulated_points[:, 2])
	for i, point in enumerate(triangulated_points):
		axs[2].text(point[0], point[1], point[2], i)

	plt.show()


def three_d():
	left_image = uz_image.imread_gray('./data/desk/DSC02638.JPG', uz_image.ImageType.float64)
	right_image = uz_image.imread_gray('./data/desk/DSC02639.JPG', uz_image.ImageType.float64)

	keypoints_left, keypoints_right = uz_image.find_matches(left_image, right_image)
	F, keypoints_left, keypoints_right = uz_image.ransac_fundamental(keypoints_left, keypoints_right, 1000, 1)

	lines_p1, lines_p2 = uz_image.get_epipolar_lines(keypoints_left, keypoints_right)

	plt.imshow(left_image, cmap='gray')
	for line, point in zip(lines_p1, keypoints_left):
		uz_image.draw_epiline(line, left_image.shape[0], left_image.shape[1])
		plt.plot(point[0], point[1], 'r', marker='o', markersize=10)
	
	plt.show()
	

	plt.imshow(right_image, cmap='gray')
	for line, point in zip(lines_p2, keypoints_right):
		uz_image.draw_epiline(line, right_image.shape[0], right_image.shape[1])
		plt.plot(point[0], point[1], 'r', marker='o', markersize=10)

	plt.show()


# ######## #
# SOLUTION #
# ######## #

def main():
	#one_d()
	three_d()
	#three_a()
	#two_c()
	#ex2()
	#ex3()

if __name__ == '__main__':
	main()