基于SVD法求解激光跟踪仪坐标系转换关系

前言

坐标系间转换的通用模型是7参数模型，分别是1缩放，3平移，3旋转。关于这个模型的介绍及算法实现可以参考笔者原先的博客。
https://blog.csdn.net/iamqianrenzhan/article/details/78789335
该文章使用罗德里格法进行坐标系转换的公式推导与算法实现，是一个比通用的方法，只要有不共线的3个以上的点在两个坐标系中的坐标，就可以进行坐标系转换关系的计算。这在标定两个测量坐标系的关系时比较适合。
但是在实际的坐标系转换应用中，一般是要得到测量坐标系和装配坐标系的关系。这个应用中没有缩放参数，并且点在装配坐标系的位置测量需要使用其他方法。本文介绍一种不需要知道装配坐标系中点的具体坐标就可以进行测量坐标系和装配坐标系转换的方法。

方法简介

坐标系转换前测点工作

1. 测量装配坐标系原点在测量坐标系中的位置，记为O点。
2. 测量装配坐标系x轴正方向上任意一点在测量坐标系中的位置，记为X点。
3. 测量装配坐标系中xoy平面上y坐标为正任意一点在测量坐标系中位置，记为A点。

处理过程

1. 得到OX，OA向量。（OX即为装配坐标系X轴方向在测量坐标系下的向量）
2. OX，OA叉乘得到OZ。（OZ即为装配坐标系Z轴方向在测量坐标系下的向量）
3. OZ，OX叉乘得到OY。（OY即为装配坐标系Y轴方向在测量坐标系下的向量）
4. OX，OY，OZ进行归一化。
5. 测量坐标系O点坐标对应装配坐标系原点（0,0,0）。
6. 测量坐标系O点偏移OX，对应装配坐标系（1,0,0）。
7. 测测量坐标O点偏移OX+OY+OZ，对应装配坐标系（1,1,1）。
8. 使用三对点进行坐标系转换关系解算。

SVD法坐标关系计算过程

原理

关于SVD法求解坐标转换关系的原理，本文不打算细讲，可能会在后续博文中介绍。

代码

//输入3个点，顺序分别是装配坐标系中原点，x轴上一点，xoy平面上y轴正方向上一点在测量坐标系下是位置
void getRT(Vector3d *MeasualPosition, Matrix3d &R, Vector3d &T)
{
   
    //MeasualPosition的3个点按顺序分别是O,X,A。
    Vector3d OX = MeasualPosition[1] - MeasualPosition[0];  //装配坐标系中的x轴正方向在测量坐标系下的向量
    Vector3d OA = MeasualPosition[2] - MeasualPosition[0];

    Vector3d OZ;
    OZ = OX.cross(OA);   //装配坐标系中的z轴正方向在测量坐标系下的向量

    Vector3d OY;
    OY = OZ.cross(OX);   //装配坐标系中的y轴正方向在测量坐标系下的向量

    //对OX,OY,OZ进行归一化
    OX = OX/OX.norm();
    OY = OY/OY.norm();
    OZ = OZ/OZ.norm();

    Vector3d CalOriPosition[3];
    Vector3d CalNewPosition[3];

    CalOriPosition[0] = MeasualPosition[0];
    CalOriPosition[1] = MeasualPosition[0] + OX;
    CalOriPosition[2] = MeasualPosition[0] + OX + OY + OZ;
    CalNewPosition[0] = Vector3d(0,0,0);
    CalNewPosition[1] = Vector3d(