直接线性变换得到的旋转矩阵R和平移向量t如何投影到SE(3)流形上？

1前期分析

  记直接线性变换（Direct Linear Transformation）得到的旋转矩阵为$R$，平移向量为$t$，投影方式为，
$$R\leftarrow (R{R}^T{)}^{-\frac{1}{2}}R$$
然后代入线性方程组，利用最小二乘法更新平移向量$t$。

  随机生成4个路标点$[X,Y,Z{]}^T$，$X$从-100至100米取值，$Y$从-30至30米取值，$Z$从0至100米取值，同时在X方向上加上均值为0标准差为0.5的高斯噪声，在Y方向上加上均值为0标准差为0.5的高斯噪声，在Z方向上加上均值为0标准差为0.4的高斯噪声。
  旋转的真值设置为：先绕Z轴旋转10度，再绕Y轴旋转15度，最后绕X轴旋转20度。平移的真值设置为：$[0.2,0.5,0.7{]}^T$。
  对比DLT求解出的R和t的误差和先DLT再投影到SE(3)流形上求解出来的R和t的误差。

2代码

Ubuntu下C++代码：

#include 
#include 
#include 
#include 
#include 
#include 
#include 
#include 
#include 
#include 
#include 
#include 

using namespace std;
using namespace cv;
using namespace Eigen;

clock_t time_s, time_e;

void FourPoints(Matrix<double,6,1> a, Matrix<double,6,1> b, Matrix<double,6,1> c, Matrix<double,6,1> d, Matrix<double,3,3>& R1, Vector3d& t1, Matrix<double,3,3>& R2, Vector3d& t2);


