Question

背景

以下是我的设备的Roll / Pitch / Yaw值的图表：

注意：值并不准确，但仅供说明用途

PI_2是pi或大约1.57
PI_4是pi或大约0.78

方案

我使用Euler Angles在VTK 3D空间中定位相机。我的模型的3D空间与我的设备XYZ不同：

模型中的Z是真实世界Y，反之亦然，X在两者中都相同。

我研究了旋转矩阵并在维基百科上发现了一篇文章，甚至在SOF上找到了一些c ++代码，我没有运气就转换为ObjC。它关闭但轴始终关闭。我已经看过RPY和XYZ，因为我已经移动了设备，但我没有看到任何关系。

问题

如何从我的XYZ设备中获取Euler角度以在VTK XZY空间中工作？

代码

const float DISTANCE = 10;
typedef float Matrix[3][3];
struct EulerAngle { float X,Y,Z; };
enum EEulerOrder
{
    ORDER_XYZ,
    ORDER_YZX,
    ORDER_ZXY,
    ORDER_ZYX,
    ORDER_YXZ,
    ORDER_XZY
};

...

vesVector3f cpv = self->mKiwiApp->cameraPosition();
vesVector3f fpv = self->mKiwiApp->cameraFocalPoint();
vesVector3f cuv = self->mKiwiApp->cameraViewUp();

double pitch = self.motionData.attitude.pitch;
double roll = self.motionData.attitude.roll;
double yaw = self.motionData.attitude.yaw;

NSArray *eaArr = [NSArray arrayWithObjects:[NSNumber numberWithFloat:r],
                    [NSNumber numberWithFloat:p],
                    [NSNumber numberWithFloat:y],
                    nil];
//I've tried mixtures of pry, ypy, rpr, etc into `eulerAnglesInOrder`
//I've also tried all various orders into `eulerAnglesInOrder`
NSArray *xyz = [self eulerAnglesInOrder:eaArr withOrder:ORDER_XYZ];

cpv(0, 0) = 0;
cpv(1, 0) = 0;
cpv(2, 0) = 0;
self->mKiwiApp->setCameraPosition(cpv);

fpv(0, 0) = [[xyz objectAtIndex:0] doubleValue] * DISTANCE;
fpv(1, 0) = [[xyz objectAtIndex:1] doubleValue] * DISTANCE;
fpv(2, 0) = [[xyz objectAtIndex:2] doubleValue] * DISTANCE;
self->mKiwiApp->setCameraFocalPoint(fpv);

cuv(0, 0) = [[xyz objectAtIndex:3] doubleValue];
cuv(1, 0) = [[xyz objectAtIndex:4] doubleValue];
cuv(2, 0) = [[xyz objectAtIndex:5] doubleValue];
self->mKiwiApp->setCameraViewUp(cuv);

...



- (NSArray *)eulerAnglesInOrder:(const NSArray *)inEulerAngle withOrder:(const int)eulerOrder
{
    // Convert Euler Angles passed in a vector of Radians
    // into a rotation matrix.  The individual Euler Angles are
    // processed in the order requested.
    float x = [[inEulerAngle objectAtIndex:0] floatValue];
    float y = [[inEulerAngle objectAtIndex:1] floatValue];
    float z = [[inEulerAngle objectAtIndex:2] floatValue];

    float x0, x1, x2;
    float y0, y1, y2;
    float z0, z1, z2;

    const float    Sx    = sin(x);
    const float    Sy    = sin(y);
    const float    Sz    = sin(z);
    const float    Cx    = cos(x);
    const float    Cy    = cos(y);
    const float    Cz    = cos(z);

    switch(eulerOrder)
    {
        case ORDER_XYZ:
            x0=Cy*Cz;
            x1=-Cy*Sz;
            x2=Sy;
            y0=Cz*Sx*Sy+Cx*Sz;
            y1=Cx*Cz-Sx*Sy*Sz;
            y2=-Cy*Sx;
            z0=-Cx*Cz*Sy+Sx*Sz;
            z1=Cz*Sx+Cx*Sy*Sz;
            z2=Cx*Cy;
            break;

        case ORDER_YZX:
            x0=Cy*Cz;
            x1=Sx*Sy-Cx*Cy*Sz;
            x2=Cx*Sy+Cy*Sx*Sz;
            y0=Sz;
            y1=Cx*Cz;
            y2=-Cz*Sx;
            z0=-Cz*Sy;
            z1=Cy*Sx+Cx*Sy*Sz;
            z2=Cx*Cy-Sx*Sy*Sz;
            break;

        case ORDER_ZXY:
            x0=Cy*Cz-Sx*Sy*Sz;
            x1=-Cx*Sz;
            x2=Cz*Sy+Cy*Sx*Sz;
            y0=Cz*Sx*Sy+Cy*Sz;
            y1=Cx*Cz;
            y2=-Cy*Cz*Sx+Sy*Sz;
            z0=-Cx*Sy;
            z1=Sx;
            z2=Cx*Cy;
            break;

        case ORDER_ZYX:
            x0=Cy*Cz;
            x1=Cz*Sx*Sy-Cx*Sz;
            x2=Cx*Cz*Sy+Sx*Sz;
            y0=Cy*Sz;
            y1=Cx*Cz+Sx*Sy*Sz;
            y2=-Cz*Sx+Cx*Sy*Sz;
            z0=-Sy;
            z1=Cy*Sx;
            z2=Cx*Cy;
            break;

        case ORDER_YXZ:
            x0=Cy*Cz+Sx*Sy*Sz;
            x1=Cz*Sx*Sy-Cy*Sz;
            x2=Cx*Sy;
            y0=Cx*Sz;
            y1=Cx*Cz;
            y2=-Sx;
            z0=-Cz*Sy+Cy*Sx*Sz;
            z1=Cy*Cz*Sx+Sy*Sz;
            z2=Cx*Cy;
            break;

        case ORDER_XZY:
            x0=Cy*Cz;
            x1=-Sz;
            x2=Cz*Sy;
            y0=Sx*Sy+Cx*Cy*Sz;
            y1=Cx*Cz;
            y2=-Cy*Sx+Cx*Sy*Sz;
            z0=-Cx*Sy+Cy*Sx*Sz;
            z1=Cz*Sx;
            z2=Cx*Cy+Sx*Sy*Sz;
            break;
    }

    NSArray *arr = [NSArray arrayWithObjects:
                      [NSNumber numberWithFloat:x0+x1+x2]
                    , [NSNumber numberWithFloat:y0+y1+y2]
                    , [NSNumber numberWithFloat:z0+z1+z2]
                    , [NSNumber numberWithFloat:x1] // for camera up
                    , [NSNumber numberWithFloat:y1] // for camera up
                    , [NSNumber numberWithFloat:z1] // for camera up
                    , nil];

    return(arr);
}

设备姿态（欧拉角）到3D坐标

0 个答案: