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]
"""