diff --git a/fps_alg.py b/fps_alg.py new file mode 100644 index 0000000000000000000000000000000000000000..68632a80b2eff2e8553d936f0a41198e886a3fd3 --- /dev/null +++ b/fps_alg.py @@ -0,0 +1,198 @@ +import numpy as np +import open3d as o3d +from matplotlib import pyplot as plt +from matplotlib import image +import cv2 +from skimage.io import imshow + + +def fps(points, centroid, n_samples): + """ + points: [N, 3] array containing the whole point cloud + n_samples: samples you want in the sampled point cloud typically << N + """ + points = np.array(points) + + # Represent the points by their indices in points + points_left = np.arange(len(points)) # [P] + + # Initialise an array for the sampled indices + sample_inds = np.zeros(n_samples, dtype='int') # [S] + + # Initialise distances to inf + dists = np.ones_like(points_left) * float('inf') # [P] + + # Select a point from points by its index, save it + selected = 0 + sample_inds[0] = points_left[selected] + + # Delete selected + points_left = np.delete(points_left, selected) # [P - 1] + + # Iteratively select points for a maximum of n_samples + for i in range(1, n_samples): + # Find the distance to the last added point in selected + # and all the others + last_added = sample_inds[i - 1] + + dist_to_last_added_point = ((points[last_added] - points[points_left]) ** 2).sum(-1) # [P - i] + + # If closer, updated distances + dists[points_left] = np.minimum(dist_to_last_added_point, dists[points_left]) # [P - i] + + # We want to pick the one that has the largest nearest neighbour + # distance to the sampled points + selected = np.argmax(dists[points_left]) + sample_inds[i] = points_left[selected] + + # Update points_left + points_left = np.delete(points_left, selected) + + return points[sample_inds] + + +def labelDrawPoints(drawList): # (b, f = back, front), (l, r = left, right), (u, d = up , down) + drawDict = {} + drawDict['bld'] = ((int(drawList[0][0])), int(drawList[0][1])) + drawDict['blu'] = ((int(drawList[1][0])), int(drawList[1][1])) + drawDict['fld'] = ((int(drawList[2][0])), int(drawList[2][1])) + drawDict['flu'] = ((int(drawList[3][0])), int(drawList[3][1])) + drawDict['brd'] = ((int(drawList[4][0])), int(drawList[4][1])) + drawDict['bru'] = ((int(drawList[5][0])), int(drawList[5][1])) + drawDict['frd'] = ((int(drawList[6][0])), int(drawList[6][1])) + drawDict['fru'] = ((int(drawList[7][0])), int(drawList[7][1])) + return drawDict + + +def drawPose(img, drawPoints, colour=(255, 0, 0)): # draw bounding box + + cv2.line(img, drawPoints['bld'], drawPoints['blu'], colour, 2) + cv2.line(img, drawPoints['bld'], drawPoints['fld'], colour, 2) + cv2.line(img, drawPoints['bld'], drawPoints['brd'], colour, 2) + cv2.line(img, drawPoints['blu'], drawPoints['flu'], colour, 2) + cv2.line(img, drawPoints['blu'], drawPoints['bru'], colour, 2) + cv2.line(img, drawPoints['fld'], drawPoints['flu'], colour, 2) + cv2.line(img, drawPoints['fld'], drawPoints['frd'], colour, 2) + cv2.line(img, drawPoints['flu'], drawPoints['fru'], colour, 2) + cv2.line(img, drawPoints['fru'], drawPoints['bru'], colour, 2) + cv2.line(img, drawPoints['fru'], drawPoints['frd'], colour, 2) + cv2.line(img, drawPoints['frd'], drawPoints['brd'], colour, 2) + cv2.line(img, drawPoints['brd'], drawPoints['bru'], colour, 2) + + +def showImage(img): # displays image using plt + imshow(img) + plt.show() + + +def apply_fps(pcd, fps_num): + point_cloud_in_numpy = np.asarray(pcd.points) + center = point_cloud_in_numpy.mean(0) + fps_points = fps(point_cloud_in_numpy, center, fps_num) + + return fps_points + + +def process2(pcd, R_exp, tVec, camera, img, vis= True): + + # point_cloud_in_numpy = np.asarray(pcd.points) + # center = point_cloud_in_numpy.mean(0) + # + # new_point = fps(point_cloud_in_numpy, center, fps_num) + # print(new_point) + + # pcd_fps = o3d.geometry.PointCloud() + # pcd_fps.points = o3d.utility.Vector3dVector(pcd) + + camera = np.array(camera) + R_exp = np.array(R_exp, dtype="float64") + tVec = np.array(tVec, dtype="float64") + + pcd_fps_numpy = np.asarray(pcd) + keypoint_2d = cv2.projectPoints(pcd_fps_numpy, R_exp, tVec, camera, np.zeros(shape=[5, 1], dtype='float64')) + + for n in range(len(pcd_fps_numpy)): + print(pcd_fps_numpy[n], '==>', keypoint_2d[0][n]) + + if vis: + out = np.zeros((img.shape[0], img.shape[1], 16)) + fig, ax = plt.subplots() + ax.imshow(img) + for n in range(len(pcd_fps_numpy)): + point = keypoint_2d[0][n] + ax.plot(point[0][0], point[0][1], marker='.', color="red") + + plt.imshow(img) + plt.show() + return keypoint_2d[0] + + +def process(pcd_box, pcd2, R_exp, tVec, camera, img): + + camera = np.array(camera) + R_exp = np.array(R_exp, dtype="float64") + tVec = np.array(tVec, dtype="float64") + + pcd2_in_numpy2 = np.asarray(pcd2.points) + pcd2_in_numpy = pcd_box + + keypoint_2d = cv2.projectPoints(pcd2_in_numpy, R_exp, tVec, camera, np.zeros(shape=[5, 1], dtype='float64')) + keypoint_2d2 = cv2.projectPoints(pcd2_in_numpy2, R_exp, tVec, camera, np.zeros(shape=[5, 1], dtype='float64')) + + for n in range(len(pcd2_in_numpy)): + print(pcd2_in_numpy[n], '==>', keypoint_2d[0][n]) + + points = [] + for n in range(len(pcd2_in_numpy)): + point = keypoint_2d[0][n] + points.append(point[0]) + + copy_img = img.copy() + fig, ax = plt.subplots() + ax.imshow(copy_img) + + for n in range(len(pcd2_in_numpy2)): + point = keypoint_2d2[0][n] + plt.plot(int(point[0][0]), int(point[0][1]), marker='.', color="red") + + copy_img = img.copy() + drawPose(copy_img, labelDrawPoints(points), (0, 1, 0)) + showImage(copy_img) + + +# ============================================================================== + +def generate_fps(data_name, camera, vis=False): + # Read the point cloud + for obj in ["Banana", "Orange", "Pear"]: + obj_id = 1 + point_cloud = f'{data_name}/Models/{obj}/{obj.lower()}.ply' + pcd = o3d.io.read_point_cloud(point_cloud) + + fps_points = apply_fps(pcd, 8) + np.savetxt(f'{data_name}/FPS/{obj}_fps_3d.txt', fps_points) + + for i in range(4995): + img = image.imread(f"{data_name}/RGB/{i}.png") + + pose = np.load(f'{data_name}/Pose_transformed/{obj}/{i}.npy') + R_exp = pose[0:3, 0:3] + tVec = pose[0:3, 3] + + # camera = np.matrix([[1386.4138492513919, 0.0, 960.5], + # [0.0, 1386.4138492513919, 540.5], + # [0.0, 0.0, 1.0]]) + + # process(pcd_bbox, pcd, R_exp, tVec, camera, img) + points = process2(fps_points, R_exp, tVec, camera, img, vis) + out = np.zeros((1, 17)) + + out[0] = obj_id + ind = 1 + for point in points: + out[0][ind] = point[0][0] / img.shape[1] + out[0][ind + 1] = point[0][1] / img.shape[0] + ind += 2 + np.savetxt(f'{data_name}/FPS/{obj}/{i}.txt', out) + print("stop") + obj_id += 1