-
Mahmoud Ahmed Ali authored0bb58091
test_fps.py 8.09 KiB
import numpy as np
import open3d as o3d
from matplotlib import pyplot as plt
from matplotlib import image
from numpy.linalg import norm
import math
import cv2
from pose import rotation_matrix, convert2
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 unit_vector_gt(outimage, x, y, keypoint):
for i in range(len(keypoint[0])):
xdiff = float(keypoint[0][i][0][0]) - x
ydiff = float(keypoint[0][i][0][1]) - y
mag = math.sqrt(ydiff ** 2 + xdiff ** 2)
outimage[x][y][i * 2] = xdiff / mag
outimage[x][y][i * 2 + 1] = ydiff / mag
def plot_unit_vector(point, vector):
V = np.array([vector[0], vector[1]])
origin = point[0] # np.array([[0, 0, 0], [0, 0, 0]]) # origin point
plt.quiver(*origin, V, color=['r', 'b', 'g'], scale=21)
plt.show()
plt.quiver([0, 0, 0], [0, 0, 0], [1, -2, 4], [1, 2, -7], angles='xy', scale_units='xy', scale=1)
plt.xlim(-10, 10)
plt.ylim(-10, 10)
plt.show()
X = np.arange(-10, 10, 1)
Y = np.arange(-10, 10, 1)
U, V = np.meshgrid(X, Y)
fig, ax = plt.subplots()
q = ax.quiver(X, Y, U, V)
ax.quiverkey(q, X=0.3, Y=1.1, U=10,
label='Quiver key, length = 10', labelpos='E')
plt.show()
def process(pcd, R_exp, tVec, camera, img, id):
# pcd = o3d.io.read_point_cloud(obj_path) # Read the point cloud
# textured_mesh = o3d.io.read_triangle_mesh("banana1_visual.obj")
# o3d.visualization.draw_geometries([textured_mesh])
# Visualize the point cloud within open3d
# o3d.visualization.draw_geometries([pcd])
# Convert open3d format to numpy array
# Here, you have the point cloud in numpy format.
point_cloud_in_numpy = np.asarray(pcd.points)
center = point_cloud_in_numpy.mean(0)
new_point = fps(point_cloud_in_numpy, center, 8)
# print(new_point)
pcd2 = o3d.geometry.PointCloud()
pcd2.points = o3d.utility.Vector3dVector(new_point)
# o3d.visualization.draw_geometries([pcd2])
# vis = o3d.visualization.Visualizer()
# vis.create_window()
# vis.add_geometry(pcd)
# vis.add_geometry(pcd2)
# vis.run()
# vis.destroy_window()
# print(pose)
camera = np.array(camera)
R_exp = np.array(R_exp, dtype="float64")
tVec = np.array(tVec, dtype="float64")
# print(R_exp)
# print(tVec)
pcd2_in_numpy = np.asarray(pcd.points)
keypoint_2d = cv2.projectPoints(pcd2_in_numpy, R_exp, tVec, camera, np.zeros(shape=[8, 1], dtype='float64'))
for n in range(len(pcd2_in_numpy)):
print(pcd2_in_numpy[n], '==>', keypoint_2d[0][n])
out = np.zeros((img.shape[0], img.shape[1], 16))
fig, ax = plt.subplots()
ax.imshow(img)
for n in range(len(pcd2_in_numpy)):
point = keypoint_2d[0][n]
ax.plot(point[0][0], point[0][1], marker='.', color="red")
plt.imshow(img)
plt.show()
# plt.savefig(f"result/img_{id}.png")
# ==============================================================================
x, y, z = [ 2.9788743387155581, -0.27449049579661572, -1.2476345641806936 ]
# Banana [ 0.085909521758723559, -0.10993803084296662, 2.4625571948393996 ]
# Pear [2.940213420813008, -0.91304327878070535, -2.6173582584096144]
# Orange [ 2.9788743387155581, -0.27449049579661572, -1.2476345641806936 ]
dic = [[x, y, z],
[x, z, y],
[y, x, z],
[y, z, x],
[z, x, y],
[z, y, x],
# ===========
[-x, -y, -z],
[-x, -z, -y],
[-y, -x, -z],
[-y, -z, -x],
[-z, -x, -y],
[-z, -y, -x],
# ===========
[-x, y, z],
[-x, z, y],
[-y, x, z],
[-y, z, x],
[-z, x, y],
[-z, y, x],
# ===========
[x, -y, z],
[x, -z, y],
[y, -x, z],
[y, -z, x],
[z, -x, y],
[z, -y, x],
# ===========
[x, y, -z],
[x, z, -y],
[y, x, -z],
[y, z, -x],
[z, x, -y],
[z, y, -x],
# ===========
[-x, -y, z],
[-x, -z, y],
[-y, -x, z],
[-y, -z, x],
[-z, -x, y],
[-z, -y, x],
# ===========
[-x, y, -z],
[-x, z, -y],
[-y, x, -z],
[-y, z, -x],
[-z, x, -y],
[-z, y, -x],
# ===========
[x, -y, -z],
[x, -z, -y],
[y, -x, -z],
[y, -z, -x],
[z, -x, -y],
[z, -y, -x],
]
point_cloud = "/home/mahmoud/GUIMOD/Models/Orange/orange2_visual2.ply"
pose = np.load('/home/mahmoud/GUIMOD/Pose/Orange/0.npy')
new = np.matrix([[0.0000000, -1.0000000, 0.0000000],
[0.0000000, 0.0000000, -1.0000000],
[1.0000000, 0.0000000, 0.0000000]])
t_org = pose[0:3, 3]
tVec = new @ t_org
print(tVec)
img = image.imread('/media/mahmoud/F/GUIMOD/data/1/grabber_1/color/image/0_0.png')
camera = [[1386.4138492513919, 0.0, 960.5], [0.0, 1386.4138492513919, 540.5], [0.0, 0.0, 1.0]]
pcd = o3d.io.read_point_cloud(point_cloud) # Read the point cloud
if __name__ == '__main__':
# Read .ply file
obj = 'cat'
# point_cloud = "/home/mahmoud/GUIMOD/Models/Pear/pear2_visual2.ply"
# point_cloud = f"{obj}.ply"
# --------------------
# pose = np.load(f'./{obj}/pose/pose0.npy')
# pose = np.load('/home/mahmoud/GUIMOD/Pose/Pear/1.npy')
# R_exp = pose[0:3, 0:3]
# XYZ change x with z and remove - from y
# R_xyz = np.matrix([[0.9902972, -0.0852859, 0.1097167],
# [0.0018819, -0.7812214, -0.6242512],
# [0.1389529, 0.6184007, -0.7734809]])
# t_org = pose[0:3, 3]
# tVec = new @ t_org
#
# print(tVec)
# --------------------
# img = image.imread(f'./{obj}/JPEGImages/000000.jpg')
# img = image.imread('/media/mahmoud/F/GUIMOD/data/1/grabber_2/color/image/0_0.png')
# --------------------
# camera = [[1386.4138492513919, 0.0, 960.5], [0.0, 1386.4138492513919, 540.5], [0.0, 0.0, 1.0]]
i = 0
for pos in dic:
rot = rotation_matrix(pos)
R_exp = rot
process(pcd, R_exp, tVec, camera, img, i)
i += 1
"""
D: [0.0, 0.0, 0.0, 0.0, 0.0]
K: [1386.4138492513919, 0.0, 960.5, 0.0, 1386.4138492513919, 540.5, 0.0, 0.0, 1.0]
R: [1.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 1.0]
P: [1386.4138492513919, 0.0, 960.5, -0.0, 0.0, 1386.4138492513919, 540.5, 0.0, 0.0, 0.0, 1.0, 0.0]
The output matrices of the camera_calibration package are: 1) Distortion parameters (D) 2) Intrinsic camera matrix (K) 3) Rectification matrix (R) 4) Projection matrix of the processed (rectified) image (P)
D: [0.0, 0.0, 0.0, 0.0, 0.0]
K: [1086.5054444841007, 0.0, 640.5, 0.0, 1086.5054444841007, 360.5, 0.0, 0.0, 1.0]
R: [1.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 1.0]
P: [1086.5054444841007, 0.0, 640.5, -0.0, 0.0, 1086.5054444841007, 360.5, 0.0, 0.0, 0.0, 1.0, 0.0]
"""