三维空间刚体运动
旋转矩阵
内积和外积
内积：对于三维实数空间中的两个向量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;
}
运行结果
1961
扫一扫
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
