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

YMWM_

已于 2022-05-03 18:32:30 修改

阅读量1.2k

点赞数

分类专栏： SLAM杂项文章标签：笔记

于 2021-11-11 20:38:26 首次发布

本文链接：https://blog.csdn.net/YMWM_/article/details/121275674

版权

SLAM杂项专栏收录该内容

15 篇文章 3 订阅

订阅专栏

本文通过对比直接线性变换（DLT）与DLT后投影到SE(3)流形上的旋转和平移误差，分析了两种方法在角度误差和平移误差上的表现。实验结果显示，尽管两者在角度误差上接近，但在平移误差的均值和标准差上，DLT+SE(3)方法显著优于DLT，尤其在标准差上。这表明DLT+SE(3)方法在处理平移误差时更具优势。

摘要由CSDN通过智能技术生成

1前期分析

记直接线性变换（Direct Linear Transformation）得到的旋转矩阵为 $R$ ，平移向量为 $t$ ，投影方式为，
$\leftarrow (RR^T)^{-\frac{1}{2}}R$
然后代入线性方程组，利用最小二乘法更新平移向量 $t$ 。其中矩阵的平方根，表示其Cholesky分解，将原矩阵分解为下三角矩阵 $L$ 和上三角矩阵 $L^T$ 的乘积。因此，Cholesky分解也被称为平方根分解。

随机生成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$ 。也就是说，旋转和平移的真值如下，

$\begin{bmatrix} 0.9513 & -0.1677 & 0.2588 \\ 0.2504 & 0.9100 & -0.3304 \\ -0.1801 & 0.3791 & 0.9077 \end{bmatrix}$

$t=\begin{bmatrix} 0.2\\ 0.5\\ 0.7\\ \end{bmatrix}$

对比DLT求解出的R和t的误差和先DLT再投影到SE(3)流形上求解出来的R和t的误差。

2代码

Ubuntu下C++代码：

#include <iostream>
#include <fstream>
#include <random>
#include <ctime>
#include <opencv2/features2d.hpp>
#include <opencv2/xfeatures2d.hpp>
#include <opencv2/highgui.hpp>
#include <opencv2/core.hpp>
#include <opencv/cv.hpp>
#include <Eigen/Core>
#include <Eigen/Geometry>
#include <Eigen/Dense>

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案例分析

对于方程 $A x = b$ ，如果矩阵 $A$ 或常数项 $b$ 的微小变化，引起线性方程组的解 $x$ 的巨大变化，则称此线性方程组为病态方程组，矩阵 $A$ 称为病态矩阵（相对方程组而得），否则称线性方程组为“良态”方程组， $A$ 称为“良态”矩阵。
设 $A$ 为非奇异阵，称数 $cond(A)_v=||A^{-1}||_v||A||_v$ （ $v=1,2或\infty$ ）为矩阵 $A$ 的条件数。矩阵 $A$ 的条件数越小说明该矩阵越良态！
矩阵 $A$ 的无穷范数可计算为，
$||A||_\infty=\underset{1\leq i \leq n}{\operatorname {\max}} \sum_{j=1}^{n}|a_{ij}|$
可以理解为是最大行。