int main()
{
     
    time_s = clock();


    //构建变换矩阵真值T
    //平移向量为[0.2,0.5,0.7]^T；旋转矩阵为向绕Z轴转动10度，再绕Y轴转动15度，最后绕X轴转动20度！
    Vector3d t = Vector3d(0.2,0.5,0.7);
    AngleAxisd rt1(M_PI / 18, Vector3d(0,0,1));//绕Z轴转动10度，rotationVector简称rt
    AngleAxisd rt2(M_PI / 12, Vector3d(0,1,0));//绕Y轴转动15度
    AngleAxisd rt3(M_PI / 9, Vector3d(1,0,0));//绕X轴转动20度
    Matrix3d R = rt3.matrix() * rt2.matrix() * rt1.matrix();

    Vector3d ea = R.eulerAngles(0,1,2);//012表示R=Rx*Ry*Rz
    //ea中的第1个元素表示绕x轴转过的角度，ea中的第2个元素表示绕y轴转动的角度，ea中的第3个元素表示绕z轴转过的角度
    ea = 180.0 / M_PI * ea;//ea等于[20,15,10]^T
    //cout << "ea = \n" << ea << endl;

    int iterations = 50000;

    //误差向量：前3维表示利用DLT变换求解出的旋转和平移的误差，后3维表示将其投影到SE(3)流形上得到的旋转和平移的误差
    double perr[(const int)iterations][6];//perr全称为Position Error，表示定位误差
    double aerr[(const int)iterations][6];//aerr全称为Angle Error，表示角度误差

    for(int i = 0; i < iterations; i++)
    {
     
        //随机生成4对点，X的范围从-100到100米取值，Y从-30到30米取值，Z从0到100米取值；
        //X方向上的匹配误差服从均值为0标准差为0.5的高斯分布；Y方向上的匹配误差服从均值为0标准差为0.5的高斯分布；
        //Z方向上的匹配误差服从均值为0标准差为0.4的高斯分布。
        Matrix<double, 6, 1> a = Matrix<double,6,1>::Zero();
        Matrix<double, 6, 1> b = Matrix<double,6,1>::Zero();
        Matrix<double, 6, 1> c = Matrix<double,6,1>::Zero();
        Matrix<double, 6, 1> d = Matrix<double,6,1>::Zero();

        a[0] = (double) rand() / RAND_MAX * 200.0 - 100.0;
        a[1] = (double) rand() / RAND_MAX * 60.0 - 30.0;
        a[2] = (double) rand() / RAND_MAX * 100.0;

        srand(rand());
        b[0] = (double) rand() / RAND_MAX * 200.0 - 100.0;
        b[1] = (double) rand() / RAND_MAX * 60.0 - 30.0;
        b[2] = (double) rand() / RAND_MAX * 100.0;

        srand(rand());
        c[0] = (double) rand() / RAND_MAX * 200.0 - 100.0;
        c[1] = (double) rand() / RAND_MAX * 60.0 - 30.0;
        c[2] = (double) rand() / RAND_MAX * 100.0;

        srand(rand());
        d[0] = (double) rand() / RAND_MAX * 200.0 - 100.0;
        d[1] = (double) rand() / RAND_MAX * 60.0 - 30.0;;
        d[2] = (double) rand() / RAND_MAX * 100.0;

        //仿真第2帧中的匹配点
        RNG rng;
        Vector3d v1 = R * Vector3d(a[0],a[1],a[2]) + t
                      + Vector3d(rng.gaussian(0.5),rng.gaussian(0.5),rng.gaussian(0.4));
        Vector3d v2 = R * Vector3d(b[0],b[1],b[2]) + t
                      + Vector3d(rng.gaussian(0.5),rng.gaussian(0.5),rng.gaussian(0.4));
        Vector3d v3 = R * Vector3d(c[0],c[1],c[2]) + t
                      + Vector3d(rng.gaussian(0.5),rng.gaussian(0.5),rng.gaussian(0.4));
        Vector3d v4 = R * Vector3d(d[0],d[1],d[2]) + t
                      + Vector3d(rng.gaussian(0.5),rng.gaussian(0.5),rng.gaussian(0.4));

        a[3] = v1[0]; a[4] = v1[1]; a[5] = v1[2];
        b[3] = v2[0]; b[4] = v2[1]; b[5] = v2[2];
        c[3] = v3[0]; c[4] = v3[1]; c[5] = v3[2];
        d[3] = v4[0]; d[4] = v4[1]; d[5] = v4[2];

        Matrix3d R_esti1, R_esti2;
        Vector3d t_esti1, t_esti2;
        FourPoints(a,b,c,d,R_esti1,t_esti1,R_esti2,t_esti2);

        //计算平移误差
        Vector3d err = t_esti1 - t;//计算方法1的平移误差
        perr[i][0] = err[0]; perr[i][1] = err[1]; perr[i][2] = err[2];
        err = t_esti2 - t;//计算方法2的平移误差
        perr[i][3] = err[0]; perr[i][4] = err[1]; perr[i][5] = err[2];

        //计算旋转误差
        Vector3d ea_esti = R_esti1.eulerAngles(0,1,2);//012表示R=Rx*Ry*Rz
        err = ea_esti * 180.0 / M_PI - ea;//计算方法1的旋转误差
        aerr[i][0] = err[0]; aerr[i][1] = err[1]; aerr[i][2] = err[2];
        ea_esti = R_esti2.eulerAngles(0,1,2);//012表示R=Rx*Ry*Rz
        err = ea_esti * 180.0 / M_PI - ea;//计算方法2的旋转误差
        aerr[i][3] = err[0]; aerr[i][4] = err[1]; aerr[i][5] = err[2];

    }


    ofstream of1;
    of1.open("./perr.txt");
    for(int i = 0; i < iterations; i++)
        of1 << perr[i][0] << ' ' << perr[i][1] << ' ' << perr[i][2] << ' ' << perr[i][3] << ' ' << perr[i][4] << ' ' << perr[i][5] << endl;
    of1.close();

    ofstream of2;
    of2.open("./aerr.txt");
    for(int i = 0; i < iterations; i++)
        of2 << aerr[i][0] << ' ' << aerr[i][1] << ' ' << aerr[i][2] << ' ' << aerr[i][3] << ' ' << aerr[i][4] << ' ' << aerr[i][5] << endl;
    of2.close();



    time_e = clock();

    cout << "Total time: " << (double)(time_e - time_s)/CLOCKS_PER_SEC << "s" << endl;

    return 0;
}




