相机移动情况下的装配关系确定

装配关系假定为抓手A到被抓取物B的关系，也就是A坐标系与B坐标系重合。

相机cam固定在A上，随着A的移动而移动。此时需要一个固定的基准Control。B的位置在装配过程中不动。这样就得到两组固定关系，cam与A，control与B。

最终装配上时，A与B的坐标系重合。此时就会得到cam与control的一组关系，此时相机坐标系为camBase。此关系位装配的基准关系。得到这一组关系，就明确了最终装配的目标。

一般情况下，获取的装配调整量，最终要传递给调姿机构。所以计算出的装配调整量必须是在调姿机构坐标系下的。整个装配过程只有control不动，可以将调姿机构坐标系设为control坐标系。

这样，需要计算的调整量就转换为，control坐标系下，当前cam坐标系到装配基准camBase坐标系的变换参数。注意此变换参数的计算必须是满足固定坐标系control下，两坐标系关系的计算。

 

计算control坐标系下，当前相机cam坐标系到装配基准camBase坐标系的转换关系，需要建立4*4矩阵Rcur和Rcali。

最终需要计算的转换矩阵计算公式为  R = Rcur-1 * Rcali

解算结果中，旋转矩阵计算旋转角时，需要用到固定轴计算公式：

//旋转矩阵 计算 固定轴XYZ 旋转角
//计算角度的时候，输出的是°，不需要在外部进行弧度转°计算了
void RotMatrixToFixXYZ(CStation& rot,double& dRx,double &dRy,double &dRz)
{
    double dPi = 4*atan(1.0);

    //欧拉角XYZ 到 欧拉角 ZYX(此旋转系统就是固定轴XYZ)
    dRz = atan2(rot.b1,rot.a1) * 180 / dPi;
    dRy = atan2f(-rot.c1,sqrtf(powf(rot.c2,2)+powf(rot.c3,2)))*180/dPi;
    dRx = atan2(rot.c2,rot.c3) * 180 /dPi;
}