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

1前期分析

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

  随机生成4个路标点 [ X , Y , Z ] T [X,Y,Z]^T [X,Y,Z]T X X X从-100至100米取值, Y Y Y从-30至30米取值, Z Z 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 [0.2,0.5,0.7]^T [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角度误差

直接线性变换得到的旋转矩阵R和平移向量t如何投影到SE(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

直接线性变换得到的旋转矩阵R和平移向量t如何投影到SE(3)流形上?_第2张图片

3.2平移误差

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

方法 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

直接线性变换得到的旋转矩阵R和平移向量t如何投影到SE(3)流形上?_第4张图片

4结论

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

你可能感兴趣的:(SLAM杂项,笔记)