任意两空间直角坐标系的转换的数学模型和算法实现

数学模型：

两个空间直角坐标系进行转换，需要7个参数来描述，分别是3个旋转角，3个平移距离，1个比例因子。
如果知道3个点在两个坐标系中分别的位置，那么这7个参数可以唯一确定。
坐标转换的数学模型为：

⎡⎣⎢XYZ⎤⎦⎥T=λ⎡⎣⎢ΔXΔYΔZ⎤⎦⎥+λR⎡⎣⎢XYZ⎤⎦⎥S(1)

其中，T代表转换后的坐标，S代表转换前的坐标。 R是旋转矩阵，Δ是平移向量。
比例因子λ最容易确定，是两个刚体对应边长的比。平移向量Δ需要在旋转矩阵R确定后才能确定，所以旋转矩阵R的确定是参数解算的核心。R是3x3的矩阵，但实际上只用3个参数就可以表示。
这里利用反对称矩阵S来构造旋转矩阵 ，设反对称矩阵

S=⎡⎣⎢0cb−c0a−b−a0⎤⎦⎥

那么

R=(I+S)(I−S)−1(2)

其中I是单位矩阵。
那么只要求出a,b,c ，旋转矩阵R 就可以确定。
对于每个点，代入式（1）可以得到一组3个方程。用2点减去1点，消去平移参数Δ ，并与式（2）联立，得到：

λ(I+S)⎡⎣⎢XS2−XS1YS2−YS1ZS2−ZS1⎤⎦⎥=(I−S)⎡⎣⎢XT2−XT1YT2−YT1ZT2−ZT1⎤⎦⎥(3)

展开并整理（3）式，得到

⎡⎣⎢0−λZS21−ZT21λYS21+YT21−λZS21−ZT210λXS21+XT21−λYS21−YT21λXS21+XT210⎤⎦⎥⎡⎣⎢abc⎤⎦⎥=⎡⎣⎢XT21−λXS21YT21−λYS21ZT21−λZS21⎤⎦⎥(4)

其中

XS21=XS2−XS1

其他类似。

(4)式只有两个独立方程，不能解出 a,b,c三个参数。
用3点所列方程减去1点，得到和（4）类似的一组方程。这组方程和（4）联立，取3个独立方程，有

⎡⎣⎢0w2v3−w20u3−v2u20⎤⎦⎥⎡⎣⎢abc⎤⎦⎥=⎡⎣⎢XT21−λXS21YT21−λYS21ZT31−λZS31⎤⎦⎥(5)

其中

u2=λXS21+XT21

v2=λYS21+YT21

w2=λZS21+ZT21

u3=λXS31+XT31

v3=λYS31+YT31

w3=λZS31+ZT31

由（5）式解出a,b,c 为：

⎡⎣⎢abc⎤⎦⎥=1H⎡⎣⎢u2u3−u2v3u3w2u3v2−v2v3v3w2u2w2−v2w2w2w2⎤⎦⎥⎡⎣⎢XT21−λXS21YT21−λYS21ZT31−λZS31⎤⎦⎥(6)

其中

H=u2v3w2−u2v3w2

解出a,b,c后，由（2）式还原为旋转矩阵R。
再代入任意一点，由（1）式解出平移向量Δ。

算法实现

CoordinateTransformation.h

#pragma once
#include 
#include "eigen3\Eigen\Dense"
using namespace std;
using namespace Eigen;
//using Eigen::MatrixXd;

class CoordinateTransformation
{
public:
    CoordinateTransformation();
    ~CoordinateTransformation();

private:
    //3个对应点,用来解算7个参数
    Vector3d m_CalOriPosition[3];
    Vector3d m_CalNewPosition[3];
    Vector3d m_RigidBodyPoint[3];

    //7个参数，
    double m_ratio;
    Vector3d m_Translation; //3个参数
    Matrix3d m_Rotation;  //3个参数，由abc算出来，计算时解算的是abc
    Matrix3d m_AntiSymmetry;   //反对称矩阵，包含3个参数
    /*
            |0  -c  -b  |
    S   =   |c  0   -a  |    R=（I+S)*(I-S)^(-1)
            |b  a   0   |
    */

