def find_match(feature, feature_list, ratio_threshold=0.7):
'''
@brief Find the best match for feature in feature_list. Indicates if the
match is valid
@param feature The feature to be matched
@param feature_list A list of potential matches
@param ratio_threshold The maximum allowable ratio between the nearest
neighbour descriptor distance and second nearest
neighbour descriptor distance for the match to be
kept. Recommended values beteween 0.6 and 0.8
@return is_valid A flag indicating whether the match is valid
@return best The best feature match from feature_list
'''
for potential_match in feature_list:
if potential_match.image_idx != feature_list[0].image_idx:
raise ValueError("Features must be from the same image")
# copy list to not change it
potential_matches = feature_list[:]
# sort index list based on distance
potential_matches.sort(
key=lambda potential_match: linalg.get_euclidean_distance(feature.descriptor, potential_match.descriptor)
)
best, second_best = potential_matches[:2]
# check if match is valid
min_dist = linalg.get_euclidean_distance(best.descriptor, feature.descriptor)
next_dist = linalg.get_euclidean_distance(second_best.descriptor, feature.descriptor)
valid_match = min_dist / next_dist < ratio_threshold
return (valid_match, best)
def sort_outer_corners(image, corners):
'''
@brief Sorts the outer x-corners in a projection invariant order. The
corners are sorted in a clockwise order with a consistent choice
for the outer corner that marks "cluster 0"
@param image Image used for analysis (in grayscale)
@param corners The computed pixel coordinates of the outer x-corners.
These should already be sorted in clockwise order as
[top_left, top_right, bottom_right, bottom_left]
@return The ordered outer corner array
'''
# decide whether the side pointing towards the top-left corner is the
# longer side of the board or not
ab_distance = linalg.get_euclidean_distance(corners[0], corners[1])
ac_distance = linalg.get_euclidean_distance(corners[1], corners[2])
long_side_up = ab_distance > ac_distance
# find centroid of outer-corners
centroid_point = linalg.compute_point_group_centroid(corners)
if long_side_up:
# if long side is pointing to top-left, we are interested to see if
# top left corner has a black square in the center of its cluster
corner_of_interest = corners[0]
# find the colour of the square in the center of the cluster
movement_direction = linalg.unit_vect(corner_of_interest -
centroid_point)
check_point = (corner_of_interest -
25 * movement_direction).astype(int)
if image[check_point[0], check_point[1]] < 128:
# central cluster square is black, corner[0] marks "cluster 0"
return corners
else:
# central cluster square is white, corner[2] marks "cluster 0"
return np.roll(corners, -4)
else:
# if long side isn't pointing to top-left, we are interested to see if
# top right corner has a black square in the center of its cluster
corner_of_interest = corners[1]
# find the colour of the square in the center of the cluster
movement_direction = linalg.unit_vect(corner_of_interest -
centroid_point)
check_point = (corner_of_interest -
25 * movement_direction).astype(int)
if image[check_point[0], check_point[1]] < 128:
# central cluster square is black, corner[1] marks "cluster 0"
return np.roll(corners, -2)
else:
# central cluster square is white, corner[3] marks "cluster 0"
return np.roll(corners, -6)
def get_squared_error_sum(extrinsic_params, k, world_points, image_points):
'''
@brief Computes the sum of squared projection errors.
@param extrinsic_params The set of extrinsic calibration params in a (6,)
shape ndarray. The array should be formatted as
[alpha, beta, gamma, tx, ty, tz]
@param k The 3x3 intrinsic calibration parameter matrix
@param world_points The coordinates of the x-corner 3D world points
@param image_points The actual 2D image points of detected x-corners
@return The sum of squared projection errors as a float
'''
# extrinsic calibration matrix
G = reconstruct_extrinsic_matrix_from_parameters(extrinsic_params)
# projection matrix
P = (k @ G)
expected_image_points = [P @ world_point for world_point in world_points]
# convert from homogeneous coordinates
expected_image_points = [
point[:2, 0] / point[2] for point in expected_image_points
]
squared_error = [
linalg.get_euclidean_distance(expected_image_point, image_point)**2
for (expected_image_point,
image_point) in zip(expected_image_points, image_points)
]
return sum(squared_error)
def compute_mean_reprojection_error(reconstructed_point, feature_group,
P_mats):
'''
@brief Computes the of mean reprojection error of a single reconstructed
point
@param reconstructed_point The reconstructed point
@param feature_groups The feature group corresponding to the
reconstructed point
@param P_mats The projection matrices for each image,
ordered by the image index
@return The mean reprojection error of the
reconstructed point
'''
total_error = 0
for feature in feature_group:
# compute reprojection
P = P_mats[feature.image_idx]
X = np.append(reconstructed_point, [1], axis=0).reshape(4, 1)
reprojection_homogeneous = P @ X
reprojection = reprojection_homogeneous[:2,
0] / reprojection_homogeneous[
2, 0]
# get original image point
image_point = np.array(feature.coordinates)
total_error += linalg.get_euclidean_distance(reprojection, image_point)
return total_error / len(feature_group)
def find_k_nearest_neighbours(point, neighbours, k):
'''
@brief Finds a list of kth nearest neighbours based on a euclidean
distance criteria
@param point The coordinates of the point for which nearest
neighbours should be found
@param neighbours A list of points to search in. This could but is not
required to include "point"
@param k The number of nearest neighbours to return
@return returns a list of the k-nearest neighbours
'''
# copy list to not mutate input
neighbour_list = neighbours[:]
# sort index list based on distance
neighbour_list.sort(
key=lambda neighbour: linalg.get_euclidean_distance(point, neighbour))
# check if point is included in the neighbour list
point_in_neighbour_list = any(
np.array_equal(point, neighbour) for neighbour in neighbour_list)
# return the k nearest neighbours excluding the point itself
return neighbour_list[1:k +
1] if point_in_neighbour_list else neighbour_list[:k]
def find_high_error_proj_mat_indices(image_points,
P_mats,
mean_error_threshold=10,
max_error_threshold=20,
length=9,
width=6,
square_size=1.9):
'''
@brief Uses a nonlinear least squares (NLS) method to compute the
extrinsic camera calibration matrix
@param image_points The actual 2D image points of detected
x-corners
@param P_mats The 3x4 camera projection matrix
(P= K[R | t])
@param mean_error_threshold The maximum allowable mean projection error
@param max_error_threshold The maximum allowable max projection error
@param length The number of x-corners along the longer side
before the middle section is removed (for the
image provided in images/chess_pattern.png,
the length is 9)
@param width The number of x-corners along the shorter side
before the middle section is removed (for the
image provided in images/chess_pattern.png,
the length is 6)
@param square_size The side length, in cm, of the chessboard
square
@return The computed 3x4 extrinsic calibration matrix
'''
x_corners = get_world_points(length, width, square_size)
# project x corners using projection matrix
projections = [[P @ vec for vec in x_corners] for P in P_mats]
# convert from homogeneous coordinates
projections = [[point[:2].reshape(2, ) / point[2] for point in points]
for points in projections]
# compute mean and max projection errors for every projection matrix
projection_errors = [[
linalg.get_euclidean_distance(proj, point)
for (proj, point) in zip(proj_points, points)
] for (proj_points, points) in zip(projections, image_points)]
mean_errors = [np.mean(error_dist) for error_dist in projection_errors]
max_errors = [np.max(error_dist) for error_dist in projection_errors]
# filter based on thresholds
return [
i for i in range(len(P_mats) - 1, -1, -1)
if mean_errors[i] > mean_error_threshold
or max_errors[i] > max_error_threshold
]
def filter_corner_distance(corner_mask, distance_threshold, num_neighbours=3):
'''
@brief Filters the boolean corner mask to remove points that do not have
N neighbours within some threshold distance
@param corner_mask A boolean-valued mask indicating the location
of detected x-corners
@param distance_theshold The distance threshold used by the filter
@param num_neighbours The number of neighbours required to be within
the distance threshold. For x-corners this
should be 3
@return The filtered boolean corner mask
'''
x = np.where(corner_mask)[1]
y = np.where(corner_mask)[0]
corner_indices = [np.array([y_val, x_val]) for (y_val, x_val) in zip(y, x)]
nearest_neighbours = [
find_k_nearest_neighbours(index_pair, corner_indices, num_neighbours)
for index_pair in corner_indices
]
# create copy of amsk to not mutate input
outmask = corner_mask[:]
# eliminate corners not matching the distance criteria
for index_pair, neighbours in zip(corner_indices, nearest_neighbours):
distances_above_threshold = [
linalg.get_euclidean_distance(index_pair, neighbour) >
distance_threshold for neighbour in neighbours
]
above_thresh_exists = reduce(lambda a, b: a or b,
distances_above_threshold)
if above_thresh_exists:
outmask[index_pair[0], index_pair[1]] = False
return outmask
def sort_x_corners(corner_mask, image, d):
'''
@brief Sorts x-corners in the list according to a projection invariant
order
@param corner_mask A boolean-valued mask indicating the location of
detected x-corners
@param image Image used for analysis (in grayscale)
@param d Distance threshold for determining whether outer
corners were detected correctly
@return Sorted list of x-corners
'''
x = np.where(corner_mask)[1]
y = np.where(corner_mask)[0]
corner_list = [np.array([y_val, x_val]) for (y_val, x_val) in zip(y, x)]
# case 2 - image is aligned so points are extrema of x-y and x+y
# try this approach and see if it fails
sum_arr = x + y
diff_arr = y - x
A_index = np.argmin(sum_arr)
B_index = np.argmin(diff_arr)
C_index = np.argmax(sum_arr)
D_index = np.argmax(diff_arr)
A = np.array([y[A_index], x[A_index]])
B = np.array([y[B_index], x[B_index]])
C = np.array([y[C_index], x[C_index]])
D = np.array([y[D_index], x[D_index]])
sorted_corners_of_interest = sort_outer_corners(image, [A, B, C, D])
is_succ = True
for i in range(len(sorted_corners_of_interest)):
for j in range(len(sorted_corners_of_interest)):
if i == j:
continue
corner1 = sorted_corners_of_interest[i]
corner2 = sorted_corners_of_interest[j]
if linalg.get_euclidean_distance(corner1, corner2) < d:
is_succ = False
if not is_succ:
# case 2 - image is tilted so points are extrema of x and y
A_index = np.argmin(x)
B_index = np.argmin(y)
C_index = np.argmax(x)
D_index = np.argmax(y)
A = np.array([y[A_index], x[A_index]])
B = np.array([y[B_index], x[B_index]])
C = np.array([y[C_index], x[C_index]])
D = np.array([y[D_index], x[D_index]])
# check whether this is correct
sorted_corners_of_interest = sort_outer_corners(image, [A, B, C, D])
sorted_point_list = []
for corner in sorted_corners_of_interest:
neighbours = find_k_nearest_neighbours(corner, corner_list, 3)
sorted_point_list += sort_corner_neighbourhood(corner, neighbours)
return sorted_point_list