三维空间刚体运动
旋转矩阵
内积和外积
内积:对于三维实数空间中的两个向量a,b,内积=aTb=a0b0+a1b1+a2b2=|a||b|cos<a,b>,内积可以描述两个向量间的投影关系。
外积:外积的方向垂直与这两个向量,大小为|a||b|sin<a,b>,是两个向量张成的四边形的有向面积。
值得强调的是,对于外积,我们引入了^符号,用这个符号将向量a转换成了一个反对称矩阵,所以该符号可以被看成是一个反对称符号。利用这个反对称符号就可以把外积a×b写成矩阵与向量的乘法,从而把它变成了线性运算。外积可以被用来表示向量的旋转。
w表示旋转向量,它的方向同时垂直于向量a和b,模值反映了旋转角度的大小。
坐标间的欧式变换
对于一个向量p,它在世界坐标系下的坐标pw和在相机坐标系下的坐标pc的变换,可以通过坐标系间的变换矩阵T来描述。
相机运动是一个刚体运动,它保证了同一个向量在各个坐标系下的长度和夹角都不会发生变化,这种变换称为欧式变换。
欧式变换由一个旋转和一个平移两部分组成。
首先考虑旋转:
假设某个单位正交基(e1,e2,e3)经过一次旋转变成了(e1’,e2’,e3’).那么对于同一个向量a,它在两个坐标系下的坐标为[a1,a2,a3]T和[a1’,a2’,a3’]T,那么有:
从而
我们把中间的矩阵定义成旋转矩阵R,它描述了同一个向量在旋转前后的坐标变换关系。旋转矩阵是一个行列式为1的正交矩阵,反之,行列式为1的正交矩阵也是一个旋转矩阵,可以把旋转矩阵的集合定义如下:
SO(n)是特殊正交群的意思,显然这里的特殊包括行列式等于1这条性质。通过旋转矩阵而不是基来描述相机的旋转显然更加简洁专业。
由于旋转矩阵为正交矩阵,它的逆(即转置)描述了一个相反的旋转。
a’=R-1a=RTa
再考虑平移:
a’=Ra+t
其中,t表示平移向量,通过上式,我们用一个旋转矩阵R和一个平移矩阵t完整地描述了一个欧式空间的坐标变换关系。
变换矩阵与齐次坐标
非齐次坐标表示多次坐标变换是很复杂的,例如,假设我们进行了两次坐标变换R1,t1和R2,t2,满足b=R1a+t1,c=R2b+t2,但是从a到c的变换为c=R2(R1a+t1)+t2,如果再继续多次坐标变换将会更加复杂,究其原因,发现每次变换都要加t,如果引入齐次坐标,式子将会变得非常简洁。
其中,T称为变换矩阵,它具有比较特别的结构:左上角为旋转矩阵R,右侧为平移矩阵t,左下角为0向量,右下角为1,这种矩阵又称为特殊欧式群。
与SO(3)一样,求解该矩阵的逆表示一个反向的变换
为了保持符号的简洁,对齐次坐标的符号与普通坐标的符号不加以区分,默认使用符合运算法则的那一种。
因此,可以将b=T1a,c=T2b表示成c=T2T1a,形式非常简洁。
实践:Eigen
1.ubuntu下Eigen的安装
sudo apt-get install libeigen3-dev
Eigen头文件的默认位置在"/usr/include/eigen3/"中.如果不确定,可以输入以下命令查找:
sudo updatedb
locate eigen3
2.Eigen头文件
//Eigen部分
#include <Eigen/Core>
//稠密矩阵的代数运算
#include <Eigen/Dense>
3.Eigen::Matrix()
Eigen以矩阵为基本数据单元。它是一个模板类。它的前三个参数为:数据类型,行,列。
例如:声明一个2行3列的float矩阵
Eigen::Matrix<float,2,3> matrix_23;
Vector3d实质上是Eigen::Matrix<double,3,1>
Matrix3d实质上是Eigen::Matrix<double,3,3>
//动态大小的矩阵
Eigen::Matrix<double,Eigen::Dynamic,Eigen::Dynamic> matrix_dynamic;
//更简单的动态大小矩阵
Eigen::MatrixXd matrix_x;
4.用<<为矩阵输入数据
matrix_23<<1,2,3,4,5,6;
5.用()访问矩阵中的元素
matrix_23(i,j);//访问第i行第j列的数据
6.矩阵相乘
注意:这里不能直接混合两种不同类型的矩阵相乘,应该显式转换
v_3d<<3,2,1;
Eigen::Matrix<double,2,1> result = matrix_23.cast<double>() * v_3d;
7.矩阵转置
matrix_33.transpose()
8.矩阵各个元素的和
matrix_33.sum()
9,矩阵的迹
matrix_33.trace()
10.矩阵数乘
10*matrix_33
11.矩阵直接求逆
matrix_33.inverse()
12.求矩阵的行列式
matrix_33.determinant()
13.生成随机矩阵
Eigen::Matrix3d::Random()
Eigen::MatrixXd::Random(MATRIX_SIZE,MATRIX_SIZE)等
14.求特征值和特征向量
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;
15.解方程
//我们求解matrix_NN * x = v_Nd这个方程
//N由宏定义指定,矩阵由随机数生成
//直接求逆自然是最直接的,但是求逆运算量大
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 inverse is:"<< 1000*(clock()-time_stt)/(double)CLOCKS_PER_SEC<<"ms"<<endl;
//通常用矩阵分解来求逆,例如QR分解,速度会快很多
time_stt = clock();
x = matrix_NN.colPivHouseholderQr().solve(v_Nd);
cout<< "Time use in Qr conposition is:"<< 1000*(clock()-time_stt)/(double)CLOCKS_PER_SEC<<"ms"<<endl;
完整代码:
#include <iostream>
#include <ctime>
using namespace std;
//Eigen部分
#include <Eigen/Core>
//稠密矩阵的代数运算(逆、特征值等)
#include <Eigen/Dense>
#define MATRIX_SIZE 50
//演示Eigen基本类型的使用
int main()
{
//Eigen以矩阵为基本数据单元。它是一个模板类。它的前三个参数为:数据类型,行,列
//声明一个2×3的float矩阵
Eigen::Matrix<float,2,3> matrix_23;
//同时,Eigen通过typedef提供了许多内置类型,不过底层仍是Eigen:Matrix
//例如,Vector3d实质上是Eigen::Matrix<double,3,1>
Eigen::Vector3d v_3d;
//还有Matrix3d实质上是Eigen::Matrix<double,3,3>
Eigen::Matrix3d matrix_33 = Eigen::Matrix3d::Zero();//初始化为0
//如果不确定矩阵大小,可以使用动态大小的矩阵
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_wrong_type = matrix_23 * v_3d;
//应该显式转换
Eigen::Matrix<double,2,1> result = matrix_23.cast<double>() * v_3d;
cout << result << endl;
//同样也不能搞错矩阵的维度
//Eigen::Matrix<double,2,3> result_wrong_dimension = matrix_23 * v_3d;
//一些矩阵运算
matrix_33 = Eigen::Matrix3d::Random();//随机生成一些数据
cout << matrix_33 << 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;
//解方程
//我们求解matrix_NN * x = v_Nd这个方程
//N由宏定义指定,矩阵由随机数生成
//直接求逆自然是最直接的,但是求逆运算量大
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 inverse is:"<< 1000*(clock()-time_stt)/(double)CLOCKS_PER_SEC<<"ms"<<endl;
//通常用矩阵分解来求逆,例如QR分解,速度会快很多
time_stt = clock();
x = matrix_NN.colPivHouseholderQr().solve(v_Nd);
cout<< "Time use in Qr conposition is:"<< 1000*(clock()-time_stt)/(double)CLOCKS_PER_SEC<<"ms"<<endl;
return 0;
}
CMakeLists.txt文件
#添加Eigen的头文件
include_directories("/usr/include/eigen3")
#添加一个可执行程序
add_executable( eigenMatrix eigenMatrix.cpp )
运行结果:
旋转向量和欧拉角
旋转向量
引入旋转向量的原因:
1.旋转矩阵具有冗余性:SO(3)的旋转矩阵具有9个量,但是一次旋转本质上只有3个自由度。同理,变换矩阵用16个量表达了6个自由度,也具有冗余性。
2.旋转矩阵自身带有约束:它必须是行列式为1的正交矩阵,这些约束将使得优化一个旋转矩阵\变换矩阵变得困难。
旋转向量:
使用一个向量,其方向n与旋转轴一致,而长度等旋转角theta,这种向量称为旋转向量。
同理,对于变换矩阵,我们使用一个旋转向量和一个平移向量即可表达一次变换。
旋转向量与旋转矩阵的相互转换由罗德里格斯公式表明:
由旋转向量转换成旋转矩阵:
由旋转矩阵转化为旋转向量:
转轴n是旋转矩阵R特征值1对应的特征向量。
欧拉角
rpy欧拉角采用的旋转顺序是ZYX,ZYX转角相当于把任意旋转分解成以下三个轴上的转角:
1.绕物体的Z轴旋转,得到偏航角yaw
2.绕旋转之后的Y轴旋转,得到俯仰角pitch
3.绕旋转之后的X轴旋转,得到转滚角roll
此时,可以使用[r,p,y]T这样一个三维的向量描述任意旋转。
欧拉角的一个重大缺点是会碰到著名的万向锁问题:在俯仰角为±90度时,第一次旋转与第三次旋转将使用同一个轴,使得系统丢失了一个自由度(由三次旋转变成了两次旋转)。
四元数
四元数的定义
旋转矩阵表达不紧凑,旋转向量表达具有奇异性,事实上,我们找不到不带奇异性的三维向量描述方式。
四元数是一种扩展的复数,它既是紧凑的,也没有奇异性,但是其表达不够直观,运算也稍复杂。
定义:一个四元数q拥有一个实部和三个虚部。q=q0+q1i+q2j+q3k,其中这三个虚部满足以下关系式:
用用一个标量和一个向量来表达之:
这里s称为四元数的实部,而v称为它的虚部。如果一个四元数的虚部为0,称之为实四元数,反之,若它的实部为0,则称之为虚四元数。
复数的乘法可以表示复平面的旋转,比如,乘上复数i相当于逆时针把一个复向量旋转90度。四元数也具有相似的性质。只不过这里,乘以i对应着绕i轴旋转180度,而绕着i轴旋转360度会得到一个跟原来相反的东西,这个东西要再旋转360度才会和它原来的样子相等。
四元数与旋转向量的相互转换
假设某个旋转是绕单位向量n=[nx,ny,nz]T进行了角度为theta的旋转,那么
从旋转向量转化为四元数:
从四元数转换为旋转向量:
注意:这里有一些奇怪的地方,旋转向量旋转360度仍然为它本身,而四元数旋转360度则变为它的相反数。因此,在四元数中,任意的旋转都可以由两个互为相反数的四元数表示。
四元数的运算
现有两个四元数qa和qb,它们的向量表示为[sa,va],[sb,vb]
-
加法和减法
qa+qb=[sa+sb,va+vb]
qa-qb=[sa-sb,va-vb] -
乘法
乘法将qa 与 qb 的每一项相乘,然后相加,合并项。
或者
-
共轭
四元数的共轭是把虚部取成相反数:
四元数共轭与其本身相乘,会得到一个实四元数,其实部为模长的平方
-
模长
可以验证,两个四元数乘积的模即为模的乘积:
- 逆
按照这个定义,四元数和自己逆的乘积为实四元数1
- 数乘和点乘
数乘:
kq=[ks,kv]
点乘:
用四元数表示旋转
假设有一个空间三维点p=[x,y,z]以及一个由轴角n,theta指定的旋转,三维点p经过旋转之后变为p’。
如果用旋转向量R来表示这个旋转,那么很显然:p’=Rp
如果用四元数来表示:
把三维空间点用一个虚四元数来表示,p=[0,x,y,z]=[0,v],这相当于把四元数的三个虚部与空间中的三个轴相对应。
把轴角表示成四元数q
那么用四元数来表示这个点的旋转为:
p’ = qpq-1
四元数与旋转矩阵的相互转换
设四元数q=q0+q1i+q2j+q3k,则对应的旋转矩阵为R。
四元数转换为旋转矩阵:
旋转矩阵转换为四元数:
设旋转矩阵
则:
注意:实际上,由于q和 -q 表示同一个旋转,所以一个R对应的四元数表示并不是唯一的。
相似、仿射、射影变换
实践:Eigen几何模块
1.Eigen中各种形式的表达方式
2.将旋转向量转换成旋转矩阵
rotation_vector.matrix()
或
rotation_matrix = rotation_vector.toRotationMatrix();
3.用旋转向量进行坐标转换
Eigen::Vector3d v(1,0,0);
Eigen::Vector3d v_rotated = rotation_vector*v;
cout << "(1,0,0)after rotation="<<v_rotated.transpose()<<endl;
4.用旋转矩阵进行坐标转换
v_rotated = rotation_matrix*v;
5.将旋转矩阵转换成欧拉角
Eigen::Vector3d euler_angles = rotation_matrix.eulerAngles(2,1,0);//ZYX顺序,即yaw pitch roll顺序
cout << "yaw,pitch,roll = "<<euler_angles.transpose()<<endl;
6.用旋转向量和平移向量构建欧式变换矩阵
Eigen::Isometry3d T=Eigen::Isometry3d::Identity();//虽然称为3d,实质上是4×4的矩阵
T.rotate(rotation_vector);//按照旋转向量rotation_vector进行旋转
T.pretranslate(Eigen::Vector3d(1,3,4));//把平移向量设置成(1,3,4)
cout<<"Transform matrix = \n"<<T.matrix()<<endl;
7.用变换矩阵进行坐标变换
Eigen::Vector3d v_transformed = T*v;//这相当于R*v+t
cout<<"v transformed = "<<v_transformed.transpose()<<endl;
8.将旋转向量转换成四元数
Eigen::Quaterniond q = Eigen::Quaterniond(rotation_vector);
cout<<"quaternion = \n"<<q.coeffs()<<endl;//这里coeffs的顺序是(x,y,z,w),w为实部,前三者为虚部
9.将旋转矩阵转换成四元数
q = Eigen::Quaterniond(rotation_matrix);
10.用四元数进行坐标变换
v_rotated = q*v;//这里用的是重载的乘法,数学上是qvq^{-1}
cout<<"(1,0,0) after rotation = "<<v_rotated.transpose()<<endl;
完整代码
//2019.08.04
#include <iostream>
#include <cmath>
using namespace std;
#include <Eigen/Core>
//Eigen几何模块
#include <Eigen/Geometry>
//Eigen几何模块使用方法演示
int main()
{
//Eigen/Geometry模块提供了各种旋转和平移的表示
//3D旋转矩阵直接使用Matrix3d或者Matrix3f
Eigen::Matrix3d rotation_matrix = Eigen::Matrix3d::Identity();//将旋转矩阵初始化为单位阵
//旋转向量使用AngleAxis,它底层不直接是Matrix,但运算可以当作矩阵(因为重载了运算符)
Eigen::AngleAxisd rotation_vector(M_PI/4,Eigen::Vector3d(0,0,1));//将旋转向量初始化为沿Z轴旋转度
//将旋转向量转换成旋转矩阵
//cout.percision(3);
cout << "rotation matrix = \n"<<rotation_vector.matrix()<<endl;
//转换方式2,可以直接赋值给旋转矩阵
rotation_matrix = rotation_vector.toRotationMatrix();
//用旋转向量进行坐标转换
Eigen::Vector3d v(1,0,0);
Eigen::Vector3d v_rotated = rotation_vector*v;
cout << "(1,0,0)after rotation="<<v_rotated.transpose()<<endl;
//用旋转矩阵进行坐标转换
v_rotated = rotation_matrix*v;
cout << "(1,0,0)after rotation="<<v_rotated.transpose()<<endl;
//将旋转矩阵转换成欧拉角
Eigen::Vector3d euler_angles = rotation_matrix.eulerAngles(2,1,0);//ZYX顺序,即yaw pitch roll顺序
cout << "yaw,pitch,roll = "<<euler_angles.transpose()<<endl;
//用旋转向量和平移向量构建欧式变换矩阵
Eigen::Isometry3d T=Eigen::Isometry3d::Identity();//虽然称为3d,实质上是4×4的矩阵
T.rotate(rotation_vector);//按照旋转向量rotation_vector进行旋转
T.pretranslate(Eigen::Vector3d(1,3,4));//把平移向量设置成(1,3,4)
cout<<"Transform matrix = \n"<<T.matrix()<<endl;
//用变换矩阵进行坐标变换
Eigen::Vector3d v_transformed = T*v;//这相当于R*v+t
cout<<"v transformed = "<<v_transformed.transpose()<<endl;
//对于仿射和射影变换,使用Eigen::Affine3d和Eigen::Projective3d即可,这里略
//将旋转向量转换成四元数
Eigen::Quaterniond q = Eigen::Quaterniond(rotation_vector);
cout<<"quaternion = \n"<<q.coeffs()<<endl;//这里coeffs的顺序是(x,y,z,w),w为实部,前三者为虚部
//将旋转矩阵转换成四元数
q = Eigen::Quaterniond(rotation_matrix);
//用四元数进行坐标变换
v_rotated = q*v;//这里用的是重载的乘法,数学上是qvq^{-1}
cout<<"(1,0,0) after rotation = "<<v_rotated.transpose()<<endl;
return 0;
}