Personal blog: http://www.chenjianqu.com/
Original link: http://www.chenjianqu.com/show-82.html
This article is a summary of <visual SLAM14 talk> notes made. In the foregoing Bowen ROS- coordinate transformation (TF) describes the coordinate transformation mechanism in communication with ROS, but no mention coordinate transformation itself, and therefore this post supplement these basic concepts.
Movement between the two coordinate systems by one rotation plus translation of a composition, if the same vector length and angle coordinates at each change will not occur, this movement is called rigid body motion . Two coordinate systems before and after a difference of rigid motion European transformation. In addition to the European transformation, as well as other transformations summarized as follows:
European transformation: holding vector length and angle, including rotation and translation.
Similarity transformation: European transformation than a scaling factor s, the scaling may be performed after the rotation.
Affine transformation: affine transformation is required is an invertible matrix A, but not necessarily orthogonal matrix. After the affine transformation, the cube may become inclined, but each side still parallelogram.
Projective transformation: projective transformation is the most general transformation, the upper left corner is invertible matrix A, the upper right corner of the translation t, the lower left corner to scale a.
Each transform comparison:
Generally the coordinate transformation is the European transformation.
Rotation matrix and the transformation matrix
Rotation matrix
For the coordinate transformation, transformation vectors themselves do not occur, but the coordinate system changed, in accordance with a defined coordinate:
While on the left and right sides on the left by formula [e1, e2, e3] T, to obtain:
Obtained rotation matrix R & lt , each component of the matrix is the product of two coordinate groups, since the length of the base vector is 1, so this is actually the angle of each of the cosine basis vectors, also called the direction cosine matrix (Direction Cosine matrix ) . An orthogonal rotation matrix is a matrix determinant 1, whereas determinant 1 matrix is also orthogonal rotation matrix. Rotation matrix set:
SO (n) referred to the special orthogonal group (Group Special Orthogonal) . The inverse of the rotation matrix describes a reverse rotation, and R & lt RT-=. 1 .
Translation vector
European transformed by the rotational transformation and translation transformation composed by a rotational transformation rotation matrix R described, a translation transform t translation vectors described, i.e. the coordinate transformation A '= the RA + t .
Transformation matrix
Using the rotation matrix and translation vector described coordinate transformation, after many transformation formula is too complicated, and therefore the introduction of homogeneous coordinates and the transformation matrix . Homogeneous coordinates: Add 1 at the end of a three-dimensional vector into a four-dimensional vector. Transformation matrix T; the rotation and translation written in a matrix. Coordinate transformation can be expressed as follows:
Transform matrix set:
SE (3) means that a special European group (Group Special the Euclidean) . Inverse transform matrix also said that a reverse transformation:
And the Euler angles of rotation vector
Rotation vector
Description rotating with rotation matrix has the following disadvantages:
1.SO (3) rotation matrix nine amount, but only three degrees of freedom one rotation, there is redundancy in this way. The same transformation matrix.
2.旋转矩阵自身带有约束：必须是正交矩阵，而且行列式为1。当想要估计或优化一个旋转矩阵/变换矩阵时，这些约束会使得求解变得更加困难。
因此需要一种更加紧凑方式的描述旋转和平移。任意旋转都可以用一个旋转轴和一个旋转角来刻画，使用一个向量，其方向和旋转轴一致，长度等于旋转角，这种向量称为旋转向量（或轴角，Axis-Angle）。这种表示法仅需要一个三维向量即可描述旋转。使用旋转向量加平移向量，只有六维，即可描述一次变换。
旋转向量和旋转矩阵之间的转换
假设有一个旋转轴为n，角度为θ的旋转，则其对应的旋转向量为θ*n。从旋转向量到旋转矩阵的转换使用罗德里格斯公式：
符号^表示向量到反对称的转换符，如下：
反之从旋转矩阵转换到旋转向量，对于转角θ，有：
其中tr(R)表示矩阵R的迹。对于转轴n，由于旋转轴上的向量在旋转后不发生改变，即Rn=n，因此转轴n是旋转矩阵R特征值1对应的特征向量。求解此方程，再归一化，就得到了旋转轴。
欧拉角
旋转矩阵和旋转向量虽然能描述旋转，但是非常不直观。欧拉角提供了非常直观的方式描述旋转，将一个旋转分解为3次绕不同轴的旋转。比如先绕X轴，再绕Y轴，最后绕Z轴就得到XYZ轴的旋转。同理得ZYX,ZXY等方式的旋转。这里使用ZYX分解，假设一个刚体的前方(朝向我们的方向)为X轴，右侧为Y轴，上方为Z轴。
1.绕物体的Z轴旋转，得到偏航角yaw；
2.绕旋转之后的Y轴旋转，得到俯仰角pitch；
3.绕旋转之后的X轴旋转，得到滚转角roll。
欧拉角和旋转向量的一个重大缺点是万向锁问题(Gimbal Lock)：在俯仰角为+-90度时，第一次旋转和第三次旋转将使用同一个轴，这使得系统丢失了一个自由度，这被称为奇异性问题。三个实数表示旋转一定会碰到奇异性问题，因为三维旋转是一个三维流形，想要无奇异的表达它，三个量是不够的。因此欧拉角往往只用于人际交互。
四元数
概念
旋转矩阵具有冗余性，而欧拉角和旋转向量虽然紧凑，但是具有奇异性。四元数(Quaternion)是一种扩展的复数，用它来表示旋转既紧凑，又没有奇异性。 一个四元数q有一个实部和三个虚部，如q = q0 + q1*i + q2*j + q3*k ，其中i,j,k为四元数的三个虚部。三个虚部满足如下关系式：
也可以用一个标量和一个向量表示四元数：q=[s,v]，s=q0，v=[q1,q2,q3]，s称为实部，v称为虚部。若虚部为0，则该四元数称为实四元数，反之称之为虚四元数。
四元数的运算
1.加减法
2.乘法
或
3.模长
两个四元数乘积的模即为模的乘积
4.共轭
四元数共轭与其本身相乘，得到一个实四元数，其实部为模长的平方：
5.逆
一个四元数的逆为：
且：
6.数乘
用四元数表示旋转
可以用一个单位四元数q表示旋转。假设空间点p为[x,y,z]，旋转后的点为p’。用一个虚四元数描述p点p=[0,x,y,z]，则p’=q*p*q-1，结果p’的虚部就是旋转后的坐标。
将四元数乘法写成矩阵形式
q=[s,v]，定义如下符号：
则可以将四元数的乘法写成矩阵形式：
则用四元数矩阵乘法表示旋转：
四元数和旋转矩阵的转换
四元数转换为旋转矩阵：
设四元数q=q0+q1*i+q2*j+q3*k，对应的旋转矩阵R为：
旋转矩阵到四元数的转换：已知旋转矩阵为R={m_ij}，i,j属于[1,2,3]，其对应的四元数为：
四元数和旋转向量的转换
Eigen实现
Eigen是一个开源的C++线性代数库。坐标变换的Eigen数据类型：
CMakeLists.txt
cmake_minimum_required(VERSION 2.6)
project(eigengeometrytest)
include_directories("/usr/include/eigen3")
add_executable(eigengeometrytest main.cpp)
install(TARGETS eigengeometrytest RUNTIME DESTINATION bin)
main.cpp
#include <iostream>
#include<ctime>
#include<Eigen/Core>
#include<Eigen/Geometry>
#include<Eigen/Dense>
using namespace std;
using namespace Eigen;
int main(int argc, char **argv) {
std::cout << "Hello, world!" << std::endl;
//旋转矩阵使用Matrix3d
Eigen::Matrix3d rotation_m=Eigen::Matrix3d::Identity();
//旋转向量 转角,转轴
Eigen::AngleAxisd rotation_v(M_PI/4,Eigen::Vector3d(0,0,1));
cout.precision(3);
//将旋转向量转换为旋转矩阵
cout<<"rotation_v.matrix:\n"<<rotation_v.matrix()<<endl<<endl;
rotation_m=rotation_v.toRotationMatrix();
cout<<"rotation_m:\n"<<rotation_m<<endl<<endl;
//使用旋转向量进行旋转变换
Eigen::Vector3d v(1,0,0);
Eigen::Vector3d v_r=rotation_v*v;
cout<<v<<endl<<"after v rotated:"<<v_r<<endl<<endl;
//使用旋转矩阵进行旋转变换
v_r=rotation_m*v;
cout<<v<<endl<<"after m rotated:"<<v_r<<endl<<endl;
//将旋转矩阵转换为欧拉角ZYX
Eigen::Vector3d euler_a=rotation_m.eulerAngles(2,1,0);
cout<<"yaw pitch roll="<<euler_a.transpose()<<endl<<endl;
//欧式变换矩阵
Eigen::Isometry3d T=Eigen::Isometry3d::Identity();//实质是4x4矩阵
//设置旋转
T.rotate(rotation_v);
//设置平移向量
T.pretranslate(Eigen::Vector3d(1,3,4));
cout<<"Transform m:\n"<<T.matrix()<<endl<<endl;
//旋转向量转换为四元数
Eigen, Q = :: Quaterniond Eigen, :: Quaterniond (rotation_v);
// outputs q1 q2 q3 q0, the last one is the real part
cout << "q: \ n" << q.coeffs () << endl << endl ;
// convert rotation vector rotation matrix
Q :: = Eigen, (rotation_m) Quaterniond;
COUT << "Q \ n-" << q.coeffs () << endl << endl;
// quaternion using rotation
v_r = q * v; // multiplication is overloaded, ^ -1 it is mathematically QVQ
COUT << "v_q_r:" << endl << V_R;
return 0;
}
