视觉SLAM十四讲 读书编程笔记 Chapte3 三维空间刚体运动

旋转矩阵

内积和外积

内积:对于三维实数空间中的两个向量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,t1R2,t2,满足b=R1a+t1,c=R2b+t2,但是从ac的变换为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度则变为它的相反数。因此,在四元数中,任意的旋转都可以由两个互为相反数的四元数表示。

四元数的运算

现有两个四元数qaqb,它们的向量表示为[sa,va],[sb,vb]

  1. 加法和减法
    qa+qb=[sa+sb,va+vb]
    qa-qb=[sa-sb,va-vb]

  2. 乘法
    乘法将qaqb 的每一项相乘,然后相加,合并项。
    在这里插入图片描述
    或者
    在这里插入图片描述

  3. 共轭
    四元数的共轭是把虚部取成相反数:
    在这里插入图片描述
    四元数共轭与其本身相乘,会得到一个实四元数,其实部为模长的平方
    在这里插入图片描述

  4. 模长

在这里插入图片描述
可以验证,两个四元数乘积的模即为模的乘积:
在这里插入图片描述


  1. 在这里插入图片描述
    按照这个定义,四元数和自己逆的乘积为实四元数1
    在这里插入图片描述
  2. 数乘和点乘
    数乘:
    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;
	
}

运行结果

在这里插入图片描述

评论
添加红包

请填写红包祝福语或标题

红包个数最小为10个

红包金额最低5元

当前余额3.43前往充值 >
需支付:10.00
成就一亿技术人!
领取后你会自动成为博主和红包主的粉丝 规则
hope_wisdom
发出的红包
实付
使用余额支付
点击重新获取
扫码支付
钱包余额 0

抵扣说明:

1.余额是钱包充值的虚拟货币,按照1:1的比例进行支付金额的抵扣。
2.余额无法直接购买下载,可以购买VIP、付费专栏及课程。

余额充值