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