class QVolumeViewWidget(QWidget): def __init__(self, parent=None): super(QVolumeViewWidget, self).__init__(parent) # set up vtk pipeline and create vtkWidget self.renw = vtk.vtkRenderWindow() self.iren = vtk.vtkGenericRenderWindowInteractor() self.iren.SetRenderWindow(self.renw) kw = {'rw': self.renw, 'iren': self.iren} self.vtkWidget = QVTKRenderWindowInteractor(parent, **kw) self.MainLayout = QVBoxLayout() self.MainLayout.addWidget(self.vtkWidget) self.setLayout(self.MainLayout) # self.vtkWidget = QVTKRenderWindowInteractor() self.ren = vtk.vtkRenderer() self.vtkWidget.GetRenderWindow().AddRenderer(self.ren) # add the orientation marker widget self.axes = vtk.vtkAxesActor() self.widget = vtk.vtkOrientationMarkerWidget() xyzLabels = ['R', 'A', 'S'] self.axes.SetXAxisLabelText(xyzLabels[0]) self.axes.SetYAxisLabelText(xyzLabels[1]) self.axes.SetZAxisLabelText(xyzLabels[2]) self.widget.SetOrientationMarker(self.axes) self.widget.SetInteractor(self.vtkWidget) self.widget.SetViewport(0.8, 0.0, 1, 0.3) self.widget.SetEnabled(True) self.widget.InteractiveOn() self.picker = vtk.vtkVolumePicker() def set_data_reader(self, reader): volumeMapper = vtk.vtkFixedPointVolumeRayCastMapper() # volumeMapper.SetBlendModeToMaximumIntensity() volumeMapper.SetInputData(reader.GetOutput()) volume = vtk.vtkVolume() volume.SetMapper(volumeMapper) self.ren.AddVolume(volume) def start_render(self): # start render and interactor self.vtkWidget.Initialize() self.vtkWidget.Start() self.ren.SetBackground(1, 1, 1) self.ren.ResetCamera()
class MainWindow(QtWidgets.QMainWindow): def __init__(self, stlScale = 0.001, parent = None): QtWidgets.QMainWindow.__init__(self, parent) self.frame = QtWidgets.QFrame() self.vl = QtWidgets.QVBoxLayout() self.vtkWidget = QVTKRenderWindowInteractor(self.frame) self.vl.addWidget(self.vtkWidget) self.ren = vtk.vtkRenderer() self.vtkWidget.GetRenderWindow().AddRenderer(self.ren) self.iren = self.vtkWidget.GetRenderWindow().GetInteractor() # Create sphere source = vtk.vtkSphereSource() source.SetCenter(0, 0, 0) source.SetRadius(0.003) sphereMapper = vtk.vtkPolyDataMapper() sphereMapper.SetInputConnection(source.GetOutputPort()) self.sphereActor = vtk.vtkActor() self.sphereActor.SetMapper(sphereMapper) self.sphereActor.GetProperty().SetColor(1, 0, 0) self.ren.AddActor(self.sphereActor) # Read in STL reader = vtk.vtkSTLReader() reader.SetFileName('/home/biomed/october_15_ws/src/dvrk_vision/defaults/femur.stl') scaler = vtk.vtkTransformFilter() if vtk.VTK_MAJOR_VERSION <= 5: scaler.SetInputConnection(reader.GetOutput()) else: scaler.SetInputConnection(reader.GetOutputPort()) scaleTransform = vtk.vtkTransform() scaleTransform.Identity() scaleTransform.Scale(stlScale, stlScale, stlScale) scaler.SetTransform(scaleTransform) # Create a mapper mapper = vtk.vtkPolyDataMapper() if vtk.VTK_MAJOR_VERSION <= 5: mapper.SetInput(scaler.GetOutput()) else: mapper.SetInputConnection(scaler.GetOutputPort()) # Create an actor self.actor = vtk.vtkActor() self.actor.SetMapper(mapper) # Read in camera registration scriptDirectory = os.path.dirname(os.path.abspath(__file__)) filePath = os.path.join(scriptDirectory, '..', '..', 'defaults', 'registration_params.yaml') with open(filePath, 'r') as f: data = yaml.load(f) self.camTransform = np.array(data['transform']) # Add point cloud self.pointCloud = VtkPointCloud() tf = np.linalg.inv(self.camTransform) transform = vtk.vtkTransform() transform.SetMatrix(tf.ravel()) self.pointCloud.vtkActor.SetPosition(transform.GetPosition()) self.pointCloud.vtkActor.SetOrientation(transform.GetOrientation()) self.ren.AddActor(self.pointCloud.vtkActor) self.ren.AddActor(self.actor) poseSub = rospy.Subscriber("/stereo/registration_pose", PoseStamped, self.poseCallback) self.ren.ResetCamera() self.frame.setLayout(self.vl) self.setCentralWidget(self.frame) self.vtkThread = QThread() self.actorMat = np.eye(4) self.thread = QThread() robot = psm('PSM2') self.worker = Worker(robot) self.worker.intReady.connect(self.onIntReady) self.worker.moveToThread(self.thread) self.worker.finished.connect(self.thread.quit) self.thread.started.connect(self.worker.procCounter) self.thread.start() self.started = False self.show() self.iren.Initialize() def saveData(self): print("Saving data in") np.savetxt("probed_points.txt", self.worker.probedPoints) np.savetxt("bone_transform.txt", self.actorMat) np.savetxt("camera_transform.txt", np.linalg.inv(self.camTransform)) def poseCallback(self, data): pos = data.pose.position rot = data.pose.orientation self.actorMat = transformations.quaternion_matrix([rot.x,rot.y,rot.z,rot.w]) self.actorMat[0:3,3] = [pos.x,pos.y,pos.z] transform = vtk.vtkTransform() transform.Identity() transform.SetMatrix(self.actorMat.ravel()) self.actor.SetPosition(transform.GetPosition()) self.actor.SetOrientation(transform.GetOrientation()) self.actor.VisibilityOn() if not self.started: self.ren.ResetCamera() self.started = True def onIntReady(self): mat = np.eye(4) mat[0:3,3] = self.worker.pos tf = np.linalg.inv(self.camTransform) mat = np.dot(tf, mat) transform = vtk.vtkTransform() transform.Identity() transform.SetMatrix(mat.ravel()) self.sphereActor.SetPosition(transform.GetPosition()) self.sphereActor.SetOrientation(transform.GetOrientation()) # Modify probedPoints self.pointCloud.clearPoints() for point in self.worker.probedPoints: self.pointCloud.addPoint(point) self.vtkWidget.GetRenderWindow().Render()
class CamTabsWidget(QWidget): frames_ready = pyqtSignal(QImage, QImage, QImage) def __init__(self, parent, path): super(QWidget, self).__init__(parent) self.__path = path self.__counter = 0 self.layout = QVBoxLayout(self) # initialize tab screen self.__tabs = QTabWidget() self.__tabs.blockSignals(True) # just for not showing the initial message self.__tabs.currentChanged.connect(self.on_change) self.__tab3D = QWidget() self.__tab2D = QWidget() # add tabs self.__tabs.addTab(self.__tab3D, "3D") self.__tabs.addTab(self.__tab2D, "2D") # create the content and layout of the tab with 3D cameras self.__set_tab3d_layout() # create the content and layout of the tab with 2D cameras self.__set_tab2d_layout() # set all objects that support the pipeline to create 3D images self.__set_3d_supporting_objects() # capture the cameras' frames to create a movie self.layout.addWidget(self.__tabs) self.__run_movie() self.__tabs.blockSignals(False) def __set_3d_supporting_objects(self): self.pd_collection = [] self.mapper_collection = [] self.actor_collection = [] self.np_array = [] self.cells_npy = [] self.timer_count = 0 self._n_coordinates = 0 self.align = rs.align(rs.stream.color) self.__iren = self.__vtkWidget.GetRenderWindow().GetInteractor() self.__iren.GetInteractorStyle().SetCurrentStyleToTrackballActor() self.__iren.GetInteractorStyle().SetCurrentStyleToTrackballCamera() self._timer = QTimer(self) self._timer.timeout.connect(self.timer_event) self.view_coordinates = [[0., .5, .5, 1.], [.5, .5, 1., 1.], [0., 0., .5, .5], [.5, 0., 1., .5]] def __set_tab3d_layout(self): frame = QFrame() # add a vtk-based window for interaction self.__vtkWidget = QVTKRenderWindowInteractor(frame) # add a label and a text box for input of the object id self.__lbl_3d_idx = QLabel('ID:', self) self.__txt_box_3d_idx = QLineEdit(self) # add a button to save 3D images self.__btn_save_3d_img = QPushButton('Save Image', self) self.__btn_save_3d_img.setToolTip('Save 3D image to the file.') self.__btn_save_3d_img.clicked.connect(self.save_3d_image) # set the layout of the tab which holds the 3D cameras self.__tab3D.setLayout(self.__create_3d_cams_grid_layout()) def __set_tab2d_layout(self): self.__lbl_2d_cam_1 = QLabel() self.__lbl_2d_cam_2 = QLabel() self.__lbl_2d_cam_3 = QLabel() self.__lbl_2d_cam_4 = QLabel() # add a label and a text box for input of the object id self.__lbl_2d_idx = QLabel('ID:', self) self.__txt_box_2d_idx = QLineEdit(self) # add a button to save 3D images self.__btn_save_2d_img = QPushButton('Save Image', self) self.__btn_save_2d_img.setToolTip('Save 2D image to the file.') self.__btn_save_2d_img.clicked.connect(self.save_2d_image) # set the layout of the tab which holds the 2D cameras self.__tab2D.setLayout(self.__create_2d_cams_grid_layout()) def get_txt_box_2d_idx(self): return self.__txt_box_2d_idx def get_txt_box_3d_idx(self): return self.__txt_box_3d_idx def __run_movie(self): cam_counter = 0 for pipe in pipelines: frame_set = pipe.wait_for_frames() # Wait for a coherent color frame # frames = None # real_sense_cam.get_pipeline().wait_for_frames() # Align the depth frame to color frame aligned_frames = self.align.process(frame_set) depth_frame = aligned_frames.get_depth_frame() color_frame = aligned_frames.get_color_frame() color_image = np.asanyarray(color_frame.get_data()) color_image = color_image.reshape((color_image.shape[0] * color_image.shape[1], 3)) # self._colors.SetNumberOfTuples(color_image.shape[0]) colors = vtk.vtkUnsignedCharArray() colors.SetNumberOfComponents(3) # colors.SetName("Colors") current_pd = vtk.vtkPolyData() self.pd_collection.append(current_pd) colors.SetArray(vtk_np.numpy_to_vtk(color_image), color_image.shape[0] * color_image.shape[1], 1) current_pd.GetPointData().SetScalars(colors) pc = rs.pointcloud() point_cloud = pc.calculate(depth_frame) pc.map_to(color_frame) v, t = point_cloud.get_vertices(), point_cloud.get_texture_coordinates() vertices = np.asanyarray(v).view(np.float32).reshape(-1, 3) # xyz self._n_coordinates = vertices.shape[0] points = vtk.vtkPoints() cells = vtk.vtkCellArray() points.SetData(vtk_np.numpy_to_vtk(vertices)) cells_npy = np.vstack([np.ones(self._n_coordinates, dtype=np.int64), np.arange(self._n_coordinates, dtype=np.int64)]).T.flatten() cells.SetCells(self._n_coordinates, vtk_np.numpy_to_vtkIdTypeArray(cells_npy)) self.pd_collection[cam_counter].SetPoints(points) self.pd_collection[cam_counter].SetVerts(cells) mapper = vtk.vtkPolyDataMapper() self.mapper_collection.append(mapper) self.mapper_collection[cam_counter].SetInputData(self.pd_collection[cam_counter]) transform = vtk.vtkTransform() transform.SetMatrix(flip_transform) actor = vtk.vtkActor() self.actor_collection.append(actor) self.actor_collection[cam_counter].SetMapper(self.mapper_collection[cam_counter]) self.actor_collection[cam_counter].GetProperty().SetRepresentationToPoints() self.actor_collection[cam_counter].SetUserTransform(transform) current_ren = vtk.vtkRenderer() current_ren.GetActiveCamera() # set viewports if the number of cams ara greater than one if len(pipelines) > 1: current_ren.SetViewport(self.view_coordinates[cam_counter]) current_ren.AddActor(self.actor_collection[cam_counter]) self.__vtkWidget.GetRenderWindow().AddRenderer(current_ren) cam_counter += 1 self.__iren.AddObserver('TimerEvent', self.update_poly_data) dt = 30 # ms self.__iren.CreateRepeatingTimer(dt) def update_poly_data(self, obj=None, event=None): cam_counter = 0 rgb_cam_images = [] for pipe in pipelines: frame_set = pipe.wait_for_frames() # Wait for a coherent color frame # frames = real_sense_cam.get_pipeline().wait_for_frames() # Align the depth frame to color frame aligned_frames = self.align.process(frame_set) depth_frame = aligned_frames.get_depth_frame() color_frame = aligned_frames.get_color_frame() pc = rs.pointcloud() point_cloud = pc.calculate(depth_frame) pc.map_to(color_frame) v, t = point_cloud.get_vertices(), point_cloud.get_texture_coordinates() vertices = np.asanyarray(v).view(np.float32).reshape(-1, 3) # xyz color_image = np.asanyarray(color_frame.get_data()) rgb_cam_images.append(np_color_img_to_q_image(color_image)) color_image = color_image.reshape((color_image.shape[0] * color_image.shape[1], 3)) colors = vtk.vtkUnsignedCharArray() colors.SetNumberOfComponents(3) colors.SetArray(vtk_np.numpy_to_vtk(color_image), color_image.shape[0] * color_image.shape[1], 1) self.pd_collection[cam_counter].GetPointData().SetScalars(colors) points = vtk.vtkPoints() cells = vtk.vtkCellArray() points.SetData(vtk_np.numpy_to_vtk(vertices)) cells_npy = np.vstack([np.ones(self._n_coordinates, dtype=np.int64), np.arange(self._n_coordinates, dtype=np.int64)]).T.flatten() cells.SetCells(self._n_coordinates, vtk_np.numpy_to_vtkIdTypeArray(cells_npy)) self.pd_collection[cam_counter].SetPoints(points) self.pd_collection[cam_counter].SetVerts(cells) self.pd_collection[cam_counter].Modified() cam_counter += 1 self.frames_ready.emit(rgb_cam_images[0], rgb_cam_images[1], rgb_cam_images[2]) self.__iren.GetRenderWindow().Render() # print(self.timer_count) self.timer_count += 1 def __create_2d_cams_grid_layout(self): layout = QGridLayout() layout_idx = QHBoxLayout() layout_idx.addWidget(self.__lbl_2d_idx) layout_idx.addWidget(self.__txt_box_2d_idx) # add all widgets to the layout # add four labels of images, one for each camera layout.addWidget(self.__lbl_2d_cam_1, 0, 0, 1, 1) layout.addWidget(self.__lbl_2d_cam_2, 0, 1, 1, 1) layout.addWidget(self.__lbl_2d_cam_3, 1, 0, 1, 1) layout.addWidget(self.__lbl_2d_cam_4, 1, 1, 1, 1) # add a label and a text box to input the object id # layout.addWidget(self.__lbl_2d_idx, 2, 0, 1, 1) # layout.addWidget(self.__txt_box_2d_idx, 2, 1, 1, 1) layout.addLayout(layout_idx, 2, 0, 1, 2) # add a save button layout.addWidget(self.__btn_save_2d_img, 3, 0, 1, 2) # could we create a shared button to save all images at once? return layout def __create_3d_cams_grid_layout(self): layout = QGridLayout() layout.addWidget(self.__vtkWidget, 0, 0, 1, 2) # add a label and a text box to input the object id layout.addWidget(self.__lbl_3d_idx, 1, 0, 1, 1) layout.addWidget(self.__txt_box_3d_idx, 1, 1, 1, 1) # add a save button layout.addWidget(self.__btn_save_3d_img, 2, 0, 1, 2) return layout def timer_event(self): self.__iren.TimerEvent() def create_timer(self, obj, evt): self._timer.start(0) def destroy_timer(self, obj, evt): self._timer.stop() return 1 def get_iren(self): return self.__iren @QtCore.pyqtSlot(QImage, QImage, QImage) def receive_frame(self, rgb_1, rgb_2, rgb_3): ratio = 1. p_cam1 = QPixmap.fromImage(rgb_1) p_cam2 = QPixmap.fromImage(rgb_2) p_cam3 = QPixmap.fromImage(rgb_3) # p_cam4 = QPixmap.fromImage(rgb_4) w_cam1, h_cam1 = p_cam1.width() * ratio, p_cam1.height() * ratio w_cam2, h_cam2 = p_cam1.width() * ratio, p_cam1.height() * ratio w_cam3, h_cam3 = p_cam1.width() * ratio, p_cam1.height() * ratio # w_cam4 = self.__lbl_2d_cam_1.width() # h_cam4 = self.__lbl_2d_cam_1.height() self.__lbl_2d_cam_1.setPixmap(p_cam1.scaled(h_cam1, w_cam1, PyQt5.QtCore.Qt.KeepAspectRatio)) self.__lbl_2d_cam_2.setPixmap(p_cam2.scaled(h_cam1, w_cam1, PyQt5.QtCore.Qt.KeepAspectRatio)) self.__lbl_2d_cam_3.setPixmap(p_cam3.scaled(h_cam1, w_cam1, PyQt5.QtCore.Qt.KeepAspectRatio)) # self.__lbl_2d_cam_4.setPixmap(QPixmap.fromImage(rgb_4)) def on_change(self, i): if i == 0: # 3d tab self.__txt_box_3d_idx.setText(self.__txt_box_2d_idx.text()) else: self.__txt_box_2d_idx.setText(self.__txt_box_3d_idx.text()) # QMessageBox.information(self, # "Tab Index Changed!", # "Current Tab Index: %d" % i) # changed! def save_2d_image(self): cam_1_pixmap = self.__lbl_2d_cam_1.pixmap() cam_2_pixmap = self.__lbl_2d_cam_2.pixmap() cam_3_pixmap = self.__lbl_2d_cam_3.pixmap() # cam_4_pixmap = self.__lbl_2d_cam_4.pixmap() # set the correct path to save the images files cam_1_pixmap.save("") cam_2_pixmap.save("") cam_3_pixmap.save("") # cam_4_pixmap.save("") def save_3d_image(self): for i in range(nr_devices): writer = vtkXMLPolyDataWriter() path_cam = self.__path + serial_numbers[i] + "/3D/" path_exists = os.path.isdir(path_cam) today_str = datetime.now().strftime("%H:%M:%S.%f") if not path_exists: os.makedirs(path_cam) f_name = path_cam + self.__txt_box_3d_idx.text() + "_img_" + str(self.__counter) + "_" + today_str + ".vtp" writer.SetFileName(f_name) writer.SetInputData(self.pd_collection[i]) writer.Write() self.__counter += 1
class MyWindow(QtGui.QMainWindow): def __init__(self): super(MyWindow, self).__init__() os.chdir(functionPath) uic.loadUi("registration_gui.ui", self) os.chdir(startPath) # Connect buttons to functions # import moving data self.ptcldMovingButton.clicked.connect(self.import_data) # import fixed data self.ptcldFixedButton.clicked.connect(self.import_data) # execute qr_register function self.registrationButton.clicked.connect(self.register) self.vl = QtGui.QVBoxLayout() self.vtkWidget = QVTKRenderWindowInteractor(self.vtkFrame) self.vl.addWidget(self.vtkWidget) self.ren = vtk.vtkRenderer() self.vtkWidget.GetRenderWindow().AddRenderer(self.ren) self.iren = self.vtkWidget.GetRenderWindow().GetInteractor() self.actor_moving = vtk.vtkActor() self.moving_color = (0, 0.2) self.actor_fixed = vtk.vtkActor() self.fixed_color = (.8, 1) self.ren.AddActor(self.actor_moving) self.ren.AddActor(self.actor_fixed) self.ren.ResetCamera() self.vtkFrame.setLayout(self.vl) self.show() self.iren.Initialize() self.iren.SetInteractorStyle(vtk.vtkInteractorStyleTrackballCamera()) def _readFile(self, filename): # Takes filename as string and returns polyData extension = os.path.splitext(filename)[1].lower() print filename # # Read PLY into vtkPoints # if extension == ".ply" or extension == ".obj" or extension == ".stl": # if (extension == ".ply"): # reader = vtk.vtkPLYReader() # if (extension == ".obj"): # reader = vtk.vtkOBJReader() # if (extension == ".stl"): # reader = vtk.vtkSTLReader() # reader.SetFileName(filename) # reader.Update() # points = reader.GetOutput().GetPoints() # vertices = vtk.vtkCellArray() # for i in range(0,reader.GetOutput().GetNumberOfPoints()): # vertices.InsertNextCell(1) # vertices.InsertCellPoint(i) # # Add all generated data to polydata # polydata = vtk.vtkPolyData() # polydata.SetPoints(points) # polydata.SetVerts(vertices) # Read TXT into vtkPoints if extension == ".txt": textReader = vtk.vtkSimplePointsReader() textReader.SetFileName(filename) textReader.Update() polydata = textReader.GetOutput() else: print("input must be a .txt file") return None # raise InputError("file must be ply, stl, obj or txt") print("Bounds: ", polydata.GetBounds()) return polydata def _updateActorPolydata(self, actor, polydata, color): # Modifies an actor with new polydata bounds = polydata.GetBounds() # Generate colors colors = vtk.vtkElevationFilter() #colors.SetInputData(polydata) if vtk.VTK_MAJOR_VERSION <= 5: colors.SetInputConnection(polydata.GetProducerPort()) else: colors.SetInputData(polydata) colors.SetLowPoint(0, 0, bounds[5]) colors.SetHighPoint(0, 0, bounds[4]) colors.SetScalarRange(color) # Visualization mapper = actor.GetMapper() if mapper == None: mapper = vtk.vtkPolyDataMapper() if vtk.VTK_MAJOR_VERSION <= 5: mapper.SetInput(colors.GetOutput()) else: colors.Update() mapper.SetInputData(colors.GetOutput()) actor.SetMapper(mapper) transform = vtk.vtkTransform() actor.SetPosition(transform.GetPosition()) actor.SetOrientation(transform.GetOrientation()) size = 4 / len(str(polydata.GetNumberOfPoints())) + 1 actor.GetProperty().SetPointSize(size) # Function for opening .txt file and import the file path into the lineEdit def import_data(self): cwd = os.getcwd() fname = QtGui.QFileDialog.getOpenFileName(self, "Open file", cwd) if (_QT_VERSION >= 5): # QT5 stores filename in a tuple instead of just a string fname = fname[0] filename = str(fname) if fname == "": print("Using previous file") return sending_button = str(self.sender().objectName()) polydata = self._readFile(filename) if polydata == None: print "File not readable" return if sending_button == "ptcldMovingButton": self.ptcldMovingText.setText(filename) self._updateActorPolydata(self.actor_moving, polydata, self.moving_color) elif sending_button == "ptcldFixedButton": self.ptcldFixedText.setText(filename) self._updateActorPolydata(self.actor_fixed, polydata, self.fixed_color) self.ren.ResetCamera() self.iren.Render() # Take file path from the lineEdit and perform registration using those dataset def register(self): transform = vtk.vtkTransform() self.actor_moving.SetPosition(transform.GetPosition()) self.actor_moving.SetOrientation(transform.GetOrientation()) self.iren.Render() print("Registration starts\n") print(str(self.ptcldMovingText.text())) print(str(self.ptcldFixedText.text())) maxIter = self.maxIterations.value() inlierRatio = self.inlierRatio.value() windowSize = self.windowSize.value() rotTolerance = self.rotationTolerance.value() transTolerance = self.translationTolerance.value() b_string1 = str(self.ptcldMovingText.text()).encode("utf-8") b_string2 = str(self.ptcldFixedText.text()).encode("utf-8") output, error = register_txt(b_string1, b_string2, inlierRatio, maxIter, windowSize, rotTolerance, transTolerance) matrix = npMatrixToVtkMatrix( reg_params_to_transformation_matrix(output)) transform.SetMatrix(matrix) self.actor_moving.SetPosition(transform.GetPosition()) self.actor_moving.SetOrientation(transform.GetOrientation()) self.iren.Render()
class New_Render_Widget(QtGui.QWidget): def __init__(self, parent=None): parent.resize(765, 545) parent.setWindowTitle('Render scene') self.qvtkWidget = QVTKRenderWindowInteractor(parent) self.qvtkWidget.setGeometry(QtCore.QRect(0, 0, 761, 521)) self.qvtkWidget.setObjectName("qvtkWidget") self.PhotoButton = QtGui.QPushButton(parent) self.PhotoButton.setGeometry(QtCore.QRect(0, 523, 50, 20)) self.Label = QtGui.QLabel(parent) self.Label.setGeometry(QtCore.QRect(55, 523, 100, 20)) self.plainTextEdit = QtGui.QPlainTextEdit(parent) self.plainTextEdit.setGeometry(QtCore.QRect(160, 523, 50, 20)) self.XviewButton = QtGui.QPushButton(parent) self.XviewButton.setGeometry(QtCore.QRect(215, 523, 50, 20)) self.YviewButton = QtGui.QPushButton(parent) self.YviewButton.setGeometry(QtCore.QRect(270, 523, 50, 20)) self.ZviewButton = QtGui.QPushButton(parent) self.ZviewButton.setGeometry(QtCore.QRect(325, 523, 50, 20)) self.ResetCameraButton = QtGui.QPushButton(parent) self.ResetCameraButton.setGeometry(QtCore.QRect(380, 523, 80, 20)) self.BackgroundColorButton = QtGui.QPushButton(parent) self.BackgroundColorButton.setGeometry(QtCore.QRect(465, 523, 150, 20)) self.PhotoButton.setText( QtGui.QApplication.translate("SubWindow", "Photo !", None, QtGui.QApplication.UnicodeUTF8)) self.Label.setText( QtGui.QApplication.translate("MainWindow", "Set magnification", None, QtGui.QApplication.UnicodeUTF8)) self.XviewButton.setText( QtGui.QApplication.translate("MainWindow", "Xview", None, QtGui.QApplication.UnicodeUTF8)) self.YviewButton.setText( QtGui.QApplication.translate("MainWindow", "Yview", None, QtGui.QApplication.UnicodeUTF8)) self.ZviewButton.setText( QtGui.QApplication.translate("MainWindow", "Zview", None, QtGui.QApplication.UnicodeUTF8)) self.ResetCameraButton.setText( QtGui.QApplication.translate("MainWindow", "Reset Camera", None, QtGui.QApplication.UnicodeUTF8)) self.BackgroundColorButton.setText( QtGui.QApplication.translate("MainWindow", "Change background color", None, QtGui.QApplication.UnicodeUTF8)) self.renderer = vtk.vtkRenderer() self.renderer.SetBackground(1, 1, 1) self.renderer.ResetCamera() self.qvtkWidget.GetRenderWindow().AddRenderer(self.renderer) QtCore.QObject.connect(self.PhotoButton, QtCore.SIGNAL('clicked()'), self.photo) QtCore.QObject.connect(self.XviewButton, QtCore.SIGNAL('clicked()'), self.x_view) QtCore.QObject.connect(self.YviewButton, QtCore.SIGNAL('clicked()'), self.y_view) QtCore.QObject.connect(self.ZviewButton, QtCore.SIGNAL('clicked()'), self.z_view) QtCore.QObject.connect(self.ResetCameraButton, QtCore.SIGNAL('clicked()'), self.reset_camera) QtCore.QObject.connect(self.BackgroundColorButton, QtCore.SIGNAL('clicked()'), self.change_background_color) def add_actor(self, actor): try: self.renderer.AddActor(actor) except: self.renderer.AddVolume(actor) self.renderer.Render() def photo(self): magnif = str(self.plainTextEdit.toPlainText()) if magnif == "": magnif = 1 else: magnif = int(magnif) w2if = vtk.vtkWindowToImageFilter() w2if.Update() renderLarge = vtk.vtkRenderLargeImage() renderer = self.qvtkWidget.GetRenderWindow().GetRenderers( ).GetFirstRenderer() renderLarge.SetInput(renderer) renderLarge.SetMagnification(magnif) writer = vtk.vtkPNGWriter() writer.SetInputConnection(renderLarge.GetOutputPort()) writer.SetFileName( str(QtGui.QFileDialog.getOpenFileName(directory="./output"))) writer.Write() def x_view(self): try: self.renderer.GetActiveCamera().SetViewUp(1, 0, 0) self.renderer.ResetCamera() except: print "x_view" def y_view(self): try: self.renderer.GetActiveCamera().SetViewUp(0, 1, 0) self.renderer.ResetCamera() except: print "y_view" def z_view(self): try: self.renderer.GetActiveCamera().SetViewUp(0, 0, 1) self.renderer.ResetCamera() except: print "z_view" def reset_camera(self): try: self.renderer.ResetCamera() except: print "reset_camera" def change_background_color(self): try: self.renderer.SetBackground( self.qcolor2rgb(QtGui.QColorDialog.getColor())) except: print "change background color" def qcolor2rgb(self, qcolor): return [ qcolor.red() / 255.0, qcolor.green() / 255.0, qcolor.blue() / 255.0 ]
class MainWindow(Qt.QMainWindow): def __init__(self, parent=None): Qt.QMainWindow.__init__(self, parent) self.__counter = 0 self.__prefix = "/home/adriano/PycharmProjects/realsense_multicam/pics/" self.frame = Qt.QFrame() self.vtkWidget = QVTKRenderWindowInteractor(self.frame) self.pd_collection = [] self.mapper_collection = [] self.actor_collection = [] self.np_array = [] self.cells_npy = [] self.timer_count = 0 self._n_coordinates = 0 self.align = rs.align(rs.stream.color) self._iren = self.vtkWidget.GetRenderWindow().GetInteractor() self._iren.GetInteractorStyle().SetCurrentStyleToTrackballActor() self._iren.GetInteractorStyle().SetCurrentStyleToTrackballCamera() print(type(self._iren.GetInteractorStyle())) self._timer = QTimer(self) self.__label_idx = QLabel('ID:', self) self.__txt_box_idx = QLineEdit(self) self.__button = QPushButton('Save Image', self) self.__button.setToolTip('Save 3D image.') self.__button.clicked.connect(self.save_image) self._timer.timeout.connect(self.timer_event) self.view_coordinates = [[0., .5, .5, 1.], [.5, .5, 1., 1.], [0., 0., .5, .5], [.5, 0., 1., .5]] cam_counter = 0 for pipe in pipelines: frame_set = pipe.wait_for_frames() # Wait for a coherent color frame # frames = None # real_sense_cam.get_pipeline().wait_for_frames() # Align the depth frame to color frame aligned_frames = self.align.process(frame_set) depth_frame = aligned_frames.get_depth_frame() color_frame = aligned_frames.get_color_frame() color_image = np.asanyarray(color_frame.get_data()) color_image = color_image.reshape( (color_image.shape[0] * color_image.shape[1], 3)) # self._colors.SetNumberOfTuples(color_image.shape[0]) colors = vtk.vtkUnsignedCharArray() colors.SetNumberOfComponents(3) # colors.SetName("Colors") current_pd = vtk.vtkPolyData() self.pd_collection.append(current_pd) colors.SetArray(vtk_np.numpy_to_vtk(color_image), color_image.shape[0] * color_image.shape[1], 1) current_pd.GetPointData().SetScalars(colors) pc = rs.pointcloud() point_cloud = pc.calculate(depth_frame) pc.map_to(color_frame) v, t = point_cloud.get_vertices( ), point_cloud.get_texture_coordinates() vertices = np.asanyarray(v).view(np.float32).reshape(-1, 3) # xyz self._n_coordinates = vertices.shape[0] points = vtk.vtkPoints() cells = vtk.vtkCellArray() points.SetData(vtk_np.numpy_to_vtk(vertices)) cells_npy = np.vstack([ np.ones(self._n_coordinates, dtype=np.int64), np.arange(self._n_coordinates, dtype=np.int64) ]).T.flatten() cells.SetCells(self._n_coordinates, vtk_np.numpy_to_vtkIdTypeArray(cells_npy)) self.pd_collection[cam_counter].SetPoints(points) self.pd_collection[cam_counter].SetVerts(cells) mapper = vtk.vtkPolyDataMapper() self.mapper_collection.append(mapper) self.mapper_collection[cam_counter].SetInputData( self.pd_collection[cam_counter]) transform = vtk.vtkTransform() transform.SetMatrix(flip_transform) actor = vtk.vtkActor() self.actor_collection.append(actor) self.actor_collection[cam_counter].SetMapper( self.mapper_collection[cam_counter]) self.actor_collection[cam_counter].GetProperty( ).SetRepresentationToPoints() self.actor_collection[cam_counter].SetUserTransform(transform) current_ren = vtk.vtkRenderer() current_ren.GetActiveCamera() # set viewports if the number of cams ara greater than one if len(pipelines) > 1: current_ren.SetViewport(self.view_coordinates[cam_counter]) current_ren.AddActor(self.actor_collection[cam_counter]) self.vtkWidget.GetRenderWindow().AddRenderer(current_ren) cam_counter += 1 self._iren.AddObserver('TimerEvent', self.update_poly_data) dt = 30 # ms ide = self._iren.CreateRepeatingTimer(dt) self.frame.setLayout(self.__create_grid_layout()) self.setCentralWidget(self.frame) self.setWindowTitle("SmartUS Image Collector") self.__txt_box_idx.setFocus() self.show() self._iren.Initialize() self._iren.Start() def __create_grid_layout(self): layout = QGridLayout() layout.addWidget(self.vtkWidget, 0, 0, 1, 2) layout.addWidget(self.__label_idx, 1, 0, 1, 1) layout.addWidget(self.__txt_box_idx, 1, 1, 1, 1) layout.addWidget(self.__button, 2, 0, 1, 2) return layout def get_txt_box_idx(self): return self.__txt_box_idx def update_poly_data(self, obj=None, event=None): cam_counter = 0 for pipe in pipelines: frame_set = pipe.wait_for_frames() # Wait for a coherent color frame # frames = real_sense_cam.get_pipeline().wait_for_frames() # Align the depth frame to color frame aligned_frames = self.align.process(frame_set) depth_frame = aligned_frames.get_depth_frame() color_frame = aligned_frames.get_color_frame() pc = rs.pointcloud() point_cloud = pc.calculate(depth_frame) pc.map_to(color_frame) v, t = point_cloud.get_vertices( ), point_cloud.get_texture_coordinates() vertices = np.asanyarray(v).view(np.float32).reshape(-1, 3) # xyz color_image = np.asanyarray(color_frame.get_data()) color_image = color_image.reshape( (color_image.shape[0] * color_image.shape[1], 3)) colors = vtk.vtkUnsignedCharArray() colors.SetNumberOfComponents(3) colors.SetArray(vtk_np.numpy_to_vtk(color_image), color_image.shape[0] * color_image.shape[1], 1) self.pd_collection[cam_counter].GetPointData().SetScalars(colors) points = vtk.vtkPoints() cells = vtk.vtkCellArray() points.SetData(vtk_np.numpy_to_vtk(vertices)) cells_npy = np.vstack([ np.ones(self._n_coordinates, dtype=np.int64), np.arange(self._n_coordinates, dtype=np.int64) ]).T.flatten() cells.SetCells(self._n_coordinates, vtk_np.numpy_to_vtkIdTypeArray(cells_npy)) self.pd_collection[cam_counter].SetPoints(points) self.pd_collection[cam_counter].SetVerts(cells) self.pd_collection[cam_counter].Modified() cam_counter += 1 self._iren.GetRenderWindow().Render() # print(self.timer_count) self.timer_count += 1 def create_timer(self, obj, evt): self._timer.start(0) def destroy_timer(self, obj, evt): self._timer.stop() return 1 def timer_event(self): self._iren.TimerEvent() def save_image(self): today = datetime.now() today_str = today.strftime("%Y-%m-%d_%H:%M:%S.%f") writer = vtkXMLPolyDataWriter() writer.SetFileName(self.__prefix + "animal_" + self.__txt_box_idx.text() + "_img_" + str(self.__counter) + "_3D_" + today_str + ".vtp") writer.SetInputData(self.pd_collection[0]) writer.Write() self.__counter += 1