[FIXED] Pyqtgraph GLMeshItem set rotation with quaternion

Issue

I'm working on a real-time visualization for IMU data. The idea is to display a 3D object and update its attitude from the IMU data. From the IMU, I get the quaternion representing the device's attitude.

The sphere is initialized in the init as a mesh object. In the redraw method, I want to rotate the sphere, setting the attitude equal to what I get from the IMU quaternion.

I could not find a way to rotate the mesh with the quaternion, so I'm trying to use the Euler angles as a workaround. However, it seems the conversion to Euler (a,v) is not working properly. As a result, the mesh is not rotating properly.

What is wrong with my implementation? Do you have any suggestion on how I could implement it properly?

Full code

import sys
import multiprocessing as mp

import numpy as np

from PyQt5.QtWidgets import QApplication, QMainWindow
from PyQt5.QtGui import QQuaternion, QVector3D, QVector4D
from PyQt5.QtCore import QTimer
import pyqtgraph.opengl as gl

class QuatAnimation(QMainWindow):
    def __init__(self, data_queue:mp.Queue) -> None:
        super().__init__()
        self.setWindowTitle("Rotating Vector Animation")
        self.resize(800, 600)

        self.data_queue = data_queue
        self.q = QQuaternion(QVector4D(0, 0, 0, 1))

        self.view = gl.GLViewWidget(self)
        self.setCentralWidget(self.view)
        self.view.pan(0, 0, 0)
        self.view.setCameraPosition(distance=5, elevation=15)

        g = gl.GLGridItem()
        g.scale(2,2,1)
        self.view.addItem(g)
        self.view.addItem(gl.GLAxisItem())


        self.mesh = gl.MeshData.sphere(rows=10, cols=20)
        colors = np.ones((self.mesh.faceCount(), 4), dtype=float)
        colors[::2,0] = 0
        colors[:,1] = np.linspace(0, 1, colors.shape[0])
        self.mesh.setFaceColors(colors)
        self.obj = gl.GLMeshItem(meshdata=self.mesh, smooth=False)

        self.view.addItem(self.obj)

        # setup redraw timer
        self.timer = QTimer()
        self.timer.timeout.connect(self.redraw)
        self.timer.start(int(1000/65))  # ~65 fps
        
        self.show()

    def redraw(self):
        if not self.data_queue.empty():
            item = self.data_queue.get_nowait() #item = [time, x, y, z, w]

            self.q = QQuaternion(QVector4D(item[1], item[2], item[3], item[4]))
            v = self.q.vector().normalized()
            a = self.q.scalar()
            self.obj.resetTransform() # reset every loop, since we h
            self.obj.rotate(a*2*180/np.pi, v.x(), v.y(), v.z())


    def closeEvent(self, event):
        self.timer.stop()
        event.accept()


def plot(data_queue:mp.Queue):
    app = QApplication(sys.argv)
    window = QuatAnimation(data_queue)  # input queue with quaternion data
    window.show()
    app.exec_()
    sys.exit()

if __name__ == "__main__":
    plot()

Solution

You converted a quaternion into a transformation matrix incorrectly. See Using quaternions as rotations.

Do like this using the QQuaternion.getAxisAndAngle().

...
q = QQuaternion(1, 2, 3, 4).normalized()
axis, angle = q.getAxisAndAngle()

mesh = gl.GLMeshItem(...)
mesh.resetTransform()
mesh.rotate(angle, axis.x(), axis.y(), axis.z())
...

Answered By - relent95