def rotate_inv_rpy(item, roll, pitch, yaw):
roll = np.deg2rad(roll)
pitch = np.deg2rad(pitch)
yaw = np.deg2rad(yaw)
matrix = np.array([[np.cos(roll)*np.cos(pitch),
np.cos(roll)*np.sin(pitch)*np.sin(yaw)-np.sin(roll)*np.cos(yaw),
np.cos(roll)*np.sin(pitch)*np.cos(yaw)+np.sin(roll)*np.sin(yaw),
0],
[np.sin(roll)*np.cos(pitch),
np.sin(roll)*np.sin(pitch)*np.sin(yaw)+np.cos(roll)*np.cos(yaw),
np.sin(roll)*np.sin(pitch)*np.cos(yaw)-np.cos(roll)*np.sin(yaw),
0],
[-np.sin(pitch),
np.cos(pitch)*np.sin(yaw),
np.cos(pitch)*np.cos(yaw),
0],
[0, 0, 0, 1]])
inverse = np.linalg.inv(matrix)
matrixList = inverse.flatten().tolist()
transform = Transform3D(matrixList)
item.applyTransform(transform, True)
def setTransform(self, tr):
"""Set the local transform for this object.
Must be a :class:`Transform3D <pyqtgraph.Transform3D>` instance. This transform
determines how the local coordinate system of the item is mapped to the coordinate
system of its parent."""
self.__transform = Transform3D(tr)
self.update()
def rotate_inv_ypr(item, roll, pitch, yaw):
r = np.deg2rad(roll)
p = np.deg2rad(pitch)
y = np.deg2rad(yaw)
matrix = [[cos(p) * cos(y),
-cos(p) * sin(y),
sin(p),
0],
[cos(r) * sin(y) + sin(p) * sin(r) * cos(y),
cos(r) * cos(y) - sin(p) * sin(r) * sin(y),
-cos(p) * sin(r),
0],
[sin(r) * sin(y) - sin(p) * cos(r) * cos(y),
sin(p) * cos(r) * sin(y) + sin(r) * cos(y),
cos(p) * cos(r),
0],
[0, 0, 0, 1]]
inverse = np.linalg.inv(matrix)
matrixList = inverse.flatten().tolist()
transform = Transform3D(matrixList)
item.applyTransform(transform, True)
def processFace(self, edgePair):
"""
Using ransac, calculates the normals for the currently selected edge pairs and displays a vectors
corresponding the orientatoin of the face.
"""
self.currentEdgePair = edgePair
# if self.grabberElement is not None:
# self.removeItem(self.grabberElement)
self.grabberElement = GrabberElement(self.context)
self.addItem(self.grabberElement, group=RenderGroup.Variable)
self.grabberElement.show()
length = 10
position = edgePair.getCenterPoint3D()
eigenVectors = edgePair.getOrientation()
# make eigen vector into 4x4 matrix for transform3d
eigenVectors = np.insert(eigenVectors, 3, 0, axis=0)
eigenVectors = np.insert(eigenVectors, 3, 0, axis=1)
eigenVectors = eigenVectors.T
eigenVectors = eigenVectors.flatten()
# set eigen[3][3] to 1
eigenVectors[15] = 1
transformation = Transform3D(eigenVectors)
self.grabberElement.applyTransform(transformation, local=False)
self.grabberElement.translate(position[0], position[1], position[2], local=False)
self.grabberElement.applyTransform(self.pointCloudView.transform(), local=False)
self.grabberElement.update()
axisItem = gl.GLAxisItem(size=QVector3D(length, length, length))
axisItem.applyTransform(self.grabberElement.transform(), local=False)
self.addItem(axisItem, RenderGroup.Variable)
self.grabberElement.setOpenGrabber(edgePair.getDistanceBetweenEdgePair())
return
def translate(self, dx, dy, dz, local=False):
"""
Translate the object by (*dx*, *dy*, *dz*) in its parent's coordinate system.
If *local* is True, then translation takes place in local coordinates.
"""
tr = Transform3D()
tr.translate(dx, dy, dz)
self.applyTransform(tr, local=local)
def scale(self, x, y, z, local=True):
"""
Scale the object by (*dx*, *dy*, *dz*) in its local coordinate system.
If *local* is False, then scale takes place in the parent's coordinates.
"""
tr = Transform3D()
tr.scale(x, y, z)
self.applyTransform(tr, local=local)
def viewTransform(self):
tr = self.__transform
p = self
while True:
p = p.parentItem()
if p is None:
break
tr = p.transform() * tr
return Transform3D(tr)
def rotate(self, angle, x, y, z, local=False):
"""
Rotate the object around the axis specified by (x,y,z).
*angle* is in degrees.
"""
tr = Transform3D()
tr.rotate(angle, x, y, z)
self.applyTransform(tr, local=local)
def __init__(self, parentItem=None):
QtCore.QObject.__init__(self)
self.__parent = None
self.__view = None
self.__children = set()
self.__transform = Transform3D()
self.__visible = True
self.setParentItem(parentItem)
self.setDepthValue(0)
self.__glOpts = {}
def viewTransform(self):
"""Return the transform mapping this item's local coordinate system to the
view coordinate system."""
tr = self.__transform
p = self
while True:
p = p.parentItem()
if p is None:
break
tr = p.transform() * tr
return Transform3D(tr)
def set_estimated_transform(self, orientation: Quaternion,
position: np.ndarray):
transform = Transform3D()
transform.translate(*position)
self.estimatedPositions.append(position)
self.estimatedLine.setData(pos=np.array(self.estimatedPositions))
qx, qy, qz, qw = orientation
q = QtGui.QQuaternion(qw, qx, qy, qz)
transform.rotate(q)
self.estimatedFrustum.setTransform(transform)
def triad(rows, cols, lx=1.0, ly=1.0, lz=1.0):
mdX = MyMeshData.arrow(rows, cols, length=lx, radius=0.1 * lx)
mdY = MyMeshData.arrow(rows, cols, length=ly, radius=0.1 * ly)
mdZ = MyMeshData.arrow(rows, cols, length=lz, radius=0.1 * lz)
# Orientation
trX = Transform3D()
trX.rotate(90., 0.0, 1.0, 0.0)
vertX = fn.transformCoordinates(trX, mdX.vertexes(), transpose=True)
nbrVerts = vertX.size / 3
trY = Transform3D()
trY.rotate(90., 1.0, 0.0, 0.0)
vertY = fn.transformCoordinates(trY, mdY.vertexes(), transpose=True)
verts = np.vstack((vertX, vertY, mdZ.vertexes()))
faces = np.vstack(
(mdX.faces(), mdY.faces() + nbrVerts, mdZ.faces() + 2 * nbrVerts))
nbrFaces = faces.shape[0]
#Colors x: red, y: green and z: blue
colors = np.zeros((nbrFaces, 4))
colors[0:nbrFaces / 3] = np.array([1.0, 0.0, 0.0, 0.6])
colors[nbrFaces / 3:nbrFaces * 2 / 3] = np.array([0.0, 1.0, 0.0, 0.6])
colors[nbrFaces * 2 / 3:nbrFaces] = np.array([0.0, 0.0, 1.0, 0.6])
return gl.MeshData(vertexes=verts, faces=faces, faceColors=colors)
def set_projected_points(self, points: np.ndarray, orientation: Quaternion,
position: np.ndarray):
if len(points) == 0:
return
transform = Transform3D()
transform.translate(*position)
qx, qy, qz, qw = orientation
q = QtGui.QQuaternion(qw, qx, qy, qz)
transform.rotate(q)
points = transform.map(points)
self.points.setData(pos=points.T)
def updateview_object(sr):
global fire
if fire:
xscale = sr.ui.scale_3d_x.value()
yscale = sr.ui.scale_3d_y.value()
zscale = sr.ui.scale_3d_z.value()
sr.scratchReader.options.put("view", "3d", "scale", "x", value=xscale)
sr.scratchReader.options.put("view", "3d", "scale", "y", value=yscale)
sr.scratchReader.options.put("view", "3d", "scale", "z", value=zscale)
for i in sr.ui.plot3d_pot_widget.items:
t = i.viewTransform().copyDataTo()
t[0] = xscale
t[5] = yscale
t[10] = zscale
i.setTransform(Transform3D(t))
def rotate_ypr(item, roll, pitch, yaw):
r = np.deg2rad(roll)
p = np.deg2rad(pitch)
y = np.deg2rad(yaw)
matrix = [cos(p) * cos(y),
-cos(p) * sin(y),
sin(p),
0,
cos(r) * sin(y) + sin(p) * sin(r) * cos(y),
cos(r) * cos(y) - sin(p) * sin(r) * sin(y),
-cos(p) * sin(r),
0,
sin(r) * sin(y) - sin(p) * cos(r) * cos(y),
sin(p) * cos(r) * sin(y) + sin(r) * cos(y),
cos(p) * cos(r),
0,
0, 0, 0, 1]
transform = Transform3D(matrix)
item.applyTransform(transform, True)
def rotate_rpy(item, roll, pitch, yaw):
roll = np.deg2rad(roll)
pitch = np.deg2rad(pitch)
yaw = np.deg2rad(yaw)
matrix = [np.cos(roll) * np.cos(pitch),
np.cos(roll) * np.sin(pitch) * np.sin(yaw) - np.sin(roll) * np.cos(yaw),
np.cos(roll) * np.sin(pitch) * np.cos(yaw) + np.sin(roll) * np.sin(yaw),
0,
np.sin(roll) * np.cos(pitch),
np.sin(roll) * np.sin(pitch) * np.sin(yaw) + np.cos(roll) * np.cos(yaw),
np.sin(roll) * np.sin(pitch) * np.cos(yaw) - np.cos(roll) * np.sin(yaw),
0,
-np.sin(pitch),
np.cos(pitch) * np.sin(yaw),
np.cos(pitch) * np.cos(yaw),
0,
0, 0, 0, 1]
transform = Transform3D(matrix)
item.applyTransform(transform, True)
def mrotate(self, angle, x, y, z):
tr = Transform3D()
tr.rotate(angle, x, y, z)
return tr
def initGrabberModel(self, grabberMeshName, arrowName, baseMeshName):
# initialize grabber Element
grabberRMeshData = self.context.loadModel(grabberMeshName.format("_r"))
grabberLMeshData = self.context.loadModel(grabberMeshName.format("_l"))
forwardArrowMeshData = self.context.loadModel(arrowName.format("_up"))
upArrowMeshData = self.context.loadModel(arrowName.format("_forward"))
grabberBaseMeshData = self.context.loadModel(baseMeshName)
grabberRMeshArguments = {
"meshdata": grabberRMeshData,
"color": (0.75, 0.75, 0.75, 0.85),
"edgeColor": (0.4, 0.4, 0.4, 1.0),
"drawEdge": True,
"drawFace": True,
"shader": 'shaded',
"glOptions": 'opaque'
}
grabberLMeshArguments = {
"meshdata": grabberLMeshData,
"color": (0.75, 0.75, 0.75, 0.85),
"edgeColor": (0.4, 0.4, 0.4, 1.0),
"drawEdge": True,
"drawFace": True,
"shader": 'shaded',
"glOptions": 'opaque'
}
baseArguments = {
"meshdata": grabberBaseMeshData,
"color": (0.75, 0.75, 0.75, 0.85),
"edgeColor": (0.4, 0.4, 0.4, 1.0),
"drawEdge": True,
"drawFace": True,
"shader": 'shaded',
"glOptions": 'opaque'
}
forwardArrowArguments = {
"meshdata": forwardArrowMeshData,
"color": (0.75, 0.75, 0.75, 0.3),
"edgeColor": (0.4, 0.4, 0.4, 1.0),
"drawEdge": True,
"drawFace": True,
"shader": 'balloon',
"glOptions": 'additive'
}
backwardArrowArguments = {
"meshdata": forwardArrowMeshData,
"color": (0.75, 0.75, 0.75, 0.3),
"edgeColor": (0.4, 0.4, 0.4, 1.0),
"drawEdge": True,
"drawFace": True,
"shader": 'balloon',
"glOptions": 'additive'
}
upArrowArguments = {
"meshdata": upArrowMeshData,
"color": (0.75, 0.75, 0.75, 0.3),
"edgeColor": (0.4, 0.4, 0.4, 1.0),
"drawEdge": True,
"drawFace": True,
"shader": 'balloon',
"glOptions": 'additive'
}
mirrorTrans = np.identity(4)
mirrorTrans[1][1] = -1
self.rightGrabber = gl.GLMeshItem(**grabberRMeshArguments)
self.leftGrabber = gl.GLMeshItem(**grabberLMeshArguments)
self.leftGrabber.applyTransform(Transform3D(mirrorTrans.flatten()),
local=True)
self.rightGrabber.applyTransform(Transform3D(mirrorTrans.flatten()),
local=True)
self.forwardArrow = gl.GLMeshItem(**forwardArrowArguments)
self.backwardArrow = gl.GLMeshItem(**backwardArrowArguments)
mirrorTrans[1][1] = 1
mirrorTrans[0][0] = -1
self.backwardArrow.applyTransform(Transform3D(mirrorTrans.flatten()),
local=True)
self.upArrow = gl.GLMeshItem(**upArrowArguments)
self.grabberBase = gl.GLMeshItem(**baseArguments)
self.rightGrabber.setParentItem(self)
self.leftGrabber.setParentItem(self)
self.forwardArrow.setParentItem(self)
self.backwardArrow.setParentItem(self)
self.upArrow.setParentItem(self)
self.grabberBase.setParentItem(self)
def setTransform(self, tr):
self.__transform = Transform3D(tr)
self.update()