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

2019/03/08 20:09
阅读数 406

1.设线性⽅程 Ax = b,在 A 为⽅阵的前提下,请回答以下问题:
1. 在什么条件下,x 有解且唯⼀?

非齐次线性方程在A的秩与[A|B]的秩相同时方程有解,当R(A)=R(A,B)=n时方程有唯一解。

2. ⾼斯消元法的原理是什么?

原理:高斯消元法的作用是又来求解线性方程组的解,其原理是将方程组进行加减消元,然后求出未知数X。

3. QR 分解的原理是什么?

 

原理:将一个稀疏矩阵分解成一个正交矩阵和一个上三角矩阵A=QR, A左乘一个Householder反射矩阵Hj, Hn...H2H1A=QTA=[R 0]T

4. Cholesky 分解的原理是什么?

 

与SLAM问题相关的优化问题,可以很简洁的用稀疏线性代数的方式表达,要么将信息矩阵分解为平方根的形式,要么将观测雅可比矩阵A分解为平方根

原理:乔利斯基分解通过求解正规方程,然后分解信息矩阵得到一种对称正定矩阵(LU分解的百变种)得到一个n*n的上三角形矩阵。A=L*LT

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;
        return 0;

}

 

 

展开阅读全文
打赏
0
0 收藏
分享
加载中
更多评论
打赏
0 评论
0 收藏
0
分享
返回顶部
顶部