Exemplo n.º 1
0
 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)
Exemplo n.º 2
0
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
Exemplo n.º 4
0
    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
Exemplo n.º 5
0
 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
Exemplo n.º 6
0
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
Exemplo n.º 7
0
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])