void FourPoints(Matrix<double,6,1> a, Matrix<double,6,1> b, Matrix<double,6,1> c, Matrix<double,6,1> d, Matrix<double,3,3>& R1, Vector3d& t_1, Matrix<double,3,3>& R2, Vector3d& t_2)
{
     
    //根据4点对解算R和t
    Matrix<double, 12, 12> matrix_A;
    matrix_A <<
             a(0), a(1), a(2), 0, 0, 0, 0, 0, 0, 1, 0, 0,
            0, 0, 0, a(0), a(1), a(2), 0, 0, 0, 0, 1, 0,
            0, 0, 0, 0, 0, 0, a(0), a(1), a(2), 0, 0, 1,
            b(0), b(1), b(2), 0, 0, 0, 0, 0, 0, 1, 0, 0,
            0, 0, 0, b(0), b(1), b(2), 0, 0, 0, 0, 1, 0,
            0, 0, 0, 0, 0, 0, b(0), b(1), b(2), 0, 0, 1,
            c(0), c(1), c(2), 0, 0, 0, 0, 0, 0, 1, 0, 0,
            0, 0, 0, c(0), c(1), c(2), 0, 0, 0, 0, 1, 0,
            0, 0, 0, 0, 0, 0, c(0), c(1), c(2), 0, 0, 1,
            d(0), d(1), d(2), 0, 0, 0, 0, 0, 0, 1, 0, 0,
            0, 0, 0, d(0), d(1), d(2), 0, 0, 0, 0, 1, 0,
            0, 0, 0, 0, 0, 0, d(0), d(1), d(2), 0, 0, 1;
    Matrix<double, 12, 1> matrix_b;
    matrix_b <<
             a(3), a(4), a(5),
            b(3), b(4), b(5),
            c(3), c(4), c(5),
            d(3), d(4), d(5);
    Matrix<double, 12, 1> x = matrix_A.colPivHouseholderQr().solve(matrix_b);
    R1 <<
      x(0), x(1), x(2),
            x(3), x(4), x(5),
            x(6), x(7), x(8);
    t_1 << x(9), x(10), x(11);

    //计算R的欧拉角
    //Vector3d ea = R1.eulerAngles(0,1,2);//012表示R=Rx*Ry*Rz
    //ea = 180.0 / M_PI * ea;
    //cout << "ea = \n" << ea << endl;

    Matrix3d A = R1 * R1.transpose();
    Matrix3d L = A.llt().matrixL();
    R2 = L.inverse() * R1;//更新R

    Vector3d t1 = Vector3d(a(3),a(4),a(5)) - R2 * Vector3d(a(0),a(1),a(2));
    Vector3d t2 = Vector3d(b(3),b(4),b(5)) - R2 * Vector3d(b(0),b(1),b(2));
    Vector3d t3 = Vector3d(c(3),c(4),c(5)) - R2 * Vector3d(c(0),c(1),c(2));
    Vector3d t4 = Vector3d(d(3),d(4),d(5)) - R2 * Vector3d(d(0),d(1),d(2));
    t_2 = (t1 + t2 + t3 + t4) / 4;//更新t

    //计算R的欧拉角
    //Vector3d ea = R.eulerAngles(0,1,2);//012表示R=Rx*Ry*Rz
    //ea = 180.0 / M_PI * ea;
    //cout << "ea = \n" << ea << endl;

}

3对比分析

使用matlab对aerr.txt和perr.txt进行分析，可得，

3.1角度误差

方法	滚转角误差均值和标准差/单位度	俯仰角误差均值和标准差/单位度	偏航角误差均值和标准差/单位度
DLT	1.2235，12.3224	0.1292，14.3453	-0.6777，14.7657
DLT+SE(3)	1.4319，12.9115	0.0538，13.6840	-0.4533，15.0027

3.2平移误差

方法	x方向上误差均值和标准差/单位米	y方向上均值和标准差/单位米	z方向上均值和标准差/单位米
DLT	0.1294，65.7250	-0.8120，192.8713	0.0628，73.0519
DLT+SE(3)	0.1220，4.7943	0.2778，5.3497	0.2545，5.1164

4结论

虽然在角度误差上方法1和方法2相近；但在平移误差上，方法2在均值和标准差上优于方法1，尤其是在标准差上！