Skip to content
Snippets Groups Projects
Commit 023c644f authored by Mahmoud Ahmed Ali's avatar Mahmoud Ahmed Ali
Browse files

Add test resize file

parent 0bb58091
No related branches found
No related tags found
No related merge requests found
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=[8, 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, resize=False):
# Read the point cloud
for obj in ["Banana"]:
obj_id = 0
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)
point_cloud_in_numpy = np.asarray(pcd.points)
center = fps_points.mean(0)
fps_points = np.append(fps_points, [center], axis=0)
for i in range(4995):
img = image.imread(f"{data_name}/RGB/{i}.png")
if resize:
img = cv2.resize(img.copy(), (640, 480))
pose = np.load(f'{data_name}/Pose_transformed/{obj}/{i}.npy')
R_exp = pose[0:3, 0:3]
tVec = pose[0:3, 3]
# 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 = [float(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]
x = point[0][0] / img.shape[1]
y = point[0][1] / img.shape[0]
out.append(x)
out.append(y)
ind += 2
np.savetxt(f'label_cen/{i}.txt', np.array(out).reshape(1, len(out)))
print("stop")
obj_id += 1
if __name__ == '__main__':
choice = "low"
data_options = {"high": "ground_truth_rgb",
"low": "ground_truth_depth"}
dataset_type = data_options[choice]
dataset_name = f"GUIMOD_{choice}"
if choice == 'high':
camera = np.matrix([[1386.4138492513919, 0.0, 960.5],
[0.0, 1386.4138492513919, 540.5],
[0.0, 0.0, 1.0]])
else:
camera = np.matrix([[1086.5054444841007, 0.0, 640.5],
[0.0, 1086.5054444841007, 360.5],
[0.0, 0.0, 1.0]])
# resize image to 640*480
trans = np.matrix([[0.5, 0.0, 0.0],
[0.0, (2 / 3), 0.0],
[0.0, 0.0, 1.0]])
camera_new = trans @ camera
generate_fps(dataset_name, camera_new, False, True)
0% Loading or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment