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
