视觉slam14讲——习题部分

本文是记录学习高翔所著《视觉slam14讲》的习题部分，习题只做了一部分，如有错误，还请指出。习题中部分代码、图片引用自课本。

第3讲　三维刚体运动

习题1 证明旋转矩阵是正交矩阵

参考链接旋转矩阵(Rotate Matrix)的性质分析

习题6 在Eigen中实现解方程Ax=b

#define MATRIX_SIZE 50

Eigen::Matrix< double, MATRIX_SIZE, MATRIX_SIZE > A;
A = Eigen::MatrixXd::Random( MATRIX_SIZE, MATRIX_SIZE );
Eigen::Matrix< double, MATRIX_SIZE,  1> b;
b = Eigen::MatrixXd::Random( MATRIX_SIZE, 1 );

// 直接求逆
Eigen::Matrix<double, MATRIX_SIZE, 1> x = A.inverse() * b;

// 通常用矩阵分解来求，例如QR分解，速度会快很多
x = A.colPivHouseholderQr().solve(b);

第4讲 李群与李代数习题

习题1 $验证SO(3),SE(3),Sim(3)关于乘法成群$

$设R_1,R_2属于SO(3),有余SO(3)群是具有连续光滑性质的群，令R_3=R_2R_1$，对任意三维向量$V_1=[x,y,z]^T$,连续做如下式两次旋转，得到向量$V_2$

$$V_2=R_2R_1V_1=R_3V_1$$
当然 $V_2$也可以由 $V_1$经一次旋转得到，其中 $R_4$是属于 $SO(3)$群的
$$V_2=R_4V_1$$
注意到有下式成立
$$R_4=R_3$$
所以 $R_3$是属于 $SO(3)$群的，也就是 $R_2R_1$是属于 $SO(3)$群的,从而验证 $SO(3)$关于乘法成群。

$SE(3),Sim(3)关于乘法成群$验证同理。

习题2 验证$(R^3,R,×)构成李代数$

也就是验证三维向量集合见关于叉乘运算封闭。
设三维向量$V_1, V_2, V_3\in R^3 , [V_1, V_2]=V_1×V_2$

• 封闭性
  $[V_1, V_2]+b[V_2, V_3]=V_1×V_2\in R^3$
  符合条件
• 双线性
  $[aV_1+bV_2, V_3]=(aV_1+bV_2)×V_3=aV_1×V_3+bV_2×V_3=a[V_1, V_2]+b[V_2, V_3]$
  符合条件
• 自反性
  $[V_1, V_1]=V_1×V_1=0$
  符合条件
• 雅可比等价
  也就是证明
  $$[V_1, [V_2, V_3]]+[V_2, [V_3, V_1]]+[V_3, [V_1, V_2]]=0$$
  其中，
  $[V_1, [V_2, V_3]]=V_1×(V_2×V_3)$
  $[V_2, [V_3, V_1]]=V_2×(V_3×V_1)$
  $[V_3, [V_1, V_2]]=V_3×(V_1×V_2)$
  将三个向量用$i,j,k$三个分量展开，然后带入相加化简得到0。
  使用Matlab证明如下，

syms i1 j1 k1 i2 j2 k2 i3 j3 k3 v1 v2 v3 v4;

v1=[i1,j1,k1];
v2=[i2,j2,k2];
v3=[i3,j3,k3];
v4 = cross(v1,cross(v2,v3))+cross(v2,cross(v3,v1))+cross(v3,cross(v1,v2));
simplify(v4)

% 运行结果为 [ 0, 0, 0]

习题3 验证李代数$\begin{equation} \mathfrak{so}(3) = \{ \mathbf{\phi} \in \mathbb{R}^{3} \} \end{equation}$满足李代数要求的性质

使用Matlab编写符号函数来证明。
由向量生成反对称阵函数如下

function [ y ] = hat( x )
x = sym(x);
y = sym(zeros(3,3));
y(1,2)=-x(3);
y(1,3)=x(2);
y(2,1)=x(3);
y(2,3)=-x(1);
y(3,1)=-x(2);
y(3,2)=x(1);
end

