create_pose算子说明

算子作用：创建一个3D姿态

算子声明：

create_pose( : : TransX, TransY, TransZ, RotX, RotY, RotZ, OrderOfTransform, OrderOfRotation, ViewOfTransform : Pose)

举例：

create_pose (EndX - StartX, EndY - StartY, EndZ - StartZ, 0, 0, 0, 'Rp+T', 'gba', 'point', MovementPoseNSteps)

算子说明：

参数`OrderOfRotation`

选择'gba'的意义为：

注意：这里可以有两种读法，如果从右往左，这里的旋转是相对于原始（固定）坐标系旋转的，也即：先相对于原始的Z轴，再相对于原始的Y轴，最后相对于原始的X轴旋转。如果是从左往右读，这里的旋转就是相对于实时坐标系，也就是旋转完一个另外的坐标系也随着变化，也即：先绕着X轴，再绕着新的Y轴，最后绕着最新的Z轴旋转。

从右往左读的话，相当于下面的变换：

  hom_mat3d_identity (HomMat3DIdent)hom_mat3d_rotate (HomMat3DIdent, RotZ, 'z', 0, 0, 0, HomMat3DRotZ)hom_mat3d_rotate (HomMat3DRotZ, RotY, 'y', 0, 0, 0, HomMat3DRotYZ)hom_mat3d_rotate (HomMat3DRotYZ, RotX, 'x', 0, 0, 0, HomMat3DXYZ)

从左往右读的话相当于下面的变换：

hom_mat3d_identity (HomMat3DIdent)hom_mat3d_rotate_local (HomMat3DIdent, RotX, 'x', HomMat3DRotX)hom_mat3d_rotate_local (HomMat3DRotX, RotY, 'y', HomMat3DRotXY)hom_mat3d_rotate_local (HomMat3DRotXY, RotZ, 'z', HomMat3DXYZ)

上面这种是参考Yaw-Pitch-Roll转换规则。

下面还有一种选择Roll-Pitch-Yaw转换规则：

还可以选择'rodriguez'算法如下

注意：这种3D姿态有时候是模糊的，意味着一种姿态可能对应多种组合。下面举一个例子，特别是再b=90度的时候。

相应的同质转换矩阵：
Ht是平移矩阵，Hr是旋转矩阵

坐标的转换
下面演示了一个点怎么从一个坐标系转换到另一个坐标系：

这相当于下面的功能：

  pose_to_hom_mat3d(PoseOf1In2, HomMat3DFrom1In2)affine_trans_point_3d(HomMat3DFrom1In2, P1X, P1Y, P1Z, P2X, P2Y, P2Z)

参数 OrderOfTransform

先说标准姿势定义，想要生成标准姿态，OrderOfTransform需要选择'Rp+T'，ViewOfTransform需要选择'point'。

如果OrderOfTransform选择了'R(p-T)'
算法则会如下：

参数`ViewOfTransform`

如果选择'coordinate_system'，算法如下：

返回值`Pose`类型

返回值为7个元素的数组，前六个是三方向平移和三方向旋转，最后一个是下面的Code，也即标志矩阵类型