    //给定的输入和计算的输出
    Vector3d m_InputOriPosition;
    Vector3d m_OutputNewPosition;

public:
    //设置标定点原始点1 2 3
    void SetCalOriPoint1(Vector3d vector);
    void SetCalOriPoint2(Vector3d vector);
    void SetCalOriPoint3(Vector3d vector);

    //设置标定点当前点1 2 3
    void SetCalNewPoint1(Vector3d vector);
    void SetCalNewPoint2(Vector3d vector);
    void SetCalNewPoint3(Vector3d vector);

    //设置刚体上的3个点在刚体坐标系中的位置
    void SetRigidBodyPoint1(Vector3d vector);
    void SetRigidBodyPoint2(Vector3d vector);
    void SetRigidBodyPoint3(Vector3d vector);
    //标定
    void cali();

    //计算
    Vector3d calc(Vector3d input);

    //打印标定结果
    void printcaliresult();

    //计算刚体pose
    Matrix4d calcpose(Vector3d point1, Vector3d point2, Vector3d point3);

private:
    void Init();                      
    void caliratio();           //计算比例m_ratio
    void caliRotation();        //计算旋转矩阵m_Rotation
    void caliTranslation();     //计算平移向量m_Translation
};

CoordinateTransformation.cpp

#include "CoordinateTransformation.h"


CoordinateTransformation::CoordinateTransformation()
{
    //给标定的3对点赋初值
    Init();
}


CoordinateTransformation::~CoordinateTransformation()
{
}


void CoordinateTransformation::cali()
{
    //计算比例m_ratio
    caliratio();

    //计算旋转矩阵
    caliRotation();

    //计算平移向量
    caliTranslation();
}


//初始化
void CoordinateTransformation::Init()
{
    Vector3d temp1(1, 0, 0);
    SetCalOriPoint1(temp1);
    Vector3d temp11(1, 0, 0);
    SetCalNewPoint1(temp11);

    Vector3d temp2(0, 1, 0);
    SetCalOriPoint2(temp2);
    Vector3d temp22(0, 1, 0);
    SetCalNewPoint2(temp22);

    Vector3d temp3(0, 0, 1);
    SetCalOriPoint3(temp3);
    Vector3d temp33(0, 0, 1);
    SetCalNewPoint3(temp33);

    m_ratio = 1;
    m_Translation = Vector3d(0, 0, 0);
    m_Rotation <<
        1, 0, 0,
        0, 1, 0,
        0, 0, 1;
}

//给6个标定点赋初值
void CoordinateTransformation::SetCalOriPoint1(Vector3d vector)
{
    m_CalOriPosition[0] = vector;
}

void CoordinateTransformation::SetCalOriPoint2(Vector3d vector)
{
    m_CalOriPosition[1] = vector;
}

void CoordinateTransformation::SetCalOriPoint3(Vector3d vector)
{
    m_CalOriPosition[2] = vector;
}

void CoordinateTransformation::SetCalNewPoint1(Vector3d vector)
{
    m_CalNewPosition[0] = vector;
}

void CoordinateTransformation::SetCalNewPoint2(Vector3d vector)
{
    m_CalNewPosition[1] = vector;
}

void CoordinateTransformation::SetCalNewPoint3(Vector3d vector)
{
    m_CalNewPosition[2] = vector;
}

void CoordinateTransformation::SetRigidBodyPoint1(Vector3d vector)
{
    m_RigidBodyPoint[0] = vector;
}

void CoordinateTransformation::SetRigidBodyPoint2(Vector3d vector)
{
    m_RigidBodyPoint[1] = vector;
}

void CoordinateTransformation::SetRigidBodyPoint3(Vector3d vector)
{
    m_RigidBodyPoint[2] = vector;

    //赋值完成后把3个数都赋值给标定用的点
    for (int i = 0; i < 3; i++)
    {
        m_CalOriPosition[i] = m_RigidBodyPoint[i];
    }

}