由反对称阵生成向量的函数

function [ y ] = vee( r )
r = sym(r);
y = sym(zeros(1,3));
y(1) = r(3,2);
y(2) = r(1,3);
y(3) = r(2,1);
end

进行$\mathfrak{so}(3)$的李括号运算函数

function [ y ] = SO3_lie_bracket(x1,x2)
x1 = sym(x1);
x2 = sym(x2);

X1 = hat(x1);
X2 = hat(x2);

y = vee(X1*X2-X2*X1);
end

证明过程如下
(1) 定义三个向量如下

clear;clc;
syms i1 j1 k1 i2 j2 k2 i3 j3 k3;

x=[i1,j1,k1];
y=[i2,j2,k2];
z=[i3,j3,k3];

(2)证明双线性

%  双线性
syms a b;
left = SO3_lie_bracket(a*x+b*y, z);
right = a*SO3_lie_bracket(x,z)+b*SO3_lie_bracket(y,z);
simplify(left-right)% 结果为[ 0, 0, 0]，证明双线性成立

(3)证明自反性

% 自反性
simplify(SO3_lie_bracket(x,x))% 结果为[ 0, 0, 0]，证明自反性成立

(4)证明雅克比等价

% 雅克比等价
part1 = SO3_lie_bracket(x,SO3_lie_bracket(y,z));
part2 = SO3_lie_bracket(y,SO3_lie_bracket(z,x));
part3 = SO3_lie_bracket(z,SO3_lie_bracket(x,y));
simplify(part1+part2+part3) % 结果为[ 0, 0, 0]，证明雅克比等价成立

习题4 证明$a^{\wedge}a^{\wedge}=aa^T-I和a^{\wedge}a^{\wedge}a^{\wedge}=-a^{\wedge}$成立，其中$a$是长度为1的三维方向向量

(1) 证明$a^{\wedge}a^{\wedge}=aa^T-I$成立
只需证明$a^{\wedge}a^{\wedge}-aa^T-I=0_{3×3}$
利用Matlab化简

syms i1 j1 k1
x=[i1,j1,k1];

simplify(hat(x)*hat(x)-x'*x+sym(eye(3)))

化简结果是

[ 1 - j1^2 - k1^2 - abs(i1)^2, -(j1*(abs(i1)^2 - i1^2))/i1, -(k1*(abs(i1)^2 - i1^2))/i1]
[ -(i1*(abs(j1)^2 - j1^2))/j1, 1 - i1^2 - k1^2 - abs(j1)^2, -(k1*(abs(j1)^2 - j1^2))/j1]
[ -(i1*(abs(k1)^2 - k1^2))/k1, -(j1*(abs(k1)^2 - k1^2))/k1, 1 - i1^2 - j1^2 - abs(k1)^2]

由于$a$是长度为1的三维方向向量, 则有$i1^2+j1^2+k1^2=1$
最终为$0_{3×3}$，得证。

(2)证明$a^{\wedge}a^{\wedge}a^{\wedge}=-a^{\wedge}$
只需证明$a^{\wedge}a^{\wedge}a^{\wedge}+a^{\wedge}=0_{3×3}$成立
利用Matlab化简

syms i1 j1 k1
x=[i1,j1,k1];

simplify(hat(x)*hat(x)*hat(x)+hat(x))

化简结果是

[                            0,  k1*(i1^2 + j1^2 + k1^2 - 1), -j1*(i1^2 + j1^2 + k1^2 - 1)]
[ -k1*(i1^2 + j1^2 + k1^2 - 1),                            0,  i1*(i1^2 + j1^2 + k1^2 - 1)]
[  j1*(i1^2 + j1^2 + k1^2 - 1), -i1*(i1^2 + j1^2 + k1^2 - 1),                            0]

由于$a$是长度为1的三维方向向量, 则有$i1^2+j1^2+k1^2=1$
最终为$0_{3×3}$，得证。