def __init__(self, points, polyline=True, circle=None):
"""Constructs an Obstacle.
Args:
:type points: point array for polyline obstacle
:type polyline: if obstacle a polyline or not
:type circle: if obstacle in circular shape or not
"""
data = DebugData()
if polyline:
polydata = self.add_vtk_polygon(points)
polydata = filterUtils.appendPolyData([polydata])
elif circle is not None:
center = [0, 0, 0]
axis = [0, 0, 1] # Upright cylinder.
data.addCylinder(center, axis, 0.3, 0.1)
polydata = data.getPolyData()
else:
center = [points[0], points[1], -1]
axis = [0, 0, 1] # Upright cylinder.
data.addCircle(center, axis, 5)
polydata = data.getPolyData()
self._state = np.array([0., 0., 0.])
self.altitude = 0.
self._velocity = 0.
self._raw_polydata = polydata
self._polydata = polydata
def __init__(self, geomDatas):
polyDatas = []
for geomData in geomDatas:
polyDatas.extend(Geometry.createPolyDataForGeometry(geomData))
self.polyData = filterUtils.appendPolyData(polyDatas)
if geomDatas:
self.color = geomDatas[0].get("color", DEFAULT_COLOR)
else:
self.color = DEFAULT_COLOR
def __init__(self, geomDatas):
polyDatas = []
for geomData in geomDatas:
polyDatas.extend(Geometry.createPolyDataForGeometry(geomData))
self.polyData = filterUtils.appendPolyData(polyDatas)
if geomDatas:
self.color = geomDatas[0].get("color", DEFAULT_COLOR)
else:
self.color = DEFAULT_COLOR
def onMergeIntoPointCloud():
allPointClouds = om.findObjectByName("point clouds")
if allPointClouds:
allPointClouds = [
i.getProperty("Name") for i in allPointClouds.children()
]
sel = QtGui.QInputDialog.getItem(
None,
"Point Cloud Merging",
"Pick point cloud to merge into:",
allPointClouds,
current=0,
editable=False,
)
sel = om.findObjectByName(sel)
# Make a copy of each in same frame
polyDataInto = vtk.vtkPolyData()
polyDataInto.ShallowCopy(sel.polyData)
if sel.getChildFrame():
polyDataInto = segmentation.transformPolyData(
polyDataInto,
sel.getChildFrame().transform)
polyDataFrom = vtk.vtkPolyData()
polyDataFrom.DeepCopy(pointCloudObj.polyData)
if pointCloudObj.getChildFrame():
polyDataFrom = segmentation.transformPolyData(
polyDataFrom,
pointCloudObj.getChildFrame().transform)
# Actual merge
append = filterUtils.appendPolyData([polyDataFrom, polyDataInto])
if sel.getChildFrame():
polyDataInto = segmentation.transformPolyData(
polyDataInto,
sel.getChildFrame().transform.GetInverse())
# resample
append = segmentationroutines.applyVoxelGrid(append, 0.01)
append = segmentation.addCoordArraysToPolyData(append)
# Recenter the frame
sel.setPolyData(append)
t = vtk.vtkTransform()
t.PostMultiply()
t.Translate(filterUtils.computeCentroid(append))
segmentation.makeMovable(sel, t)
# Hide the old one
if pointCloudObj.getProperty("Name") in allPointClouds:
pointCloudObj.setProperty("Visible", False)
def updateCameraMesh():
scale = cameraObj.getChildFrame().getProperty('Scale') * 10.0
rayLength = scale
d = DebugData()
d.addCube(dimensions=[0.04, 0.08, 0.06], center=[-0.02, 0.0, 0.0], color=[1,0.5,0])
d.addLine([0.0, 0.0, 0.0], [0.01, 0.0, 0.0], radius=0.023, color=[1,0.5,0])
cameraModelMesh = d.getPolyData()
t = vtk.vtkTransform()
t.Scale(scale, scale, scale)
cameraModelMesh = filterUtils.transformPolyData(cameraModelMesh, t)
cameraMesh = getCameraFrustumMesh(depthScanner.view, rayLength)
cameraMesh = filterUtils.transformPolyData(cameraMesh, getCameraTransform(depthScanner.view.camera()).GetLinearInverse())
cameraMesh = filterUtils.appendPolyData([cameraMesh, cameraModelMesh])
cameraObj.setPolyData(cameraMesh)
def updateCameraMesh():
scale = cameraObj.getChildFrame().getProperty('Scale') * 10.0
rayLength = scale
d = DebugData()
d.addCube(dimensions=[0.04, 0.08, 0.06],
center=[-0.02, 0.0, 0.0],
color=[1, 0.5, 0])
d.addLine([0.0, 0.0, 0.0], [0.01, 0.0, 0.0],
radius=0.023,
color=[1, 0.5, 0])
cameraModelMesh = d.getPolyData()
t = vtk.vtkTransform()
t.Scale(scale, scale, scale)
cameraModelMesh = filterUtils.transformPolyData(cameraModelMesh, t)
cameraMesh = getCameraFrustumMesh(depthScanner.view, rayLength)
cameraMesh = filterUtils.transformPolyData(
cameraMesh,
getCameraTransform(depthScanner.view.camera()).GetLinearInverse())
cameraMesh = filterUtils.appendPolyData([cameraMesh, cameraModelMesh])
cameraObj.setPolyData(cameraMesh)
def onMergeIntoPointCloud():
allPointClouds = om.findObjectByName('point clouds')
if allPointClouds:
allPointClouds = [i.getProperty('Name') for i in allPointClouds.children()]
sel = QtGui.QInputDialog.getItem(None, "Point Cloud Merging", "Pick point cloud to merge into:", allPointClouds, current=0, editable=False)
sel = om.findObjectByName(sel)
# Make a copy of each in same frame
polyDataInto = vtk.vtkPolyData()
polyDataInto.ShallowCopy(sel.polyData)
if sel.getChildFrame():
polyDataInto = segmentation.transformPolyData(polyDataInto, sel.getChildFrame().transform)
polyDataFrom = vtk.vtkPolyData()
polyDataFrom.DeepCopy(pointCloudObj.polyData)
if pointCloudObj.getChildFrame():
polyDataFrom = segmentation.transformPolyData(polyDataFrom, pointCloudObj.getChildFrame().transform)
# Actual merge
append = filterUtils.appendPolyData([polyDataFrom, polyDataInto])
if sel.getChildFrame():
polyDataInto = segmentation.transformPolyData(polyDataInto, sel.getChildFrame().transform.GetInverse())
# resample
append = segmentationroutines.applyVoxelGrid(append, 0.01)
append = segmentation.addCoordArraysToPolyData(append)
# Recenter the frame
sel.setPolyData(append)
t = vtk.vtkTransform()
t.PostMultiply()
t.Translate(filterUtils.computeCentroid(append))
segmentation.makeMovable(sel, t)
# Hide the old one
if pointCloudObj.getProperty('Name') in allPointClouds:
pointCloudObj.setProperty('Visible', False)
def loadObjFile(filename, scaleFactor=0.001):
baseName = os.path.basename(filename)
folder = om.getOrCreateContainer(baseName)
meshes, actors = ioUtils.readObjMtl(filename)
print 'loaded %d meshes from file: %s' % (len(meshes), filename)
for i, mesh, actor in zip(xrange(len(meshes)), meshes, actors):
mesh = filterUtils.cleanPolyData(mesh)
mesh = filterUtils.computeNormals(mesh)
obj = vis.showPolyData(mesh, baseName + ' piece %d'%i, parent=folder)
obj.setProperty('Color', actor.GetProperty().GetColor())
obj.actor.SetTexture(actor.GetTexture())
pts = vnp.getNumpyFromVtk(obj.polyData, 'Points')
pts *= scaleFactor
print ' mesh %d, num points %d' % (i, obj.polyData.GetNumberOfPoints())
#if actor.GetTexture():
# showImage(actor.GetTexture().GetInput())
polyData = filterUtils.appendPolyData([obj.polyData for obj in folder.children()])
vis.showPolyData(polyData, baseName + ' merged', parent=folder, visible=False)
print ' total points:', polyData.GetNumberOfPoints()
# compute centroid and subtract from points to move the mesh to the origin
origin = np.array(filterUtils.computeCentroid(polyData))
t = transformUtils.frameFromPositionAndRPY(-origin, [0.0, 0.0, 0.0])
for obj in folder.children():
obj.setPolyData(filterUtils.transformPolyData(obj.polyData, t))
return folder.children()[-1].polyData