//计算结果
Vector3d CoordinateTransformation::calc(Vector3d input)
{
    m_InputOriPosition = input;
    //计算

    m_OutputNewPosition = m_ratio*(m_Translation+ m_Rotation*m_InputOriPosition);

    return m_OutputNewPosition;
}

//计算刚体pose
Matrix4d CoordinateTransformation::calcpose(Vector3d point1, Vector3d point2, Vector3d point3)
{

    m_CalNewPosition[0] = point1;
    m_CalNewPosition[1] = point2;
    m_CalNewPosition[2] = point3;

    cali();

    Matrix4d result;

    for (int i = 0; i < 3; i++)
    {
        for (int j = 0; j < 3; j++)
        {
            result(i, j) = m_Rotation(i, j);
        }
    }
    for (int i = 0; i < 3; i++)
    {
        result(i, 3) = m_Translation(i, 0);
    }

    result(3, 0) = 0;
    result(3, 1) = 0;
    result(3, 2) = 0;
    result(3, 3) = 1;

    return result;
}

//标定比例
void CoordinateTransformation::caliratio()
{
    //计算比例m_ratio
    double ratiotemp[3];
    double temp1 = 0;
    double temp2 = 0;
    //0和1点
    temp1 = 0;
    temp2 = 0;
    for (int i = 0; i < 3; i++)
    {
        temp1 += pow((m_CalNewPosition[0](i, 0) - m_CalNewPosition[1](i, 0)), 2);
        temp2 += pow((m_CalOriPosition[0](i, 0) - m_CalOriPosition[1](i, 0)), 2);
    }
    ratiotemp[0] = sqrt(temp1 / temp2);

    //1和2点
    temp1 = 0;
    temp2 = 0;
    for (int i = 0; i < 3; i++)
    {
        temp1 += pow((m_CalNewPosition[2](i, 0) - m_CalNewPosition[1](i, 0)), 2);
        temp2 += pow((m_CalOriPosition[2](i, 0) - m_CalOriPosition[1](i, 0)), 2);
    }
    ratiotemp[1] = sqrt(temp1 / temp2);

    //0和2点
    temp1 = 0;
    temp2 = 0;
    for (int i = 0; i < 3; i++)
    {
        temp1 += pow((m_CalNewPosition[0](i, 0) - m_CalNewPosition[2](i, 0)), 2);
        temp2 += pow((m_CalOriPosition[0](i, 0) - m_CalOriPosition[2](i, 0)), 2);
    }
    ratiotemp[2] = sqrt(temp1 / temp2);

    //取平均值
    m_ratio = (ratiotemp[0] + ratiotemp[1] + ratiotemp[2]) / 3;
}

