1.欧式变换
在视觉slam中,相机是运动定位,构图的。而它是一个刚体运动,它保证了同一个向量在各个坐标系下的长度和夹角都不会发生变化,这种变化称为欧式变换。例如: 手机抛向天空,在摔碎之前,只可能有空间位置和姿态的不同,而它自己的长度,各个面的角度等性质不会有任何变化。这样一个欧式变换由一个旋转和一个平移两部分组成。
直接给出欧式空间的坐标变换关系:
a′=Ra+t
a
′
=
R
a
+
t
矩阵R描述了旋转本身,故又称为旋转矩阵。
t 则代表平移t。
其中R的推导如下:
对于某一单位(e1, e2, e3)经过一次旋转变成(e1’ , e2’ , e3’)。对于同一个向量a, 它在两个坐标下的坐标为[a1, a2, a3]和[a1’ ,a2’ ,a3’]. 则有:
为了很好的对比两个坐标之间的关系,我们对上述等式的左右同右乘[e1T, e2T, e3T]。则
这个R就称之为旋转矩阵,可知旋转矩阵有些特别的性质,它是一个行列式为1的正交矩阵。反之,行列式为1的正交矩阵也是一个旋转矩阵。
2.Eigen库
Eigen是一个C++开源线性代数库,它提供了快速的有关线性代数运算,还有解方程等功能。许多上层的软件库也是使用Eigen进行矩阵运算,包括g2o, Sophus等。
#include <iostream>
#include <ctime>
using namespace std;
#include <Eigen/Core>
#include <Eigen/Dense>
#define MATRIX_SIZE 50
int main(int argc, char **argv)
{
//Eigen 以矩阵为基本数据单位。它是一个模板类。前三个参数为:数据类型,行,列
//声明一个2x3的float矩阵
Eigen::Matrix<float,2,3> matrix_23;
/*Eigen通过typedef提供了许多内置类型,不过底层仍然是Eigen::Matrix
*例如:Vector3d 实质上是Eigen::Matrix<double,3,1>
*/
Eigen::Vector3d v_3d;
Eigen::Matrix3d matrix_33 = Eigen::Matrix3d::Zero();
Eigen::Matrix<double,Eigen::Dynamic,Eigen::Dynamic> matrix_dynamic;
Eigen::MatrixXd matrix_x;
matrix_23 << 1,2,3,4,5,6;
cout << matrix_23 <<endl;
for(int i=0;i<1;i++)
for(int j=0;j<2;j++)
cout << matrix_23(i,j) << endl;
v_3d << 3,2,1;
Eigen::Matrix<double,2,1> result = matrix_23.cast<double>()*v_3d;
cout << result << endl;
matrix_33 = Eigen::Matrix3d::Random();
cout << matrix_33 << endl <<endl;
cout << matrix_33.transpose() << endl;
cout << matrix_33.sum() << endl;
cout << matrix_33.trace() << endl;
cout << 10*matrix_33 << endl;
cout << matrix_33.inverse() << endl;
cout << matrix_33.determinant() << endl;
Eigen::SelfAdjointEigenSolver<Eigen::Matrix3d> eigen_solver(matrix_33,transpose()*matrix_33);
cout << "Eigen values = " << eigen_solver.eigenvalues() << endl;
cout << "Eigen vectors =" << eigen_solver.eigenvectors() << endl;
Eigen::Matrix<double,MATRIX_SIZE,MATRIX_SIZE> matrix_NN;
matrix_NN = Eigen::MatrixXd::Random(MATRIX_SIZE,MATRIX_SIZE);
Eigen::Matrix<double,MATRIX_SIZE,1> v_Nd;
v_Nd = Eigen::MatrixXd::Random(MATRIX_SIZE,1);
clock_t time_stt = clock();
Eigen::Matrix<double,MATRIX_SIZE,1> x = matrix_NN.inverse*v_Nd;
cout << "time use in normal invers is " << 1000*(clock()-time_stt)/(double)CLOCKS_PER_SEC << "ms" << endl;
time_stt = clock();
x = matric_NN.colPivHouseholderQr().solve(v_Nd);
cout << "time use in Qr compsition is " << 1000* (clock()-time_stt)/(double)CLOCKS_PER_SEC << "ms" << endl;
return 0;
}
如上打出的代码段。