视觉SLAM十四讲-第三章笔记

视觉SLAM14讲-第三章笔记

3.1 旋转矩阵

欧式变换

• 欧式变换：改变位资，不改变形状、大小。
• 旋转矩阵：R(3x3)。是两个坐标系基的内积。
  • 正交阵
  • 行列式为1
  • 逆表示相反的旋转
• 平移向量：t(3x1)
• 位姿变换：a’=Ra+t

齐次坐标

• 齐次坐标

多引入一维(1*4)，为[0,0,0,1]，每一个点的表示不唯一（乘以最后一维的尺度）。

• 变换矩阵：T(4x4)

将R和t写入同一个矩阵。使得变换变成线性关系。

3.3 旋转向量和欧拉角

3.3.1 旋转向量

• 旋转轴：向量n。旋转朝向。
  • 是R特征值1对应的特征向量。（旋转后n不变，即Rn=1×n）
• 旋转角：标量theta。旋转大小。

3.3.2 欧拉角

一次旋转分解成绕三个轴的连续旋转。

• ZYX欧拉角

顺序：

1.Z轴——>偏航角 yaw

2.Y轴——>俯仰角 pitch

3.X轴——>翻滚角 rool

描述向量：[r,p,y]。（描述向量的顺序仍是X,Y,Z轴，但旋转顺序是Z,Y,X）

• 欧拉角存在问题：万向锁

俯仰角为±90°时，第一次旋转和第三次旋转是一个轴，丢失一个自由度。（Z轴和X轴在Y轴转90°后，都指向原Z轴方向）

欧拉角可以与旋转矩阵转换：
(旋转矩阵)[https://zh.wikipedia.org/wiki/旋转矩阵]

3.4 四元数

特点：无冗余，也无奇异性。
形式：

• 实四元数：虚部为0。
• 虚四元数：实部为0，表示点。（虚部表示一个向量）
• **单位四元数：模长为1，表示旋转。**任意旋转可由两个互为相反数的四元数表示。

3.5 各种变换

• 欧式变换：保留向量的长度和夹角，不改变外形。T正交、行列式为1。
• 相似变换：旋转后，均匀缩放。R——>sR，s：缩放因子。
• 放射变换：保平行、体积比，不再是方的。R——>A。A可逆，不必正交。
• 射影变换：加入缩放向量。不保平行。

练习题

3

  1 #include 
  2 #include  
  3 #include 
  4 
  5 using namespace std;
  6 int main()
  7 {
  8     Eigen::Quaterniond q1(0.35,0.2,0.3,0.1);
  9     q1=q1.normalized();
 10     Eigen::Quaterniond p(0,0.1,0.2,0.3);
 11     cout<<"p"<

p:
0.1
0.2
0.3
  0
p after rotate:
    0.347619
    0.104762
   0.0904762
-1.38778e-17

5

  1 #include 
  2 #include  
  3 
  4 using namespace std;
  5 
  6 int main()
  7 {
  8     Eigen::Matrixmat0;
  9     mat0=Eigen::MatrixXf::Random(7,9);
 10     cout<<"mat0:"<mat1=mat0.block(0,0,3,3);
 13     cout<<"mat1:"<

mat0:
  0.680375   0.536459   0.904459  -0.514226   0.997849  0.0485743  -0.199543   0.898654  -0.871657
 -0.211234  -0.444451    0.83239  -0.725537  -0.563486  -0.012834   0.783059  0.0519907  -0.959954
  0.566198    0.10794   0.271423   0.608353  0.0258648    0.94555  -0.433371  -0.827888 -0.0845965
   0.59688 -0.0452059   0.434594  -0.686642   0.678224  -0.414966  -0.295083  -0.615572  -0.873808
  0.823295   0.257742  -0.716795  -0.198111    0.22528   0.542715   0.615449   0.326454   -0.52344
 -0.604897  -0.270431   0.213938  -0.740419  -0.407937  0.0534899   0.838053   0.780465   0.941268
 -0.329554  0.0268018  -0.967399  -0.782382   0.275105   0.539828  -0.860489  -0.302214   0.804416
mat1:
 0.680375  0.536459  0.904459
-0.211234 -0.444451   0.83239
 0.566198   0.10794  0.271423
mat1:
1 0 0
0 1 0
0 0 1

7

  1 #include 
  2 #include  
  3 #include 
  4 
  5 using namespace std;
  6 int main()
  7 {
  8     Eigen::Quaterniond q1(0.35,0.2,0.3,0.1);
  9     Eigen::Quaterniond q2(-0.5,0.4,-0.1,0.2);
 10     Eigen::Vector3d t1(0.3,0.1,0.1);
 11     Eigen::Vector3d t2(-0.1,0.5,0.3);
 12     Eigen::Vector3d p1(0.5,0,0.2);
 13 
 14     q1=q1.normalized();
 15     q2=q2.normalized();
 16 
 17     Eigen::Vector3d p0=q1.inverse()*(p1-t1);
 18     Eigen::Vector3d p2=q2*p0+t2;
 19 
 20     cout<

-0.0309731
 0.73499
 0.296108