Explorer/RaspberryPi/Visualprocessing/IMU

/*****************************************************************************/
static void * drawAxes_threadImuProg(void *arg) {
  float msgpipe_l[3];
  int msgsize = sizeof(msgpipe_g);
  while(true) {
    if (msgsize==read(imufd,&msgpipe_l,msgsize)) {
      memcpy(&msgpipe_g, &msgpipe_l, msgsize); 
    } 
  }
}

static void drawAxes_init()
{
  mkfifo(IMUPIPE, 0666); 
  imufd = open(IMUPIPE, O_RDWR);

  pthread_t threadImu;
  pthread_create(&threadImu, NULL, &drawAxes_threadImuProg, NULL);
}

struct quaternion_t
{
  float w, x, y, z;
};

static void drawAxes(Mat3b *img) 
{
  float msgpipe_l[3];
  memcpy(&msgpipe_l, &msgpipe_g, sizeof(msgpipe_g)); 

  float roll  =  torad*msgpipe_l[0];   // roll_phy(x)
  float pitch = -torad*msgpipe_l[1]; // pitch_theta(y)
  float yaw   = -torad*msgpipe_l[2];   // yaw_psi(z)

  float cy = cos(roll * 0.5);
  float sy = sin(roll * 0.5);
  float cp = cos(yaw * 0.5);
  float sp = sin(yaw * 0.5);
  float cr = cos(pitch * 0.5);
  float sr = sin(pitch * 0.5);

  quaternion_t q;          // quaternion from euler
  q.w = cr * cp * cy + sr * sp * sy;
  q.x = sr * cp * cy - cr * sp * sy;
  q.y = cr * sp * cy + sr * cp * sy;
  q.z = cr * cp * sy - sr * sp * cy;

  array<Point3i,4> pts = {Point3i{0,0,0},Point3i{100,0,0},Point3i{0,100,0},Point3i{0,0,100}};
  array<Point3f,4> out;
  for (int i=0;i<4;i++) {  // rotation matrix from quaternion
    out[i].x=(1-2*q.y*q.y-2*q.z*q.z)*pts[i].x+(2*q.x*q.y-2*q.z*q.w)*pts[i].y+(2*q.x*q.z+2*q.y*q.w)*pts[i].z;
    out[i].y=(2*q.x*q.y+2*q.z*q.w)*pts[i].x+(1-2*q.x*q.x-2*q.z*q.z)*pts[i].y+(2*q.y*q.z-2*q.x*q.w)*pts[i].z;
    out[i].z=(2*q.x*q.z-2*q.y*q.w)*pts[i].x+(2*q.y*q.z+2*q.x*q.w)*pts[i].y+(1-2*q.x*q.x-2*q.y*q.y)*pts[i].z;
  } 

  Point loc=pcenter;
  array<Point,4> imgPts;   // 3D to 2D projection
  for (int i=0;i<4;i++)
    imgPts[i]={(int)(loc.x-out[i].x+0.5*out[i].z),(int)(loc.y-out[i].y-0.5*out[i].z)}; 

  line(*img, imgPts[0], imgPts[3], Scalar(0,0,255), 3); // red
  line(*img, imgPts[0], imgPts[1], Scalar(0,255,0), 3); // green
  line(*img, imgPts[0], imgPts[2], Scalar(255,0,0), 3); // blue
  circle(*img, pcenter, 5, Scalar(0,0,0), -1);
}