def runCPDRegistration(self, sourceLM, sourceSLM, targetSLM, parameters): from open3d import geometry from open3d import utility sourceArrayCombined = np.append(sourceSLM, sourceLM, axis=0) targetArray = np.asarray(targetSLM) #Convert to pointcloud for scaling sourceCloud = geometry.PointCloud() sourceCloud.points = utility.Vector3dVector(sourceArrayCombined) targetCloud = geometry.PointCloud() targetCloud.points = utility.Vector3dVector(targetArray) cloudSize = np.max(targetCloud.get_max_bound() - targetCloud.get_min_bound()) targetCloud.scale(25 / cloudSize, center=False) sourceCloud.scale(25 / cloudSize, center=False) #Convert back to numpy for cpd sourceArrayCombined = np.asarray(sourceCloud.points, dtype=np.float32) targetArray = np.asarray(targetCloud.points, dtype=np.float32) registrationOutput = self.cpd_registration(targetArray, sourceArrayCombined, parameters["CPDIterations"], parameters["CPDTolerence"], parameters["alpha"], parameters["beta"]) deformed_array, _ = registrationOutput.register() #Capture output landmarks from source pointcloud fiducial_prediction = deformed_array[-len(sourceLM):] fiducialCloud = geometry.PointCloud() fiducialCloud.points = utility.Vector3dVector(fiducial_prediction) fiducialCloud.scale(cloudSize / 25, center=False) return np.asarray(fiducialCloud.points)
def visualize_pc(pc, mask=None): ''' pc - 3 x n mask - 1 x n, [0,1] ''' pc = pc.permute(1, 0) # n x 3 pc = pc.cpu().detach().numpy() pcd = geometry.PointCloud() pcd.points = utility.Vector3dVector(pc) if mask is not None: mask = mask / mask.max() #mask=mask.pow(0.5) #dilate the values mask = mask.expand(3, -1).permute(1, 0) #nx3 red = mask.new(mask.shape).zero_() red[:, 0] += 1.0 green = mask.new(mask.shape).zero_() green[:, 1] += 1.0 blue = mask.new(mask.shape).zero_() blue[:, 2] += 1.0 less = ((2 * mask).mul(green) + (1.0 - (2 * mask)).mul(blue)).mul( mask.lt(0.5).float()) more = (((mask - 0.5) * 2).mul(red) + (1.0 - ((mask - 0.5) * 2)).mul(green)).mul( mask.ge(0.5).float()) mask = less + more mask_max, _ = mask.max(dim=1, keepdim=True) mask = mask.div(mask_max) mask = mask.cpu().detach().numpy() pcd.colors = utility.Vector3dVector(mask) visualization.draw_geometries([pcd])
def __find_plane(self, x, y, z, Trans, Rot, mean_pt, debug=False): #mean_pt = mean_pt/np.linalg.norm(mean_pt) pts = np.array([x, y, z]).T print(pts.shape) t_size = np.min((500, pts.shape[0])) t_pts = np.random.choice(np.arange(pts.shape[0]), size=t_size, replace=False) pts = pts[t_pts] print(pts.shape) all_points = open3d.utility.Vector3dVector(pts) pc = geometry.PointCloud(all_points) plane, success_pts = pc.segment_plane(0.05, int(pts.shape[0] * 0.7), 500) plane = np.array(plane) # print("Plane:", pts.shape, len(success_pts)) if debug: print("Plane: ", plane) print("Mean pt: ", mean_pt) plane_debug = plane.copy() d = np.dot(mean_pt, plane[:3]) if d > 0: plane = -plane plane_ = plane[:3] #np.matmul(R.from_quat(Rot).as_dcm(), plane[:3].T) plane_[1] = 0 plane_ /= np.linalg.norm(plane_) y = np.array([0, -1, 0]) z = np.cross(plane_, y) rot = np.array([plane_, y, z]).T # print(rot, np.linalg.det(rot), R.from_dcm(rot).as_euler('ZYX', degrees=True)) orientation = R.from_dcm(rot).as_quat() if debug: self.debugPlanePlotter(pts, plane, orientation, position=mean_pt) normal_vec = Pose() normal_vec.position.x = plane_[0] normal_vec.position.y = plane_[1] # normal_vec.position.z = plane[2] #orientation = [0, 0, 0, 1]#self.__find_rot(plane[0:3]) #normal_vec.orientation.x = orientation[0] #normal_vec.orientation.y= orientation[1] #normal_vec.orientation.z= orientation[2] #normal_vec.orientation.w = orientation[3] vecs = PoseArray() vecs.poses.append(normal_vec) header = std_msgs.msg.Header() header.stamp = self.stamp_now #rospy.Time.now() header.frame_id = "orange" vecs.header = header self.__normal_vec_publisher.publish(vecs) # print(plane) return orientation
def __init__(self, model_orig_pcd, sample_pcd): self.model_pcd_original = model_orig_pcd self.XYZ_model = np.asarray(self.model_pcd_original.points) self.XYZ_min_model = np.min(self.XYZ_model, axis=0) self.XYZ_max_model = np.max(self.XYZ_model, axis=0) self.diameter_model = np.sqrt( np.square(self.XYZ_max_model[0] - self.XYZ_min_model[0]) + np.square(self.XYZ_max_model[1] - self.XYZ_min_model[1]) + np.square(self.XYZ_max_model[2] - self.XYZ_min_model[2])) self.model_pcd = geometry.PointCloud( ) # To keep resized model points that can match the size of a sample # point cloud. self.sample_pcd = sample_pcd
def loadAndScaleFiducials(self, fiducialPath, scaling): from open3d import geometry from open3d import utility sourceLandmarkNode = slicer.util.loadMarkups(fiducialPath) self.RAS2LPSTransform(sourceLandmarkNode) point = [0, 0, 0] sourceLandmarks_np = np.zeros( shape=(sourceLandmarkNode.GetNumberOfFiducials(), 3)) for i in range(sourceLandmarkNode.GetNumberOfFiducials()): sourceLandmarkNode.GetMarkupPoint(0, i, point) sourceLandmarks_np[i, :] = point slicer.mrmlScene.RemoveNode(sourceLandmarkNode) cloud = geometry.PointCloud() cloud.points = utility.Vector3dVector(sourceLandmarks_np) cloud.scale(scaling, center=False) fiducialVTK = self.convertPointsToVTK(cloud.points) return fiducialVTK
def _draw_points(points, vis, points_size=2, point_color=(0.5, 0.5, 0.5), mode='xyz'): """Draw points on visualizer. Args: points (numpy.array | torch.tensor, shape=[N, 3+C]): points to visualize. vis (:obj:`open3d.visualization.Visualizer`): open3d visualizer. points_size (int): the size of points to show on visualizer. Default: 2. point_color (tuple[float]): the color of points. Default: (0.5, 0.5, 0.5). mode (str): indicate type of the input points, avaliable mode ['xyz', 'xyzrgb']. Default: 'xyz'. Returns: tuple: points, color of each point. """ vis.get_render_option().point_size = points_size # set points size if isinstance(points, torch.Tensor): points = points.cpu().numpy() points = points.copy() pcd = geometry.PointCloud() if mode == 'xyz': pcd.points = o3d.utility.Vector3dVector(points[:, :3]) points_colors = np.tile(np.array(point_color), (points.shape[0], 1)) elif mode == 'xyzrgb': pcd.points = o3d.utility.Vector3dVector(points[:, :3]) points_colors = points[:, 3:6] # normalize to [0, 1] for open3d drawing if not ((points_colors >= 0.0) & (points_colors <= 1.0)).all(): points_colors /= 255.0 else: raise NotImplementedError pcd.colors = o3d.utility.Vector3dVector(points_colors) vis.add_geometry(pcd) return pcd, points_colors
def test_GFP(): from open3d import visualization, geometry, utility test_points, test_labels = load_h5(test_h5_path) nr_points = ModellingParameters.NUM_POINTS_UPSAMPLE model = GFPNet(nr_points) model.compile(optimizer='adam', loss='mean_squared_error', metrics=['mse', 'accuracy']) # print the model summary model.load_weights(net_params.PATH_TO_WEIGHTS) print(model.summary()) primitive_path = PATHS.PATH_TO_PRIMITIVES.CAR.root + "CarPrimitive_15_500.off" io_primitive = IoPrimitive(primitive_path) io_primitive.load_primitive(normalize=False) pathlist = Path(PATHS.NETWORK.root).glob('*.{0}'.format('off')) for path in pathlist: lidar_cloud_path = str(path) file_name = lidar_cloud_path.split("\\")[-1] label_path = PATHS.NETWORK.TEST_MODELLED + file_name cloud_path = PATHS.NETWORK.TEST_CLOUD + file_name observation_points = read_off_file(cloud_path) io_observation_cloud = IoObservation(observation_points) io_primitive.scale_relative_to(io_observation_cloud) io_primitive.align_primitive_on_z_axis(io_observation_cloud) io_primitive.compute_normals() modelled_primitive_points = read_off_file(label_path) io_modelled_primitive = IoObservation(modelled_primitive_points) eval_X, boolIndexes = make_samples(io_observation_cloud, io_primitive, io_modelled_primitive, usePrimitivePoints=False, generate_for='test') eval_X = upsample_point_set( sample_collection=eval_X, points_count=ModellingParameters.NUM_POINTS_UPSAMPLE) cloud_bef = geometry.PointCloud() cloud_bef.points = utility.Vector3dVector( np.asarray(io_primitive.point_cloud.points)) cloud_bef.normals = utility.Vector3dVector( np.asarray(io_primitive.point_cloud.normals)) cloud_bef.paint_uniform_color([255, 0, 0]) cloud_lidar = geometry.PointCloud() cloud_lidar.points = utility.Vector3dVector( np.asarray(io_observation_cloud.point_cloud)) cloud_lidar.paint_uniform_color([0, 0, 0]) cloud_modelled = geometry.PointCloud() cloud_modelled.points = utility.Vector3dVector( np.asarray(io_modelled_primitive.point_cloud.points)) cloud_modelled.paint_uniform_color([255, 255, 0]) visualization.draw_geometries([cloud_bef, cloud_lidar, cloud_modelled]) final_pts = [] idx = 0 for i in range(0, len(eval_X)): pred = eval_X[i].reshape(-1, nr_points, 3) points = model.predict(pred) final_pts.append(points) idx = i final_pts = np.reshape(final_pts, newshape=(len(final_pts), 3)) print('Final pts len : ', len(final_pts)) final_pts = [ point * ModellingParameters.CAR.SCALE for point in final_pts ] true_indexes = [i for i, val in enumerate(boolIndexes) if val] for i in true_indexes: cloud_bef.colors[i] = [0, 255, 0] import pclpy.pcl.point_types as ptype aux = ptype.PointXYZ() new_points = [] for i in range(0, len(final_pts)): val = io_primitive.point_cloud.points[true_indexes[i]] + ( final_pts[i] - ModellingParameters.NORMALIZATION_CENTER) aux.x = val[0] aux.y = val[1] aux.z = val[2] new_points.append(val) # transform_neighbor_points_for(i, aux, srcPrimitive, None) cloud_aft = geometry.PointCloud() cloud_aft.points = utility.Vector3dVector(new_points) cloud_aft.paint_uniform_color([0, 0, 255]) # cloud.normals = utility.Vector3dVector(cloud_points[:,3:6]) visualization.draw_geometries( [cloud_bef, cloud_aft, cloud_lidar, cloud_modelled])