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。其中矩阵的平方根,表示其Cholesky分解,将原矩阵分解为下三角矩阵
L
L
L和上三角矩阵
L
T
L^T
LT的乘积。因此,Cholesky分解也被称为平方根分解。
随机生成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。也就是说,旋转和平移的真值如下,
R = [ 0.9513 − 0.1677 0.2588 0.2504 0.9100 − 0.3304 − 0.1801 0.3791 0.9077 ] R = \begin{bmatrix} 0.9513 & -0.1677 & 0.2588 \\ 0.2504 & 0.9100 & -0.3304 \\ -0.1801 & 0.3791 & 0.9077 \end{bmatrix} R=⎣⎡0.95130.2504−0.1801−0.16770.91000.37910.2588−0.33040.9077⎦⎤
t = [ 0.2 0.5 0.7 ] t=\begin{bmatrix} 0.2\\ 0.5\\ 0.7\\ \end{bmatrix} t=⎣⎡0.20.50.7⎦⎤
对比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
Ax=b
Ax=b,如果矩阵
A
A
A或常数项
b
b
b的微小变化,引起线性方程组的解
x
x
x的巨大变化,则称此线性方程组为病态方程组,矩阵
A
A
A称为病态矩阵(相对方程组而得),否则称线性方程组为“良态”方程组,
A
A
A称为“良态”矩阵。
设
A
A
A为非奇异阵,称数
c
o
n
d
(
A
)
v
=
∣
∣
A
−
1
∣
∣
v
∣
∣
A
∣
∣
v
cond(A)_v=||A^{-1}||_v||A||_v
cond(A)v=∣∣A−1∣∣v∣∣A∣∣v(
v
=
1
,
2
或
∞
v=1,2或\infty
v=1,2或∞)为矩阵
A
A
A的条件数。矩阵
A
A
A的条件数越小说明该矩阵越良态!
矩阵
A
A
A的无穷范数可计算为,
∣
∣
A
∣
∣
∞
=
max
1
≤
i
≤
n
∑
j
=
1
n
∣
a
i
j
∣
||A||_\infty=\underset{1\leq i \leq n}{\operatorname {\max}} \sum_{j=1}^{n}|a_{ij}|
∣∣A∣∣∞=1≤i≤nmaxj=1∑n∣aij∣
可以理解为是最大行。
4.1 平移误差大的例子
A | B | C | D |
---|---|---|---|
-61.83 6.49 12.66 | -53.48 4.349 26.70 | 53.83 7.66 49.53 | 42.59 16.10 2.29 |
-56.78 -13.17 25.79 | -44.63 -17.13 36.04 | 63.19 4.73 38.60 | 38.59 25.44 0.75 |
其中第1行表示随机生产的4个点,第2行表示经过真实欧式变化加噪声之后的4个点。由第一行计算出来的无穷条件数为16620304。
4.2 平移误差小的例子
A | B | C | D |
---|---|---|---|
87.78 -9.78 7.44 | 61.75 10.61 68.46 | -47.75 -0.86 42.90 | 33.08 -16.71 89.99 |
86.92 11.19 -12.07 | 74.74 3.62 55.57 | -33.71 -26.26 47.66 | 57.74 -35.77 69.63 |
计算出此例的无穷条件数为270!
由上述可知,第2个例子的条件数远小于第1个例子的,说明第2个例子的系数矩阵更良态,而结果也确实如此(第2个例子计算出来的平移误差更小)。
5结论
虽然在角度误差上方法1和方法2相近;但在平移误差上,方法2在均值和标准差上优于方法1,尤其是在标准差上!