void CoordinateTransformation::caliRotation()
{
    //计算旋转矩阵
    double w2, u2, v2, w3, u3, v3;
    //2点减1点
    w2 = m_ratio*(m_CalOriPosition[1](2, 0) - m_CalOriPosition[0](2, 0))    //(2,0)代表z，[0]代表1点，[1]代表2点
        + (m_CalNewPosition[1](2, 0) - m_CalNewPosition[0](2, 0));

    u2 = m_ratio*(m_CalOriPosition[1](0, 0) - m_CalOriPosition[0](0, 0))    //(0,0)代表x，[0]代表1点，[1]代表2点
        + (m_CalNewPosition[1](0, 0) - m_CalNewPosition[0](0, 0));

    v2 = m_ratio*(m_CalOriPosition[1](1, 0) - m_CalOriPosition[0](1, 0))    //(1,0)代表y，[0]代表1点，[1]代表2点
        + (m_CalNewPosition[1](1, 0) - m_CalNewPosition[0](1, 0));
    //3点减1点
    w3 = m_ratio*(m_CalOriPosition[2](2, 0) - m_CalOriPosition[0](2, 0))    //(2,0)代表z，[0]代表1点，[2]代表3点
        + (m_CalNewPosition[2](2, 0) - m_CalNewPosition[0](2, 0));

    u3 = m_ratio*(m_CalOriPosition[2](0, 0) - m_CalOriPosition[0](0, 0))    //(0,0)代表x，[0]代表1点，[2]代表3点
        + (m_CalNewPosition[2](0, 0) - m_CalNewPosition[0](0, 0));

    v3 = m_ratio*(m_CalOriPosition[2](1, 0) - m_CalOriPosition[0](1, 0))    //(1,0)代表y，[0]代表1点，[2]代表3点
        + (m_CalNewPosition[2](1, 0) - m_CalNewPosition[0](1, 0));

    double H = -u3*v2*w2 + u2*v3*w2;      //如果选择矩阵是单位矩阵，那么计算出的H是0

    Matrix3d uvw;
    uvw(0, 0) = u2*u3;
    uvw(0, 1) = u3*v2;
    uvw(0, 2) = u2*w2;

    uvw(1, 0) = -u2*v3;
    uvw(1, 1) = -v2*v3;
    uvw(1, 2) = -v2*w2;

    uvw(2, 0) = u3*w2;
    uvw(2, 1) = v3*w2;
    uvw(2, 2) = w2*w2;

    Vector3d abc;
    Vector3d xyz;

    xyz(0, 0) = -m_ratio*(m_CalOriPosition[1](0, 0) - m_CalOriPosition[0](0, 0))    //(0,0)代表x，[0]代表1点，[1]代表2点
        + (m_CalNewPosition[1](0, 0) - m_CalNewPosition[0](0, 0));

    xyz(1, 0) = -m_ratio*(m_CalOriPosition[1](1, 0) - m_CalOriPosition[0](1, 0))    //(1,0)代表y，[0]代表1点，[1]代表2点
        + (m_CalNewPosition[1](1, 0) - m_CalNewPosition[0](1, 0));

    xyz(2, 0) = -m_ratio*(m_CalOriPosition[2](2, 0) - m_CalOriPosition[0](2, 0))    //(2,0)代表z，[0]代表1点，[2]代表3点
        + (m_CalNewPosition[2](2, 0) - m_CalNewPosition[0](2, 0));

    abc = (1 / H) * uvw * xyz;

    Matrix3d S;
    S(0, 0) = 0;
    S(0, 1) = -abc(2, 0);
    S(0, 2) = -abc(1, 0);

    S(1, 0) = abc(2, 0);
    S(1, 1) = 0;
    S(1, 2) = -abc(0, 0);

    S(2, 0) = abc(1, 0);
    S(2, 1) = abc(0, 0);
    S(2, 2) = 0;

    Matrix3d I;
    I.setIdentity();

    m_Rotation = (I + S)*((I - S).inverse());    //结果有问题，还需要再调试。再推导公式
}

void CoordinateTransformation::caliTranslation()
{
    //计算平移矩阵
    m_Translation = (m_CalNewPosition[2] - m_ratio*m_Rotation*m_CalOriPosition[2]) / m_ratio;
}

void CoordinateTransformation::printcaliresult()
{
    cout << "标定结果是" << endl;
    cout << "比例是" << m_ratio << endl;

    cout << "旋转矩阵是" << endl;
    cout << m_Rotation << endl;

    cout << "平移矩阵是" << endl;
    cout << m_Translation << endl;
}

使用例程

#include 
#include "eigen3\Eigen\Dense"
#include "CoordinateTransformation.h"
using namespace std;
using namespace Eigen;

