《视觉SLAM十四讲课后作业》第二讲

《视觉SLAM⼗四讲课后作业》第⼆讲
1.设线性⽅程 Ax = b，在 A 为⽅阵的前提下，请回答以下问题：
1. 在什么条件下，x 有解且唯⼀？
⾮齐次线性⽅程在A的秩与[A|B]的秩相同时⽅程有解，当R(A)=R(A,B)=n时⽅程有唯⼀解。

2. ⾼斯消元法的原理是什么？
原理：⾼斯消元法的作⽤是⼜来求解线性⽅程组的解，其原理是将⽅程组进⾏加减消元，然后求出未知数X。

3. QR 分解的原理是什么？
原理：将⼀个稀疏矩阵分解成⼀个正交矩阵和⼀个上三⾓矩阵A=QR， A左乘⼀个Householder反射矩阵Hj, H n...H2H1A=Q T A=[R 0]T
4. Cholesky 分解的原理是什么？
与SLAM问题相关的优化问题，可以很简洁的⽤稀疏线性代数的⽅式表达，要么将信息矩阵分解为平⽅根的形式，要么将观测雅可⽐矩阵A 分解为平⽅根
原理：乔利斯基分解通过求解正规⽅程，然后分解信息矩阵得到⼀种对称正定矩阵（LU分解的百变种）得到⼀个n*n的上三⾓形矩阵。

A=L*L T
5. 编程实现 A 为 100 × 100 随机矩阵时，⽤ QR 和 Cholesky 分解求 x 的程序。

#include<iostream>
#include<Eigen/Core>
#include<Eigen/Geometry>
const int SIZE=100;
using namespace std;
using namespace Eigen;
int main()
{
//AX=B
Matrix<double, Dynamic, Dynamic>A;//建⽴⼀个动态矩阵
A=MatrixXd::Random(SIZE,SIZE);//建⽴⼀个100*100的随机矩阵
A=A.transpose()*A;//乔利斯基分解需要正定矩阵
Matrix<double, Dynamic,1>B;//建⽴⼀个动态矩阵B
B=MatrixXd::Random(SIZE,1);//B为100*1的随机矩阵
Matrix<double, Dynamic,1>X;//建⽴⼀个X
X=MatrixXd::Random(SIZE,1);//X为100*1的随机矩阵
X=A.llt().solve(B);//乔利斯基分解
cout<<"LLT result:\n"<<X<<endl;//显⽰结果
X=A.colPivHouseholderQr().solve(B);//QR分解
cout<<"QR result:\n"<<X<<endl;//显⽰结果
return 0;
}
2.设有⼩萝⼘1⼀号和⼩萝⼘⼆号位于世界坐标系中。

⼩萝⼘⼀号的位姿为：q1 = [0.55, 0.3, 0.2, 0.2], t1 =
[0.7, 1.1, 0.2]T（q 的第⼀项为实部）。

这⾥的 q 和 t 表达的是 Tcw，也就是世界到相机的变换关系。

⼩萝⼘
⼆号的位姿为 q2 = [−0.1, 0.3, −0.7, 0.2], t2 = [−0.1, 0.4, 0.8]T。

现在，⼩萝⼘⼀号看到某个点在⾃⾝的坐
标系下，坐标为 p1 = [0.5, −0.1, 0.2]T，求该向量在⼩萝⼘⼆号坐标系下的坐标。

请编程实现此事，并提交
你的程序。

#include<iostream>
#include<Eigen/Core>
#include<Eigen/Geometry>
using namespace std;
using namespace Eigen;
int main()
{
Quaterniond q1(0.55,0.3,0.2,0.2);//定义四元数Q1
Quaterniond q2(-0.1,0.3,-0.7,0.2);
q1=q1.normalized();//四元素归⼀化
q2=q2.normalized();
Matrix<double,3,1> t1;//定义平移矩阵
Matrix<double,3,1> t2;
Matrix<double,3,1> p1;
Matrix<double,3,1> p2;
t1<<0.7,1.1,0.2;
t2<<-0.1,0.4,0.8;
p1<<0.5,-0.1,0.2;//Pcw相对于Tcw的位姿
Isometry3d Tcw1=Isometry3d::Identity();//创建欧式群
Tcw1.rotate(q1.toRotationMatrix());//四元数转化为旋转矩阵
Tcw1.pretranslate(t1);//四元数转化为平移矩阵
p1=Tcw1.inverse()*p1;//P1=Tcw1*Pw ==>Pw=P1*inverse(Tcw1) Isometry3d Tcw2=Isometry3d::Identity();//创建Tcw2的欧式群
Tcw2.rotate(q2.toRotationMatrix());
Tcw2.pretranslate(t2);
p2=Tcw2*p1;//P2=Tcw2*Pw(Pw=P1*inverse(Tcw1))
cout<<p2<<endl;
return0;
}。