def pts_conversion_back_bbox(pts_array, bboxes_in, debug=True):
'''
convert pts in the cropped image to the pts in the original image
parameters:
bboxes_in: 1 X 4 numpy array, TLBR or TLWH format
pts_array: 2(3) x N numpy array, N should >= 1
'''
np_bboxes = safe_bbox(bboxes_in, debug=debug)
if debug:
assert is2dptsarray(pts_array) or is2dptsarray_occlusion(pts_array) or is2dptsarray_confidence(pts_array), 'the input points should have shape: 2 or 3 x num_pts vs %d x %s' % (pts_array.shape[0], pts_array.shape[1])
assert isbbox(np_bboxes), 'the input bounding box is not correct'
pts_out = pts_array.copy()
pts_out[0, :] = pts_array[0, :] + np_bboxes[0, 0]
pts_out[1, :] = pts_array[1, :] + np_bboxes[0, 1]
return pts_out
def anno_writer(pts_array,
pts_savepath,
num_pts=68,
anno_version=1,
debug=True):
'''
write the point array to a .pts file
parameter:
pts_array: 2 or 3 x num_pts numpy array
'''
if debug:
assert is_path_exists_or_creatable(
pts_savepath), 'the save path is not correct'
assert (
is2dptsarray(pts_array) or is2dptsarray_occlusion(pts_array)
or is2dptsarray_confidence(pts_array)
) and pts_array.shape[1] == num_pts, 'the input point is not correct'
with open(pts_savepath, 'w') as file:
file.write('version: %d\n' % anno_version)
file.write('n_points: %d\n' % num_pts)
file.write('{\n')
# main content
for pts_index in xrange(num_pts):
if is2dptsarray(pts_array):
file.write('%.3f %.3f %f\n' %
(pts_array[0, pts_index], pts_array[1, pts_index],
1.0)) # all visible
else:
file.write('%.3f %.3f %f\n' %
(pts_array[0, pts_index], pts_array[1, pts_index],
pts_array[2, pts_index]))
file.write('}')
file.close()
def visualize_pts_line(pts_array,
line_index_list,
method=2,
seed=0,
alpha=0.5,
vis_threshold=0.3,
pts_size=20,
line_size=10,
line_color_index=0,
fig=None,
ax=None,
save_path=None,
vis=False,
warning=True,
debug=True,
closefig=True):
'''
given a list of index, and a point array, to plot a set of points with line on it
parameters:
pts_array: 2(3) x num_pts
line_index_list: a list of index
method: 1: all points are connected, if some points are missing in the middle, just ignore that point and connect the two nearby points
2: if some points are missing in the middle of a line, the line is decomposed to sub-lines
vis_threshold: confidence to draw the points
'''
if debug:
assert is2dptsarray(pts_array) or is2dptsarray_occlusion(
pts_array) or is2dptsarray_confidence(
pts_array), 'input points are not correct'
assert islist(line_index_list), 'the list of index is not correct'
assert method in [1, 2], 'the plot method is not correct'
num_pts = pts_array.shape[1]
# expand the pts_array to 3 rows if the confidence row is not provided
if pts_array.shape[0] == 2:
pts_array = np.vstack((pts_array, np.ones((1, num_pts))))
fig, ax = get_fig_ax_helper(fig=fig, ax=ax)
np.random.seed(seed)
color_option = 'hsv'
if color_option == 'rgb': color_set_random = np.random.rand(3, num_pts)
elif color_option == 'hsv':
h_random = np.random.rand(num_pts, )
color_set_random = np.zeros((3, num_pts), dtype='float32')
for pts_index in range(num_pts):
color_set_random[:, pts_index] = colorsys.hsv_to_rgb(
h_random[pts_index], 1, 1)
line_color = color_set[line_color_index]
pts_line = pts_array[:, line_index_list]
if method == 1:
valid_pts_list = np.where(pts_line[2, :] > vis_threshold)[0].tolist()
pts_line_tmp = pts_line[:, valid_pts_list]
ax.plot(pts_line_tmp[0, :],
pts_line_tmp[1, :],
lw=line_size,
color=line_color,
alpha=alpha) # plot all lines
# plot all points
for pts_index in valid_pts_list:
pts_index_original = line_index_list[pts_index]
# ax.plot(pts_array[0, pts_index_original], pts_array[1, pts_index_original], 'o', color=color_set_big[pts_index_original % len(color_set_big)], alpha=alpha)
ax.plot(pts_array[0, pts_index_original],
pts_array[1, pts_index_original],
marker='o',
ms=pts_size,
lw=line_size,
color=color_set_random[:, pts_index],
alpha=alpha)
else:
not_valid_pts_list = np.where(
pts_line[2, :] < vis_threshold)[0].tolist()
if len(not_valid_pts_list) == 0: # all valid
ax.plot(pts_line[0, :],
pts_line[1, :],
lw=line_size,
color=line_color,
alpha=alpha)
# plot points
for pts_index in line_index_list:
# ax.plot(pts_array[0, pts_index], pts_array[1, pts_index], 'o', color=color_set_big[pts_index % len(color_set_big)], alpha=alpha)
ax.plot(pts_array[0, pts_index],
pts_array[1, pts_index],
marker='o',
ms=pts_size,
lw=line_size,
color=color_set_random[:, pts_index],
alpha=alpha)
else:
prev_index = 0
for not_valid_index in not_valid_pts_list:
plot_list = range(prev_index, not_valid_index)
pts_line_tmp = pts_line[:, plot_list]
ax.plot(pts_line_tmp[0, :],
pts_line_tmp[1, :],
lw=line_size,
color=line_color,
alpha=alpha)
# plot points
for pts_index in plot_list:
pts_index_original = line_index_list[pts_index]
ax.plot(pts_array[0, pts_index_original],
pts_array[1, pts_index_original],
marker='o',
ms=pts_size,
lw=line_size,
color=color_set_random[:, pts_index_original],
alpha=alpha)
# ax.plot(pts_array[0, pts_index_original], pts_array[1, pts_index_original], 'o', color=color_set_big[pts_index_original % len(color_set_big)], alpha=alpha)
prev_index = not_valid_index + 1
pts_line_tmp = pts_line[:, prev_index:]
ax.plot(pts_line_tmp[0, :],
pts_line_tmp[1, :],
lw=line_size,
color=line_color,
alpha=alpha) # plot last line
# plot last points
for pts_index in range(prev_index, pts_line.shape[1]):
pts_index_original = line_index_list[pts_index]
# ax.plot(pts_array[0, pts_index_original], pts_array[1, pts_index_original], 'o', color=color_set_big[pts_index_original % len(color_set_big)], alpha=alpha)
ax.plot(pts_array[0, pts_index_original],
pts_array[1, pts_index_original],
marker='o',
ms=pts_size,
lw=line_size,
color=color_set_random[:, pts_index_original],
alpha=alpha)
return save_vis_close_helper(fig=fig,
ax=ax,
vis=vis,
save_path=save_path,
warning=warning,
debug=debug,
closefig=closefig)
def visualize_pts_array(input_pts,
color_index=0,
pts_size=20,
label=False,
label_list=None,
label_size=20,
vis_threshold=0.3,
covariance=False,
plot_occl=False,
xlim=None,
ylim=None,
fig=None,
ax=None,
save_path=None,
vis=False,
warning=True,
debug=True,
closefig=True):
'''
plot keypoints with covariance ellipse
parameters:
pts_array: 2(3) x num_pts numpy array, the third channel could be confidence or occlusion
'''
# obtain the points
try:
pts_array = safe_2dptsarray(input_pts,
homogeneous=True,
warning=warning,
debug=debug)
except AssertionError:
pts_array = safe_2dptsarray(input_pts,
homogeneous=False,
warning=warning,
debug=debug)
if debug:
assert is2dptsarray(pts_array) or is2dptsarray_occlusion(
pts_array) or is2dptsarray_confidence(
pts_array), 'input points are not correct'
num_pts = pts_array.shape[1]
# obtain a label list if required but not provided
if debug: assert islogical(label), 'label flag is not correct'
if label and (label_list is None):
label_list = [str(i) for i in xrange(num_pts)]
if label_list is not None and debug:
assert islistofstring(label_list), 'labels are not correct'
# obtain the color index
if islist(color_index):
if debug:
assert not (
plot_occl or covariance
), 'the occlusion or covariance are not compatible with plotting different colors during scattering'
color_tmp = [color_set_big[index_tmp] for index_tmp in color_index]
else:
color_tmp = color_set_big[color_index % len(color_set_big)]
fig, ax = get_fig_ax_helper(fig=fig, ax=ax)
std, conf = None, 0.95
if is2dptsarray(pts_array): # only 2d points without third rows
if debug and islist(color_tmp):
assert len(
color_tmp
) == num_pts, 'number of points to plot is not equal to number of colors provided'
ax.scatter(pts_array[0, :],
pts_array[1, :],
color=color_tmp,
s=pts_size)
pts_visible_index = range(pts_array.shape[1])
pts_ignore_index = []
pts_invisible_index = []
else:
# automatically justify if the third row is confidence or occlusion flag
num_float_elements = np.where(
np.logical_and(
pts_array[2, :] != -1,
np.logical_and(pts_array[2, :] != 0,
pts_array[2, :] != 1)))[0].tolist()
if len(num_float_elements) > 0: type_3row = 'conf'
else: type_3row = 'occu'
if type_3row == 'occu':
pts_visible_index = np.where(pts_array[
2, :] == 1)[0].tolist() # plot visible points in red color
pts_ignore_index = np.where(pts_array[2, :] == -1)[0].tolist(
) # do not plot points with annotation, usually visible, but not annotated
pts_invisible_index = np.where(pts_array[
2, :] == 0)[0].tolist() # plot invisible points in blue color
else:
pts_visible_index = np.where(
pts_array[2, :] > vis_threshold)[0].tolist()
pts_invisible_index = np.where(
pts_array[2, :] <= vis_threshold)[0].tolist()
pts_ignore_index = []
if debug and islist(color_tmp):
assert len(color_tmp) == len(
pts_visible_index
), 'number of points to plot is not equal to number of colors provided'
ax.scatter(pts_array[0, pts_visible_index],
pts_array[1, pts_visible_index],
color=color_tmp,
s=pts_size)
if plot_occl:
ax.scatter(pts_array[0, pts_invisible_index],
pts_array[1, pts_invisible_index],
color=color_set_big[(color_index + 1) %
len(color_set_big)],
s=pts_size)
if covariance:
visualize_pts_covariance(pts_array[0:2, :],
std=std,
conf=conf,
fig=fig,
ax=ax,
debug=debug,
color=color_tmp)
if plot_occl: not_plot_index = pts_ignore_index
else: not_plot_index = pts_ignore_index + pts_invisible_index
if label_list is not None:
for pts_index in xrange(num_pts):
label_tmp = label_list[pts_index]
if pts_index in not_plot_index: continue
else:
# note that the annotation is based on the coordinate instead of the order of plotting the points, so the orider in pts_index does not matter
if islist(color_index):
plt.annotate(
label_tmp,
xy=(pts_array[0, pts_index], pts_array[1, pts_index]),
xytext=(-1, 1),
color=color_set_big[(color_index[pts_index] + 5) %
len(color_set_big)],
textcoords='offset points',
ha='right',
va='bottom',
fontsize=label_size)
else:
plt.annotate(label_tmp,
xy=(pts_array[0, pts_index],
pts_array[1, pts_index]),
xytext=(-1, 1),
color=color_set_big[(color_index + 5) %
len(color_set_big)],
textcoords='offset points',
ha='right',
va='bottom',
fontsize=label_size)
# set axis
if xlim is not None:
if debug:
assert islist(xlim) and len(xlim) == 2, 'the x lim is not correct'
plt.xlim(xlim[0], xlim[1])
if ylim is not None:
if debug:
assert islist(ylim) and len(ylim) == 2, 'the y lim is not correct'
plt.ylim(ylim[0], ylim[1])
return save_vis_close_helper(fig=fig,
ax=ax,
vis=vis,
save_path=save_path,
warning=warning,
debug=debug,
closefig=closefig,
transparent=False)
def triangulate_two_views(input_pts1,
input_pts2,
projection1,
projection2,
scaling_factor=1,
warning=True,
debug=True):
'''
triangulation from two views
'''
# projection1 - 3 x 4 Camera Matrix 1
# pts_array1 - 3 x N set of points
# projection2 - 3 x 4 Camera Matrix 2
# pts_array2 - 3 x N set of points
try:
pts_array1 = safe_2dptsarray(input_pts1,
homogeneous=True,
warning=warning,
debug=debug)
except AssertionError:
pts_array1 = safe_2dptsarray(input_pts1,
homogeneous=False,
warning=warning,
debug=debug)
if debug:
assert is2dptsarray(pts_array1) or is2dptsarray_occlusion(
pts_array1) or is2dptsarray_confidence(
pts_array1), 'first input points are not correct'
try:
pts_array2 = safe_2dptsarray(input_pts2,
homogeneous=True,
warning=warning,
debug=debug)
except AssertionError:
pts_array2 = safe_2dptsarray(input_pts2,
homogeneous=False,
warning=warning,
debug=debug)
if debug:
assert is2dptsarray(pts_array2) or is2dptsarray_occlusion(
pts_array2) or is2dptsarray_confidence(
pts_array2), 'second input points are not correct'
pts_array1 = pts_array1.astype('float32')
pts_array2 = pts_array2.astype('float32')
# initialization
num_pts = pts_array1.shape[1]
pts_array1[0:2, :] = pts_array1[0:2, :] / float(scaling_factor)
pts_array2[0:2, :] = pts_array2[0:2, :] / float(scaling_factor)
# least square
p1T1 = projection1[0, :] # 1 x 4
p1T2 = projection1[1, :]
p1T3 = projection1[2, :]
p2T1 = projection2[0, :]
p2T2 = projection2[1, :]
p2T3 = projection2[2, :]
# condition_matrix = np.array([[1e-3, 0, 0, 0], # 4 x 4
# [0, 1e-3, 0, 0],
# [0, 0, 1e-3, 0],
# [0, 0, 0, 1e-6]])
condition_matrix = np.array([
[1, 0, 0, 0], # 4 x 4
[0, 1, 0, 0],
[0, 0, 1, 0],
[0, 0, 0, 1]
])
pts_3d = np.zeros((num_pts, 4), 'float32') # N x 4
pts_3d.fill(-1)
p1_proj = pts_array1.copy().transpose() # N x 3
p2_proj = pts_array2.copy().transpose()
for i in range(num_pts):
if pts_array1[2, i] == -1 or pts_array2[2, i] == -1: continue
U = np.zeros((6, 4), dtype='float32') # 6 x 4
U[0, :] = np.multiply(pts_array1[1, i], p1T3) - p1T2 # y * p1T3 - p1T2
U[1, :] = p1T1 - np.multiply(pts_array1[0, i], p1T3) # p1T1 - x * p1T3
U[2, :] = np.multiply(pts_array1[0, i], p1T2) - np.multiply(
pts_array1[1, i], p1T1) # x * p1T2 - y * p1T1
U[3, :] = np.multiply(pts_array2[1, i], p2T3) - p2T2 # y * p2T3 - p2T2
U[4, :] = p2T1 - np.multiply(pts_array2[0, i], p2T3) # p2T1 - x * p2T3
U[5, :] = np.multiply(pts_array2[0, i], p2T2) - np.multiply(
pts_array2[1, i], p2T1) # x * p2T2 - y * p2T1
conditioned_U = np.matmul(U, condition_matrix)
# print(conditioned_U)
_, _, V = np.linalg.svd(conditioned_U)
conditioned_pts_3d = V[
-1, :] # 4 x 1, V is the transpose version of V in matlab
pts_3d[i, :] = np.matmul(condition_matrix,
conditioned_pts_3d.reshape(
(4, 1))).transpose()
pts_3d[i, :] = np.divide(pts_3d[i, :], pts_3d[i, 3]) # N x 4
# print(pts_3d)
# compute reprojection error
p1_proj[i, :] = (np.matmul(projection1,
pts_3d[i, :].transpose().reshape(
(4, 1)))).transpose() # 1 x 3
p2_proj[i, :] = (np.matmul(projection2,
pts_3d[i, :].transpose().reshape(
(4, 1)))).transpose()
p1_proj[i, :] = np.divide(p1_proj[i, :], p1_proj[i, -1])
p2_proj[i, :] = np.divide(p2_proj[i, :], p2_proj[i, -1])
# error = error + np.norm(p1_proj[i, 0:2] - pts_array1[0:2, i]) + np.norm(p2_proj[i, 0:2] - pts_array2[0:2, i])
error_tmp, error_list = pts_euclidean(p1_proj[:, 0:2].transpose(),
pts_array1[0:2, :],
warning=warning,
debug=debug)
error = error_tmp * num_pts
print(error_list)
error_tmp, error_list = pts_euclidean(p2_proj[:, 0:2].transpose(),
pts_array2[0:2, :],
warning=warning,
debug=debug)
error += error_tmp * num_pts
print(error_list)
pts_3d = pts_3d[:, 0:3].transpose() # 3 x N
return pts_3d, p1_proj.transpose(), p2_proj.transpose()