int main()
{
    //CoordinateTransformation类的使用例程
    //1.定义类的对象
    CoordinateTransformation my;

    //2.设置标定需要的3对点，其中 SetCalOriPoint1是变换之前的点，SetCalNewPoint1是变换之后的点
    //=================模拟三对点=================//
    Vector3d InputOriPosition1(1, 0, 0);   //变换之前
    Vector3d InputOriPosition2(0, 1, 0);
    Vector3d InputOriPosition3(0, 0, 1);

    Vector3d Translation(0.5, 1.05, 2.200);
    double ratio = 1;
    Matrix3d Rotation;       //旋转矩阵不能随便构造，它满足一定的约束关系，因为本质上旋转由3个参数决定，旋转矩阵中有9个数
    //旋转矩阵也不能是不转动
    Rotation <<
        1, 0, 0,
        0, 0.866, 0.5,
        0, -0.5, 0.866;

    cout << "test:" << Rotation(2,1)<//-0.5
    Vector3d OutputNewPosition1 = ratio*(Translation + Rotation*InputOriPosition1);     //变换之后
    Vector3d OutputNewPosition2 = ratio*(Translation + Rotation*InputOriPosition2);
    Vector3d OutputNewPosition3 = ratio*(Translation + Rotation*InputOriPosition3);

    cout << "输入的坐标是" << endl << InputOriPosition1 << endl << InputOriPosition2 << endl << InputOriPosition3 << endl;
    cout << "输出的坐标是" << endl << OutputNewPosition1 << endl << OutputNewPosition2 << endl << OutputNewPosition3 << endl;

    //=================模拟三对点=================//

    my.SetCalOriPoint1(InputOriPosition1);
    my.SetCalOriPoint2(InputOriPosition2);
    my.SetCalOriPoint3(InputOriPosition3);

    my.SetCalNewPoint1(OutputNewPosition1);
    my.SetCalNewPoint2(OutputNewPosition2);
    my.SetCalNewPoint3(OutputNewPosition3);
    //实际标定时，新坐标上3个点的坐标是事先确定的，可以先赋值。然后把新坐标中这3个点在测量坐标系下的坐标在每次测量后由按钮触发赋值给SetCalOriPoint1

    //3.标定
    my.cali();

    //4.验证结果是否正确，是否和模拟3对点时用的7个参数相同
    my.printcaliresult();

    //5.测试一对点
    Vector3d input(1, 0, 0);
    cout << "输入的点是" << endl << input << endl;
    Vector3d output = my.calc(input);
    cout << "输出的点是" << endl <//由固定在刚体上的3个点在全局坐标系下的位置来确定刚体的姿态
    //设点在刚体坐标系下的坐标为X(O)=(x,y,z,1),在全局坐标系下的位置是X(G)=(x1,y1,z1,1);
    //则转换矩阵M使M*X(O)=X(G);
    //本质上还是3对坐标点解算两个坐标系相对位置的关系
    //把这个类改造一下，使用标定的结果作为结果。

    //怎么解算飞行器的位姿
    CoordinateTransformation CT;

    //1.设置刚体上固定的3个点的坐标
    Vector3d RigidBodyPosition1(1, 0, 0);   //固定在刚体上的点在刚体坐标系中的位置
    CT.SetRigidBodyPoint1(RigidBodyPosition1);

    Vector3d RigidBodyPosition2(0, 1, 0);   //固定在刚体上的点在刚体坐标系中的位置
    CT.SetRigidBodyPoint2(RigidBodyPosition2);

    Vector3d RigidBodyPosition3(0, 0, 1);   //固定在刚体上的点在刚体坐标系中的位置
    CT.SetRigidBodyPoint3(RigidBodyPosition3);

    //2.模拟计算出的坐标系中的点
    Vector3d RigidBodyPosition11 = ratio*(Translation + Rotation*RigidBodyPosition1);     //变换之后
    Vector3d RigidBodyPosition22 = ratio*(Translation + Rotation*RigidBodyPosition2);
    Vector3d RigidBodyPosition33 = ratio*(Translation + Rotation*RigidBodyPosition3);

    //3.使用模拟坐标系中的点可以解算出飞行器的位姿
    Matrix4d temp = CT.calcpose(RigidBodyPosition11, RigidBodyPosition22, RigidBodyPosition33);    //设置3个点按顺序的坐标

    cout << temp << endl;
    while (1);
    return 0;
}

依赖的库

在里面用到了开源的矩阵计算工具库eigen。可以在http://eigen.tuxfamily.org/index.php?title=Main_Page下载。这个工具库全部由头文件组成，所以只要复制到工程目录下就可以，不需要复杂的配置库目录和可执行文件目录。