def plot_points(vis, pcd, gripper_pcd, left_tip, right_tip, gripper_trans,
gripper_mid_point, marker_radius):
# Object point cloud
if pcd is not None:
pcd.paint_uniform_color([0.5, 0.5, 0.5])
vis.add_geometry(pcd)
# Gripper cloud
if gripper_pcd is not None:
gripper_pcd.paint_uniform_color([0.4, 0.8, 0.4])
vis.add_geometry(gripper_pcd)
# Finger tip points, mid point and center point
endpoints = []
for i in range(2):
mm = o3d.create_mesh_sphere(radius=marker_radius)
mm.compute_vertex_normals()
if i == 0:
trans3d = left_tip
mm.paint_uniform_color([1., 0., 0.])
else:
trans3d = right_tip
mm.paint_uniform_color([0., 0., 1.])
tt = np.eye(4)
tt[0:3, 3] = trans3d
mm.transform(tt)
vis.add_geometry(mm)
endpoints.append(trans3d)
mm = o3d.create_mesh_sphere(radius=marker_radius)
mm.compute_vertex_normals()
mm.paint_uniform_color([0., 0., 0.])
tt = np.eye(4)
tt[0:3, 3] = gripper_trans
mm.transform(tt)
vis.add_geometry(mm)
endpoints.append(gripper_trans)
mm = o3d.create_mesh_sphere(radius=marker_radius)
mm.compute_vertex_normals()
mm.paint_uniform_color([0., 0., 0.])
tt = np.eye(4)
tt[0:3, 3] = gripper_mid_point
mm.transform(tt)
vis.add_geometry(mm)
endpoints.append(gripper_mid_point)
lines = [[0, 1], [2, 3]]
line_colors = [[1., 0., 1.], [0., 0., 0.]]
line_set = o3d.geometry.LineSet()
line_set.points = o3d.utility.Vector3dVector(endpoints)
line_set.lines = o3d.utility.Vector2iVector(lines)
line_set.colors = o3d.utility.Vector3dVector(line_colors)
vis.add_geometry(line_set)
def visualize_views(viewpoints, view_subset=None):
# Create visualizer
vis = o3d.visualization.Visualizer()
vis.create_window()
# Sphere
mesh_sphere = o3d.create_mesh_sphere(radius=0.1)
mesh_sphere.compute_vertex_normals()
mesh_sphere.paint_uniform_color([0.1, 0.1, 0.7])
vis.add_geometry(mesh_sphere)
# Get the subset of views
if view_subset is not None:
frame_keys = view_subset
else:
frame_keys = viewpoints.keys()
# Plot camera poses
for frame_id in frame_keys:
m = viewpoints[frame_id]['pose']
points = m[:3, :].T
points[0:3, :] *= 0.1
points[0:3, :] += np.tile(points[3, :], (3, 1))
lines = [[3, 0], [3, 1], [3, 2]]
line_colors = [[1., 0., 0.], [0., 1., 0.], [0., 0., 1.]]
line_set = o3d.geometry.LineSet()
line_set.points = o3d.utility.Vector3dVector(points)
line_set.lines = o3d.utility.Vector2iVector(lines)
line_set.colors = o3d.utility.Vector3dVector(line_colors)
vis.add_geometry(line_set)
# End visualizer
vis.run()
vis.destroy_window()
def addSamples(self, R=None):
for ifor in range(len(self.superPoints)):
center, radius = self.superPoints[ifor]
if R is None:
R = radius
sphere = o3d.create_mesh_sphere(R).transform(pose(center))
sphere.paint_uniform_color([0.1, 0.1, 0.7])
sphere.compute_vertex_normals()
self.showObjects.append(sphere)
def makeSphere(self, R=1, center=None):
if center is None:
center = np.mean(self.xyz, axis=0)
self.sphereCurrentPose = pose(center)
sphere = o3d.create_mesh_sphere(R).transform(pose(center))
sphere.paint_uniform_color([0.1, 0.7, 0.2])
sphere.compute_vertex_normals()
self.sphere = sphere
return sphere
def show(xyz, XYZ):
pcd = o3d.geometry.PointCloud()
pcd.points = o3d.utility.Vector3dVector(xyz)
obs = [pcd]
for ifor in range(XYZ.shape[0]):
sp = o3d.create_mesh_sphere(0.05).transform(pose(XYZ[ifor, :]))
obs.append(sp)
o3d.draw_geometries(obs)
def visualize_hand(self, joints3d, grasp_points, mid_point, wrist_point):
# Create visualizer
vis = o3d.visualization.Visualizer()
vis.create_window()
# Plot the coordinate frame
vis.add_geometry(o3d.create_mesh_coordinate_frame(size=0.05))
# Plot the object cloud
vis.add_geometry(self.pcd)
# Plot the finger joints
for i in range(len(all_indices)):
for j in range(len(all_indices[i])):
mm = o3d.create_mesh_sphere(radius=joint_sizes[j])
mm.compute_vertex_normals()
mm.paint_uniform_color(finger_colors[i])
trans3d = joints3d[all_indices[i][j]]
tt = np.eye(4)
tt[0:3, 3] = trans3d
mm.transform(tt)
vis.add_geometry(mm)
# Plot lines between joints
lines = [[0, 13], [0, 1], [0, 4], [0, 10], [0, 7],
[13, 14], [14, 15], [15, 16],
[1, 2], [2, 3], [3, 17],
[4, 5], [5, 6], [6, 18],
[10, 11], [11, 12], [12, 19],
[7, 8], [8, 9], [9, 20]]
line_colors = [finger_colors[1], finger_colors[2], finger_colors[3], finger_colors[4], finger_colors[5],
finger_colors[1], finger_colors[1], finger_colors[1],
finger_colors[2], finger_colors[2], finger_colors[2],
finger_colors[3], finger_colors[3], finger_colors[3],
finger_colors[4], finger_colors[4], finger_colors[4],
finger_colors[5], finger_colors[5], finger_colors[5]]
line_set = o3d.geometry.LineSet()
line_set.points = o3d.utility.Vector3dVector(joints3d)
line_set.lines = o3d.utility.Vector2iVector(lines)
line_set.colors = o3d.utility.Vector3dVector(line_colors)
vis.add_geometry(line_set)
# Plot the grasp points
self.plot_gripper_end_points(vis, grasp_points)
vis.run()
vis.destroy_window()
def get_keypoint_sphere(
keypoint, # type: List[float],
radius=0.01):
"""
Create a sphere mesh used for keypoint visualization
:param keypoint: length 3 list (x, y, z) for the center of the mesh
:param radius: The radius of the mesh
:return:
"""
assert len(keypoint) is 3
mesh_sphere = open3d.create_mesh_sphere(radius=radius)
translation = np.identity(4)
translation[0, 3] = keypoint[0]
translation[1, 3] = keypoint[1]
translation[2, 3] = keypoint[2]
mesh_sphere.transform(translation)
return mesh_sphere
def visualize_hand(vis, joints, shape=JointShapes.SPHERE):
# Set the colors for the fingers depending on the shape
if shape == JointShapes.SPHERE:
shape_finger_colors = finger_colors
else:
shape_finger_colors = finger_colors_box
# Plot the finger joints
for i in range(len(all_indices)):
for j in range(len(all_indices[i])):
if shape == JointShapes.SPHERE:
mm = o3d.create_mesh_sphere(radius=joint_sizes[j])
else:
mm = o3d.create_mesh_box(width=joint_sizes[j],
height=joint_sizes[j],
depth=joint_sizes[j])
mm.compute_vertex_normals()
mm.paint_uniform_color(shape_finger_colors[i])
trans3d = joints[all_indices[i][j]]
tt = np.eye(4)
tt[0:3, 3] = trans3d
mm.transform(tt)
vis.add_geometry(mm)
# Plot lines between joints
lines = [[0, 13], [0, 1], [0, 4], [0, 10], [0, 7], [13, 14], [14, 15],
[15, 16], [1, 2], [2, 3], [3, 17], [4, 5], [5, 6], [6, 18],
[10, 11], [11, 12], [12, 19], [7, 8], [8, 9], [9, 20]]
line_colors = [
shape_finger_colors[1], shape_finger_colors[2],
shape_finger_colors[3], shape_finger_colors[4],
shape_finger_colors[5], shape_finger_colors[1],
shape_finger_colors[1], shape_finger_colors[1],
shape_finger_colors[2], shape_finger_colors[2],
shape_finger_colors[2], shape_finger_colors[3],
shape_finger_colors[3], shape_finger_colors[3],
shape_finger_colors[4], shape_finger_colors[4],
shape_finger_colors[4], shape_finger_colors[5],
shape_finger_colors[5], shape_finger_colors[5]
]
line_set = o3d.geometry.LineSet()
line_set.points = o3d.utility.Vector3dVector(joints)
line_set.lines = o3d.utility.Vector2iVector(lines)
line_set.colors = o3d.utility.Vector3dVector(line_colors)
vis.add_geometry(line_set)
def callback(self,*args):
print("YEET")
norm=0
rgb = IRos.rosImg2RGB(args[0])
depth_reg = IRos.rosImg2Depth(args[1])
#copy image
hello = rgb.astype(np.uint8).copy()
#cv2.imshow("wow",hello)
#cv2.waitKey(0)
#cv2.destroyAllWindows()
#finds markers
det_corners, ids, rejected = aruco.FindMarkers(rgb, self.K,self.D)
#draw maerkers
if ids is not None:
hello = cv2.aruco.drawDetectedMarkers(hello,det_corners,np.asarray(ids))
#cv2.imshow("Detected Markers",hello)
#cv2.waitKey(0)
#cv2.destroyAllWindows()
if ids is None:
return
ids = ids.squeeze()
#makes a single id into a list with only it self
if (helperfuncs.is_empty(ids.shape)):
ids=[int(ids)]
#place where all geometries will be stores
sphs = []
'''
#3D WAY (Scaled Procrustes)
if ids is not None and len(ids)>0:
#filter ids and cornerds
validids=[]
validcordners= []
#fetches only ids that are on the cangalho
for i in range(0,len(ids)):
if ids[i] in self.arucoData['ids']:
#print("Valid marker id: "+str(ids[i]))
validids.append(ids[i])
validcordners.append(det_corners[i])
Rr,tt = aruco.GetCangalhoFromMarkersProcrustes(validids,validcordners,self.K,self.arucoData,self.arucoModel,depth_reg)
DATAprocrustes= (Rr,tt)
if(Rr is not None):
H = mmnip.Rt2Homo(Rr.T,tt)
refe = open3d.create_mesh_coordinate_frame(0.16, origin = [0, 0, 0])
refe.transform(H)
sphs.append(refe)
sphere2 = open3d.create_mesh_sphere(0.02)
sphere2.transform(H)
sphere2.paint_uniform_color([1,0,1])
sphs.append(sphere2)
#PnP way
if ids is not None and len(ids)>0:
#only fetches corners and ids, for the markers ids that exist in cangalho (2-13)
validids=[]
validcordners=[]
for i in range(0,len(ids)):
if ids[i] in self.arucoData['ids']:
validids.append(ids[i])
validcordners.append(det_corners[i])
#calculates pose
Rr,tt = aruco.GetCangalhoFromMarkersPnP(validids,validcordners,self.K,self.D,self.arucoData,self.arucoModel,None)#(Rr.T,tt)
#converts in homogeneous
H = mmnip.Rt2Homo(Rr,tt.T)
#prints results, in green
sphere1 = open3d.create_mesh_sphere(0.02)
sphere1.transform(H)
sphere1.paint_uniform_color([0,1,0])
sphs.append(sphere1)
refe = open3d.create_mesh_coordinate_frame(0.16, origin = [0, 0, 0])
refe.transform(H) #Transform it according tom p
sphs.append(refe)
DATApnp= (Rr,tt)
print(DATAprocrustes[1])
print(DATApnp[1].squeeze())
if(DATAprocrustes[1] is not None):
norm = np.linalg.norm(DATAprocrustes[1]-DATApnp[1].squeeze())
print(norm)
if(norm>self.prevnorm):
self.dir=self.dir*-1
self.prevnorm=norm
self.iterations=self.iterations+1
self.K[0,0]=self.K[0,0]+self.dir
self.K[1,1]=self.K[1,1]+self.dir
print(self.K)
pointsu = np.empty((3,0))
corneee = np.squeeze(det_corners)
#find detected corners in 3D and paint them
for cor in det_corners:
for i in range(0,4):
point = mmnip.singlePixe2xyz(depth_reg,cor[0,i,:],self.K)
point = np.expand_dims(point,axis=1)
H = np.eye(4)
H[0:3,3]=point.T
#paints corners in 3D space
sphere = open3d.create_mesh_sphere(0.006)
sphere.transform(H)
sphere.paint_uniform_color([1,0,1])
sphs.append(sphere)
pointsu=np.hstack((pointsu,point))
'''
#Marker-by-Marker WAY
rvecs, tvecs, _ = cv2.aruco.estimatePoseSingleMarkers(det_corners,self.arucoData['size'],K,D)
tvecs=np.squeeze(tvecs)
#turn 0D into 1D
if len(tvecs.shape)==1:
tvecs = np.expand_dims(tvecs,axis=0)
for i in range(0,tvecs.shape[0]):
sphere = open3d.create_mesh_sphere(0.016)
#converts in 3x3 rotation matrix
Rr,_ = cv2.Rodrigues(rvecs[i])
H = mmnip.Rt2Homo(Rr,tvecs[i,:])
#prints marker position estimates
refe = open3d.create_mesh_coordinate_frame(0.1, origin = [0, 0, 0])
refe.transform(H)
sphere.transform(H)
sphere.paint_uniform_color([0,0,1])
sphs.append(sphere)
sphs.append(refe)
#converts image 2 depth
points = mmnip.depthimg2xyz2(depth_reg,self.K)
points = points.reshape((480*640, 3))
#print(colors.shape)
rgb1 = rgb.reshape((480*640, 3))#colors
pc = pointclouder.Points2Cloud(points,rgb1)
pc2 = pointclouder.Points2Cloud(pointsu.T)
pc2.paint_uniform_color([1,0,1])
#DRAW
if(norm<0.005):
open3d.draw_geometries([pc]+sphs)
def callback(self, *args):
rgb = IRos.rosImg2RGB(args[0])
depth_reg = IRos.rosImg2Depth(args[1])
#copy image
hello = rgb.astype(np.uint8).copy()
cv2.imshow("wow", hello)
cv2.waitKey(0)
cv2.destroyAllWindows()
#get matrix intrinsics
K = self.intrinsics['K'][self.camName]
D = self.intrinsics['D'][self.camName]
#finds markers
det_corners, ids, rejected = aruco.FindMarkers(rgb, K, D)
#draw maerkers
if ids is not None:
hello = cv2.aruco.drawDetectedMarkers(hello, det_corners,
np.asarray(ids))
cv2.imshow("Detected Markers", hello)
cv2.waitKey(0)
cv2.destroyAllWindows()
if ids is None:
return
ids = ids.squeeze()
#makes a single id into a list with only it self
if (helperfuncs.is_empty(ids.shape)):
ids = [int(ids)]
#place where all geometries will be stores
sphs = []
#3D WAY (Scaled Procrustes)
if ids is not None and len(ids) > 0:
#filter ids and cornerds
validids = []
validcordners = []
#fetches only ids that are on the cangalho
for i in range(0, len(ids)):
if ids[i] in self.arucoData['ids']:
#print("Valid marker id: "+str(ids[i]))
validids.append(ids[i])
validcordners.append(det_corners[i])
Rr, tt = aruco.GetCangalhoFromMarkersProcrustes(
validids, validcordners, K, self.arucoData, self.arucoModel,
depth_reg)
if (Rr is not None):
H = mmnip.Rt2Homo(Rr.T, tt)
refe = open3d.create_mesh_coordinate_frame(0.16,
origin=[0, 0, 0])
refe.transform(H)
sphs.append(refe)
sphere2 = open3d.create_mesh_sphere(0.02)
sphere2.transform(H)
sphere2.paint_uniform_color([1, 0, 1])
sphs.append(sphere2)
#PnP way
if ids is not None and len(ids) > 0:
#only fetches corners and ids, for the markers ids that exist in cangalho (2-13)
validids = []
validcordners = []
for i in range(0, len(ids)):
if ids[i] in self.arucoData['ids']:
validids.append(ids[i])
validcordners.append(det_corners[i])
#calculates pose
Rr, tt = aruco.GetCangalhoFromMarkersPnP(validids, validcordners,
K, D, self.arucoData,
self.arucoModel,
None) #(Rr.T,tt)
#converts in homogeneous
H = mmnip.Rt2Homo(Rr, tt.T)
#prints results, in green
sphere1 = open3d.create_mesh_sphere(0.02)
sphere1.transform(H)
sphere1.paint_uniform_color([0, 1, 0])
sphs.append(sphere1)
refe = open3d.create_mesh_coordinate_frame(0.16, origin=[0, 0, 0])
refe.transform(H) #Transform it according tom p
sphs.append(refe)
pointsu = np.empty((3, 0))
#print(hello.shape)
#print("detected cornerds")
#print(det_corners)
corneee = np.squeeze(det_corners)
#print(corneee)
#corn2paint = corneee[2,:]
#offset=5
#for ii in range(int(corn2paint[0])-offset,int(corn2paint[0])+offset):
# for jj in range(int(corn2paint[1])-offset,int(corn2paint[1])+offset):
# hello[jj,ii,:]= [255,0,255]
#cv2.imshow("wow",hello)
#cv2.waitKey(0)
#cv2.destroyAllWindows()
#map1,map2 = cv2.initUndistortRectifyMap(K,D,np.eye(3),K,(640,480),cv2.CV_32FC1)
#print(wowl)
#img2 = cv2.remap(hello, map1, map2,cv2.INTER_NEAREST)
#cv2.imshow("woweeeeeeeeeeeeeeeeeeeeeeeeeeeeeeee",img2)
#cv2.waitKey(0)
#cv2.destroyAllWindows()
#find detected corners in 3D and paint them
for cor in det_corners:
for i in range(0, 4):
point = mmnip.singlePixe2xyz(depth_reg, cor[0, i, :], K)
point = np.expand_dims(point, axis=1)
H = np.eye(4)
H[0:3, 3] = point.T
#paints corners in 3D space
sphere = open3d.create_mesh_sphere(0.006)
sphere.transform(H)
sphere.paint_uniform_color([1, 0, 1])
sphs.append(sphere)
pointsu = np.hstack((pointsu, point))
#Marker-by-Marker WAY
rvecs, tvecs, _ = cv2.aruco.estimatePoseSingleMarkers(
det_corners, self.arucoData['size'], K, D)
tvecs = np.squeeze(tvecs)
#turn 0D into 1D
if len(tvecs.shape) == 1:
tvecs = np.expand_dims(tvecs, axis=0)
for i in range(0, tvecs.shape[0]):
sphere = open3d.create_mesh_sphere(0.016)
#converts in 3x3 rotation matrix
Rr, _ = cv2.Rodrigues(rvecs[i])
H = mmnip.Rt2Homo(Rr, tvecs[i, :])
#prints marker position estimates
refe = open3d.create_mesh_coordinate_frame(0.1, origin=[0, 0, 0])
refe.transform(H)
sphere.transform(H)
sphere.paint_uniform_color([0, 0, 1])
sphs.append(sphere)
sphs.append(refe)
#converts image 2 depth
points = mmnip.depthimg2xyz2(depth_reg, K)
points = points.reshape((480 * 640, 3))
#print(colors.shape)
rgb1 = rgb.reshape((480 * 640, 3)) #colors
pc = pointclouder.Points2Cloud(points, rgb1)
pc2 = pointclouder.Points2Cloud(pointsu.T)
pc2.paint_uniform_color([1, 0, 1])
open3d.draw_geometries([pc] + sphs)
def visualize_hand_and_grasp(self, joints3d, transform, grasp_points, mid_point, wrist_point, scene_pcd=None,
hand_mesh=None):
# Create visualizer
vis = o3d.visualization.Visualizer()
vis.create_window()
# Plot the coordinate frame
vis.add_geometry(o3d.create_mesh_coordinate_frame(size=0.05))
# Plot the object cloud
vis.add_geometry(self.pcd)
# Plot the finger joints
for i in range(len(all_indices)):
for j in range(len(all_indices[i])):
mm = o3d.create_mesh_sphere(radius=joint_sizes[j])
mm.compute_vertex_normals()
mm.paint_uniform_color(finger_colors[i])
trans3d = joints3d[all_indices[i][j]]
tt = np.eye(4)
tt[0:3, 3] = trans3d
mm.transform(tt)
vis.add_geometry(mm)
# Plot lines between joints
lines = [[0, 13], [0, 1], [0, 4], [0, 10], [0, 7],
[13, 14], [14, 15], [15, 16],
[1, 2], [2, 3], [3, 17],
[4, 5], [5, 6], [6, 18],
[10, 11], [11, 12], [12, 19],
[7, 8], [8, 9], [9, 20]]
line_colors = [finger_colors[1], finger_colors[2], finger_colors[3], finger_colors[4], finger_colors[5],
finger_colors[1], finger_colors[1], finger_colors[1],
finger_colors[2], finger_colors[2], finger_colors[2],
finger_colors[3], finger_colors[3], finger_colors[3],
finger_colors[4], finger_colors[4], finger_colors[4],
finger_colors[5], finger_colors[5], finger_colors[5]]
line_set = o3d.geometry.LineSet()
line_set.points = o3d.utility.Vector3dVector(joints3d)
line_set.lines = o3d.utility.Vector2iVector(lines)
line_set.colors = o3d.utility.Vector3dVector(line_colors)
vis.add_geometry(line_set)
# Plot the gripper cloud
self.plot_gripper_cloud(vis, self.gripper_pcd, transform)
# Plot the grasp points
self.plot_gripper_end_points(vis, grasp_points)
# Plot scene
if scene_pcd is not None:
vis.add_geometry(scene_pcd)
# Visualize hand
'''
if hand_mesh is not None:
mesh = o3d.geometry.TriangleMesh()
if hasattr(hand_mesh, 'r'):
mesh.vertices = o3d.utility.Vector3dVector(np.copy(hand_mesh.r) * 0.001)
numVert = hand_mesh.r.shape[0]
elif hasattr(hand_mesh, 'v'):
mesh.vertices = o3d.utility.Vector3dVector(np.copy(hand_mesh.v) * 0.001)
numVert = hand_mesh.v.shape[0]
else:
raise Exception('Unknown Mesh format')
mesh.triangles = o3d.utility.Vector3iVector(np.copy(hand_mesh.f))
mesh.vertex_colors = o3d.utility.Vector3dVector(
np.tile(np.array([[0.9, 0.4, 0.4]]), [numVert, 1]))
o3d.visualization.draw_geometries([mesh])
'''
vis.run()
vis.destroy_window()
from open3d import create_mesh_sphere, create_mesh_cylinder, create_mesh_coordinate_frame, draw_geometries
mesh_sphere = create_mesh_sphere(radius=1.0)
mesh_sphere.compute_vertex_normals()
mesh_sphere.paint_uniform_color([0.1, 0.1, 0.7])
mesh_cylinder = create_mesh_cylinder(radius=0.3, height=4.0)
mesh_cylinder.compute_vertex_normals()
mesh_cylinder.paint_uniform_color([0.1, 0.9, 0.1])
mesh_frame = create_mesh_coordinate_frame(size=0.6, origin=[-2, -2, -2])
print("We draw a few primitives using collection.")
draw_geometries([mesh_sphere, mesh_cylinder, mesh_frame])
def visualize(self, voxels, hand_joints, grasp_pose=None):
offset = np.expand_dims(np.mean(hand_joints, axis=0), 0)
nearest_voxel = None
if grasp_pose is not None:
dists = np.linalg.norm(voxels -
(grasp_pose[:3, 3].flatten() - offset),
axis=1)
min_idx = np.argmin(dists)
# print('Translation error {:.0f}mm'.format(dists[min_idx] * 1000))
nearest_voxel = voxels[min_idx]
# Create visualizer
vis = o3d.visualization.Visualizer()
vis.create_window()
# Add the coordinate frame
vis.add_geometry(o3d.create_mesh_coordinate_frame(size=0.05))
# Add the hand joints
for i in range(len(hand_joints)):
mm = o3d.create_mesh_sphere(radius=0.005)
mm.compute_vertex_normals()
mm.paint_uniform_color([0, 0, 1])
tt = np.eye(4)
tt[0:3, 3] = hand_joints[i] - offset
mm.transform(tt)
vis.add_geometry(mm)
# Add the voxels
for v in voxels:
mm = o3d.create_mesh_box(width=0.1 * self.res,
height=0.1 * self.res,
depth=0.1 * self.res)
mm.compute_vertex_normals()
mm.paint_uniform_color([0.5, 0.5, 0.5])
tt = np.eye(4)
tt[0:3, 3] = v
mm.transform(tt)
vis.add_geometry(mm)
if nearest_voxel is not None:
mm = o3d.create_mesh_box(width=0.005, height=0.005, depth=0.005)
mm.compute_vertex_normals()
mm.paint_uniform_color([0, 1, 0])
tt = np.eye(4)
tt[0:3, 3] = nearest_voxel
mm.transform(tt)
vis.add_geometry(mm)
# Add the grasp position
if grasp_pose is not None:
mm = o3d.create_mesh_box(width=0.005, height=0.005, depth=0.005)
mm.compute_vertex_normals()
mm.paint_uniform_color([1, 0, 0])
tt = np.eye(4)
tt[0:3, 3] = grasp_pose[:3, 3].flatten() - offset
mm.transform(tt)
vis.add_geometry(mm)
gripper_gt = self.get_transformed_gripper_pcd(
(grasp_pose[:3, 3] - offset).flatten(), grasp_pose[:3, :3])
gripper_pcd = o3d.geometry.PointCloud()
gripper_pcd.points = o3d.utility.Vector3dVector(gripper_gt.T)
gripper_pcd.paint_uniform_color([0.9, 0.4, 0.4])
vis.add_geometry(gripper_pcd)
gripper_approx = self.get_transformed_gripper_pcd(
nearest_voxel, grasp_pose[:3, :3])
gripper_pcd_approx = o3d.geometry.PointCloud()
gripper_pcd_approx.points = o3d.utility.Vector3dVector(
gripper_approx.T)
gripper_pcd_approx.paint_uniform_color([0.4, 0.9, 0.4])
vis.add_geometry(gripper_pcd_approx)
# Run the visualizer
vis.run()
vis.destroy_window()
training=True,
fused=True)
inputs = tf.nn.sigmoid(inputs)
return inputs[:, :1], inputs[:, 1:]
inputs = tf.placeholder(tf.float32, [None, 3])
outputs = generator(inputs, stddev=0.03)
with tf.Session() as session:
session.run(tf.global_variables_initializer())
mesh_sphere = open3d.create_mesh_sphere(radius=1.0, resolution=1000)
vertices = np.array(mesh_sphere.vertices)
scale, colors = session.run(outputs, feed_dict={inputs: vertices})
mesh_sphere.vertices = open3d.Vector3dVector(vertices * scale)
mesh_sphere.vertex_colors = open3d.Vector3dVector(colors)
mesh_sphere.compute_vertex_normals()
def call_back(visualizer):
visualizer.get_render_option().background_color = np.asarray([0, 0, 0])
visualizer.get_view_control().rotate(10.0, 0.0)
return False
open3d.draw_geometries_with_animation_callback([mesh_sphere], call_back)
def create_skeleton_geometry(self):
'''
Create human skeleton geometry
'''
geometries = []
joint_colors = [[255, 0, 0], [255, 85, 0], [255, 170,
0], [255, 255, 0],
[170, 255, 0], [85, 255, 0], [0, 255, 0], [0, 255, 85],
[0, 255, 170], [0, 255, 255], [0, 170, 255],
[0, 85, 255], [0, 0, 255], [85, 0, 255], [170, 0, 255],
[255, 0, 255], [255, 0, 170], [255, 0, 85]]
limb_colors = [
[0, 255, 0],
[0, 255, 85],
[0, 255, 170],
[0, 255, 255],
[0, 170, 255],
[0, 85, 255],
[255, 0, 0],
[255, 85, 0],
[255, 170, 0],
[255, 255, 0.],
[255, 0, 85],
[170, 255, 0],
[85, 255, 0],
[170, 0, 255.],
[0, 0, 255],
[0, 0, 255],
[255, 0, 255],
[170, 0, 255],
[255, 0, 170],
]
for i, (jointType, color) in enumerate(zip(JointType, joint_colors)):
if np.all(self.joints[jointType.name] != 0):
sphere = o3.create_mesh_sphere(radius=10.0)
pos = np.concatenate(
[np.asarray(self.joints[jointType.name]), [1]])
# create translation matrix
Tm = np.array([[1, 0, 0, 0], [0, 1, 0, 0], [0, 0, 1, 0],
pos]).T
# move sphere
sphere.transform(Tm)
# paint sphere
sphere.paint_uniform_color([v / 255 for v in color])
geometries.append(sphere)
for i, (limb,
color) in enumerate(zip(params['limbs_point'], limb_colors)):
if i != 9 and i != 13: # don't show ear-shoulder connection
l1 = limb[0].name
l2 = limb[1].name
pl1 = self.joints[l1]
pl2 = self.joints[l2]
if np.any(pl1) and np.any(pl2):
dist = np.linalg.norm(pl1 - pl2)
midpoint = np.concatenate([(pl1 + pl2) / 2, [1]])
# orientation of cylindar (z axis)
vec_cylindar = np.array([0, 0, 1])
# normalized vector of the two points connected
norm = self.normalize(pl2 - pl1)
# get rotation matrix
R = self.get_rotation(vec_cylindar, norm).T
# create translation matrix
tm1 = np.concatenate([R[0], [0]])
tm2 = np.concatenate([R[1], [0]])
tm3 = np.concatenate([R[2], [0]])
Tm = np.array([tm1, tm2, tm3, midpoint]).T
# create the cylinder
cylinder = o3.create_mesh_cylinder(radius=5.0, height=dist)
# move the cylinder
cylinder.transform(Tm)
# paint the cylinder
cylinder.paint_uniform_color([v / 255 for v in color])
geometries.append(cylinder)
return geometries
def feature_registration(source, target, MIN_MATCH_COUNT=9): #12
"""
Obtain the rigid transformation from source to target
first find correspondence of color images by performing fast registration
using SIFT features on color images.
The corresponding depth values of the matching keypoints is then used to
obtain rigid transformation through a ransac process.
Parameters
----------
source : ((n,m) uint8, (n,m) float)
The source color image and the corresponding 3d pointcloud combined in a list
target : ((n,m) uint8, (n,m) float)
The target color image and the corresponding 3d pointcloud combined in a list
MIN_MATCH_COUNT : int
The minimum number of good corresponding feature points for the algorithm to
trust the pairwise registration result with feature matching only
Returns
----------
transform: (4,4) float or None
The homogeneous rigid transformation that transforms source to the target's
frame
if None, registration result using feature matching only cannot be trusted
either due to no enough good matching feature points are found, or the ransac
process does not return a solution
"""
print("Apply SIFT registration")
rgb_src, pts_src, mask_src, ppmap_src = source
rgb_des, pts_des, mask_des, ppmap_des = target
# pts_des.points = o3d.utility.Vector3dVector(np.asarray(pts_des.points) + 0.2)
# Initiate SIFT detector
sift = cv2.xfeatures2d.SIFT_create()
# Find the keypoints and descriptors with SIFT
kp1, des1 = sift.detectAndCompute(rgb_src, None)
kp2, des2 = sift.detectAndCompute(rgb_des, None)
# Find good mathces
bf = cv2.BFMatcher()
matches = bf.knnMatch(des1, des2, k=2)
good = []
for m, n in matches:
if m.distance < 0.7 * n.distance:
good.append(m)
# If number of good matching feature point is less than the MIN_MATCH_COUNT
if len(good) < MIN_MATCH_COUNT:
print('Not enough matches: {}'.format(len(good)))
return None, None
print('Number good matches: {}'.format(len(good)))
src_pts = np.float32([kp1[m.queryIdx].pt for m in good]).reshape(-1, 1, 2)
dst_pts = np.float32([kp2[m.trainIdx].pt for m in good]).reshape(-1, 1, 2)
M, mask = cv2.findHomography(src_pts, dst_pts, cv2.RANSAC, 5.0)
matchesMask = mask.ravel().tolist()
'''
# Visualize matches in the image
h = rgb_src.shape[0]
w = rgb_src.shape[1]
pts = np.float32([[0, 0], [0, h - 1], [w - 1, h - 1], [w - 1, 0]]).reshape(-1, 1, 2)
dst = cv2.perspectiveTransform(pts, M)
img2 = cv2.polylines(rgb_des, [np.int32(dst)], True, 255, 3, cv2.LINE_AA)
draw_params = dict(matchColor=(0, 255, 0), # draw matches in green color
singlePointColor=None,
matchesMask=matchesMask, # draw only inliers
flags=2)
img3 = cv2.drawMatches(rgb_src, kp1, img2, kp2, good, None, **draw_params)
plt.imshow(img3, 'gray'), plt.show()
'''
bad_match_index = np.where(np.array(matchesMask) == 0)
src_index = np.vstack(src_pts).squeeze()
src_index = np.delete(src_index, tuple(bad_match_index[0]), axis=0)
src_index[:, [0, 1]] = src_index[:, [1, 0]]
src_index = tuple(src_index.T.astype(np.int32))
dst_index = np.vstack(dst_pts).squeeze()
dst_index = np.delete(dst_index, tuple(bad_match_index[0]), axis=0)
dst_index[:, [0, 1]] = dst_index[:, [1, 0]]
dst_index = tuple(dst_index.T.astype(np.int32))
src_good = []
dst_good = []
for i in range(len(src_index[0])):
u_s = src_index[0][i]
v_s = src_index[1][i]
u_d = dst_index[0][i]
v_d = dst_index[1][i]
if ppmap_src[u_s, v_s] > 0 and ppmap_des[u_d, v_d] > 0:
pt_s = np.asarray(pts_src.points)[ppmap_src[u_s, v_s]]
pt_d = np.asarray(pts_des.points)[ppmap_des[u_s, v_d]]
src_good.append(pt_s)
dst_good.append(pt_d)
# Get rigid transforms between two sets of feature points through RANSAC
transform = match_ransac(np.asarray(src_good), np.asarray(dst_good))
if transform is None:
print('RANSAC returned None')
return None, None
#'''
# Visualize the matches in 3D
print('3D SIFT visualization')
print(np.asarray(transform).reshape((4, 4)))
# Create visualizer
vis = o3d.visualization.Visualizer()
vis.create_window()
pts1 = copy.deepcopy(pts_src)
pts1.paint_uniform_color([1.0, 0.0, 0.0])
vis.add_geometry(pts1)
pts2 = copy.deepcopy(pts_des)
# pts2.points = o3d.utility.Vector3dVector(np.asarray(pts2.points) + 0.2)
pts2.paint_uniform_color([0.0, 0.0, 1.0])
vis.add_geometry(pts2)
pts_tf = copy.deepcopy(pts_src)
pts_tf.transform(transform)
pts_tf.paint_uniform_color([0.0, 1.0, 0.0])
vis.add_geometry(pts_tf)
points = []
lines = []
line_colors = []
for i in range(len(src_index[0])):
u_s = src_index[0][i]
v_s = src_index[1][i]
u_d = dst_index[0][i]
v_d = dst_index[1][i]
if ppmap_src[u_s, v_s] > 0 and ppmap_des[u_d, v_d] > 0:
pt_s = np.asarray(pts1.points)[ppmap_src[u_s, v_s]]
pt_d = np.asarray(pts2.points)[ppmap_des[u_s, v_d]]
points.append(pt_s)
points.append(pt_d)
lines.append([len(points) - 1, len(points) - 2])
line_colors.append([0.0, 1.0, 0.0])
mm = o3d.create_mesh_sphere(radius=0.0025)
mm.compute_vertex_normals()
mm.paint_uniform_color([1., 0., 1.])
tt = np.eye(4)
tt[0:3, 3] = pt_s
mm.transform(tt)
vis.add_geometry(mm)
mm = o3d.create_mesh_sphere(radius=0.0025)
mm.compute_vertex_normals()
mm.paint_uniform_color([0., 1., 1.])
tt = np.eye(4)
tt[0:3, 3] = pt_d
mm.transform(tt)
vis.add_geometry(mm)
line_set = o3d.geometry.LineSet()
line_set.points = o3d.utility.Vector3dVector(points)
line_set.lines = o3d.utility.Vector2iVector(lines)
line_set.colors = o3d.utility.Vector3dVector(line_colors)
vis.add_geometry(line_set)
# End visualizer
vis.run()
vis.destroy_window()
#'''
# Return transform
return transform, o3d.registration.get_information_matrix_from_point_clouds(
pts_src, pts_des, max_correspondence_distance_fine,
np.array(transform))
def addSphere(center, radius):
sphere = o3d.create_mesh_sphere(radius).transform(pose(center))
sphere.paint_uniform_color([0.1, 0.1, 0.7])
sphere.compute_vertex_normals()
return sphere
