Переопределением системы координат относительно маркеров в OpenCV с ArUco

Vec3d RotateX(Vec3d pos, double angle)
{
    Vec3d nextPos;
    nextPos.val[0] = pos.val[0];
    nextPos.val[1] = pos.val[1] * cos(angle) + pos.val[2]*sin(angle);
    nextPos.val[2] = -pos.val[1] * sin(angle) + pos.val[2]*cos(angle);
 
    return nextPos;
 
//    x'=x;
//    y':=y*cos(L)+z*sin(L);
//    z':=-y*sin(L)+z*cos(L);
}
 
Vec3d RotateY(Vec3d pos, double angle)
{
    Vec3d nextPos;
 
    nextPos.val[0] = pos.val[0] * cos(angle) + pos.val[2] * sin(angle);
    nextPos.val[1] = pos.val[1];
    nextPos.val[2] = -pos.val[0] * sin(angle) + pos.val[2] * cos(angle);
    return nextPos;
 
//    x'=x*cos(L)+z*sin(L);
//    y'=y;
//    z'=-x*sin(L)+z*cos(L);
}
 
Vec3d RotateZ(Vec3d pos, double angle)
{
    Vec3d  nextPos;
    nextPos.val[0]=pos.val[0]*cos(angle)-pos.val[1]*sin(angle);
    nextPos.val[1]=pos.val[0]*sin(angle)+pos.val[1]*cos(angle);
    nextPos.val[2]=pos.val[2];
    return nextPos;
 
//    x'=x*cos(L)-y*sin(L);
//    y'=x*sin(L)+y*cos(L);
//    z'=z;
}
 
Vec3d Rotate(Vec3d orivinPos, Vec3d  orivinRotate, Vec3d pos)
{
    Vec3d newPos;
 
    newPos = RotateZ(pos, orivinRotate.val[1]);
    newPos = RotateY(pos, orivinRotate.val[0]);
    newPos = RotateX(pos, orivinRotate.val[2]);
 
    newPos = newPos - orivinPos;
    return newPos;
}