视觉slam14讲——习题部分

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

• 第3讲　三维刚体运动
• 第4讲 李群与李代数习题

第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)关于乘法成群

设R1,R2属于SO(3),有余SO(3)群是具有连续光滑性质的群，令R3=R2R1，对任意三维向量V1=[x,y,z]T,连续做如下式两次旋转，得到向量V2

V2=R2R1V1=R3V1

当然V2也可以由V1经一次旋转得到，其中R4是属于SO(3)群的

V2=R4V1

注意到有下式成立

R4=R3

所以R3是属于SO(3)群的，也就是R2R1是属于SO(3)群的,从而验证SO(3)关于乘法成群。

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

习题2 验证(R3,R,×)构成李代数

也就是验证三维向量集合见关于叉乘运算封闭。
设三维向量V1,V2,V3∈R3,[V1,V2]=V1×V2

• 封闭性
  [V1,V2]+b[V2,V3]=V1×V2∈R3
  符合条件
• 双线性
  [aV1+bV2,V3]=(aV1+bV2)×V3=aV1×V3+bV2×V3=a[V1,V2]+b[V2,V3]
  符合条件
• 自反性
  [V1,V1]=V1×V1=0
  符合条件
• 雅可比等价
  也就是证明
  
  [V1,[V2,V3]]+[V2,[V3,V1]]+[V3,[V1,V2]]=0
  
  其中，
  [V1,[V2,V3]]=V1×(V2×V3)
  [V2,[V3,V1]]=V2×(V3×V1)
  [V3,[V1,V2]]=V3×(V1×V2)
  将三个向量用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 验证李代数so(3)={ϕ∈R3}满足李代数要求的性质

使用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

进行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∧a∧=aaT−I和a∧a∧a∧=−a∧成立，其中a是长度为1的三维方向向量

(1) 证明a∧a∧=aaT−I成立
只需证明a∧a∧−aaT−I=03×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的三维方向向量, 则有i12+j12+k12=1
最终为03×3，得证。

(2)证明a∧a∧a∧=−a∧
只需证明a∧a∧a∧+a∧=03×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的三维方向向量, 则有i12+j12+k12=1
最终为03×3，得证。