深蓝学院-视觉SLAM理论与实践-第十二期-第2章作业

最新推荐文章于 2023-10-16 14:39:47 发布

小宋今天要早睡

最新推荐文章于 2023-10-16 14:39:47 发布

阅读量1.3k

点赞数 1

分类专栏： SLAM 文章标签：线性代数矩阵算法

本文链接：https://blog.csdn.net/weixin_40918019/article/details/123038666

版权

SLAM 专栏收录该内容

11 篇文章 10 订阅

订阅专栏

熟悉Eigen矩阵运算

设线性方程 $A x = b$ ，在 $A$ 为方阵的前提下，请回答以下问题：

问题1：什么条件下，x有解且唯一？

矩阵A非奇异，满秩。

问题2：高斯消元法的原理是什么？

高斯消元法主要可以分成三步：

从上至下，以第 $m (1 < = m < n)$ 行数据作为减项，将大于 $m$ 行的数据中的第 $m$ 列变成0，从而使得矩阵变成上三角矩阵
从下至上，以第 $m (1 < = m < n)$ 行数据作为减项，将小于 $m$ 行的数据中的第 $m$ 列变成0，从而使得矩阵变成对角矩阵
根据对角矩阵求解x

问题3：QR分解的原理是什么？

定义

一个矩阵 [外链图片转存失败,源站可能有防盗链机制,建议将图片保存下来直接上传(img-aKT6i2Zf-1645373936195)(https://www.zhihu.com/equation?tex=A+%5Cin+%5Cmathbb%7BR%7D%5E%7Bm%5Ctimes+n%7D%2C%5C+m%5Cge+n)] 可以被分解成 [外链图片转存失败,源站可能有防盗链机制,建议将图片保存下来直接上传(img-kh7NRC6t-1645373936196)(https://www.zhihu.com/equation?tex=A+%3D+QR)] ，其中：

[外链图片转存失败,源站可能有防盗链机制,建议将图片保存下来直接上传(img-laWYSFHd-1645373936198)(https://www.zhihu.com/equation?tex=Q%5Cin+%5Cmathbb%7BR%7D%5E%7Bm%5Ctimes+m%7D)] 是正交矩阵
[外链图片转存失败,源站可能有防盗链机制,建议将图片保存下来直接上传(img-fRvFCdf8-1645373936199)(https://www.zhihu.com/equation?tex=R+%5Cequiv++%5Cbegin%7Bbmatrix%7D+%5Chat%7BR%7D+%5C%5C+0+%5Cend%7Bbmatrix%7D+%5Cin+%5Cmathbb%7BR%7D%5E%7Bm%5Ctimes+n%7D)]
[外链图片转存失败,源站可能有防盗链机制,建议将图片保存下来直接上传(img-MuJV29qT-1645373936201)(https://www.zhihu.com/equation?tex=%5Chat%7BR%7D+%5Cin+%5Cmathbb%7BR%7D%5E%7Bn%5Ctimes+n%7D)] 是上三角矩阵

原理/自己的一些理解

如果我们把A矩阵看成是n个m维的列向量，那么QR分解实际上在是选择一组正交基底(矩阵Q)，使得变换后每一个列向量(矩阵R从左至右)所关联的纬度逐渐增高。这里如果一个向量在某个纬度上数据不是0，就认为他们是关联的。显然，在m>n的情况下，这样的基底是可以构建出来的。

问题4：Cholesky 分解的原理是什么？

定义

如果矩阵A是一个对称正定矩阵，那么 $A=LL^{T}$ ，其中L为一个上三角矩阵。

原理/自己的一些理解

由于矩阵A为一个对称正定矩阵，可以把A拆分成如下形式

$A=B^{T}B$

其中B为一个满秩矩阵，对B应用QR分解可得

$A=B^{T}B=(QR)^{T}(QR)=R^{T}Q^{T}QR = R^{T}R$

其中，R为上三角矩阵。

问题5：编程实现 A 为 100 × 100 随机矩阵时，用 QR 和 Cholesky 分解求 x 的程序。你可以参考本次课用到的 useEigen 例程

源代码

#include <iostream>
#include <Eigen/Core>
#include <Eigen/Dense>
#include <Eigen/QR>
#include <Eigen/Cholesky>

using namespace std;
int main() {
	Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic> A = Eigen::MatrixXd::Random(100,100);
	Eigen::Matrix<double, Eigen::Dynamic, 1>b = Eigen::MatrixXd::Random(100,1);
	A = A.transpose()*A;	//保证A是半正定，这样cholesky才有效
	Eigen::Matrix<double, Eigen::Dynamic, 1> x_QR = A.colPivHouseholderQr().solve(b);//QR分解的结果
	
	Eigen::Matrix<double, Eigen::Dynamic, 1> x_Cholesky = A.llt().solve(b);;//分解的结果
	
	std::cout << "QR result: " << x_QR.transpose() << std::endl;
	std::cout << "Cholesky result: " << x_Cholesky.transpose() << std::endl;
	std::cout << "QR ||Ax - b ||=  " << (A*x_QR - b).norm() << std::endl;
	std::cout << "Cholesky ||Ax - b ||=  " << (A*x_Cholesky - b).norm()  << std::endl;
    return 0;
}

运行结果

[外链图片转存失败,源站可能有防盗链机制,建议将图片保存下来直接上传(img-febudbTY-1645373936202)(https://pic.imgdb.cn/item/61fe44352ab3f51d918828a5.jpg)]

运行结果

矩阵论基础

问题1：什么是正定矩阵和半正定矩阵？

正定矩阵：对于任何 $x > 0$ 如果 $x^{T}Ax>0$ 恒成立，那么A为正定矩阵
半正定矩阵：对于任何 $\geq 0$ 如果 $x^{T}Ax\geq0$ 恒成立，那么A为半正定矩阵

问题2：对于方阵 A，它的特征值是什么？特征向量是什么？特征值一定是实数吗？如何计算一个矩阵的特征值？

方针A的特征值和特征向量

对于一个n阶的矩阵A，存在一个实数[外链图片转存失败,源站可能有防盗链机制,建议将图片保存下来直接上传(img-vmyTtsY4-1645373936203)(https://www.zhihu.com/equation?tex=%5Clambda)]，使得我们可以找到一个非零向量x，满足：

[外链图片转存失败,源站可能有防盗链机制,建议将图片保存下来直接上传(img-epRCezf8-1645373936203)(https://www.zhihu.com/equation?tex=Ax%3D%5Clambda+x+%5C%5C)]

如果能够找到的话，我们就称[外链图片转存失败,源站可能有防盗链机制,建议将图片保存下来直接上传(img-IoKoFaJD-1645373936204)(https://www.zhihu.com/equation?tex=%5Clambda)]是矩阵A的特征值，非零向量x是矩阵A的特征向量。

如何计算一个矩阵的特征值

对原式来进行一个很简单的变形：

[外链图片转存失败,源站可能有防盗链机制,建议将图片保存下来直接上传(img-4htLqvIj-1645373936206)(https://www.zhihu.com/equation?tex=%28A-%5Clambda+I%29x+%3D+0+%5C%5C)]

在x不全为0的情况下，如果使得上述公式成立，需要 $A-\lambda I$ 奇异，即选择一个合适的 $\lambda$ 使得行列式| $A-\lambda I$ |为0即可。由求解方法可见，矩阵的特征值不一定为实数，只有当矩阵是实矩阵时，特征值才为实数。

问题3：什么是矩阵的相似性？相似性反映了什么几何意义？

相似矩阵的定义：

对于矩阵 $A$ 和矩阵 $B$ ，如果存在一个満秩方阵S使得 $A = S^{-1}BS$ ，那么矩阵 $A$ 和矩阵 $B$ 相似

相似性反映了什么几何意义

线性变换若粗略看成一个刚体的特定运动。刚体的特定运动是同一个，但坐标系改变的话这个运动的描述函数就会不一样，如果这个函数可用矩阵等价替代的话，一个坐标系就对应着一个矩阵，因此这些矩阵就不同，但这些矩阵必有关系，这个关系就是相似。

问题4：矩阵一定可以对角化吗，什么样的矩阵能够保证对角化？不能对角化的矩阵能够形成什么样的形式(Jordan 标准型)？

矩阵不一定可以对角化，满秩矩阵才可以对角化，不能对角化的矩阵可以化简成Jordan 标准型，只有对角线及下方/上方相邻元素有数据。

问题5：奇异值分解(SVD)是什么意思？

对于任何一个矩阵 $A_{m*n}$ ,可以将他分解成 $A_{m*n} = U_{m*m}\sum_{m*n}V_{n*n}$ ,其中 $U 、 V$ 为正定矩阵， $\sum_{m*n}$ 为对角矩阵。

问题6：矩阵的伪逆是什么意思（Pseudo inverse）？莫尔——彭多斯逆是如何定义的？怎么计算一个矩阵的伪逆？

矩阵伪逆

当一个矩阵 $A_{m*n}$ 是奇异矩阵的时候，无法求的矩阵的逆解，但是可以按照矩阵的三种形式，求得以下三种伪逆

当 $m > n$ 时，可以求得一个左逆矩阵使得 $A^{L}A = I$ (存在多个)
当 $m < n$ 时，可以求得一个右逆矩阵使得 $AA^{R}=I$ (存在多个)
当 $m = n$ 且A为奇异矩阵时，可以根据SVD分解求得伪逆 $A = UDV^{T}$ ,则A的伪逆为 $A^+=VD^+U^T$

莫尔——彭多斯逆

如果矩阵 $B$ 满足以下四个条件，那么为们可以称 $B$ 为 $A$ 的莫尔——彭多斯逆

$B A B = B$
$A B A = A$
$AB)^H=AB$
$BA)^H=BA$

莫尔——彭多斯逆可以通过SVD分解的方法进行计算

问题7：超定方程求解相关

对于超定方程： $A x = b$ 且 $A$ 不可逆时，我们通常计算最小二乘解： $\operatorname{arg\,max}_a||Ax − b||$ 。线性方程的最小二乘解在代数意义上是可以解析地写出来的。请回答以下小问题：

在 $\neq 0$ 时， $x$ 的解是什么形式？

当b不为0时，解的形式为 $A^TA)^{-1}A^Tb$

事实上，我们可以对 $A$ 求奇异值或对于 $A^TA$ 求特征值。请阐述两者之间的关系

矩阵A的SVD分解为 $A = UDV^{T}$ ,其中D为对角矩阵(对角元素可能为0)，进而可以计算出 $A^TA=UDV^{T}VDU^T=UDDU^T$

矩阵 $A^TA$ 的特征值可以表示为 $A^TAM=M\sum$ 其中 $\sum$ 为特征值为对角线的对角矩阵，M为特征值对应的特征向量所组成的方阵，为单位正定矩阵。上式可进一步转化为 $A^TA=M\sum M^T$

由此可以发现，在形式上当A为一个方阵时， $\sum$ 。

当 $b = 0$ 时，我们希望求 $x$ 的非零解。请说明如何求解 $x$

$x$ 为 $A^TA$ 最小特征值对应的特征向量，因为此时 $x^{T} A^{T} Ax= \lambda _{min}$

请谈谈你对上述解法在几何意义上的理解。该问题为开放问题

设A为m行n列的一个矩阵，m>n。

理解1：可以理解为有m个在n+1维空间中的平面(传统三维空间中存在的为而二维平面，矩阵A的每一行就代表一个平面)，我们需要确定一个x，使得由x确定的每个平面上的点到原点的距离总和最小
理解2：矩阵A可以看作对向量x进行旋转->缩放->升纬/降纬->旋转的操作,为们需要确定一个最有的向量(x)，使得这个向量在进行完上述一系列操作之后，模最小。

几何运算练习

下面我们来练习如何使用 Eigen/Geometry 计算一个具体的例子。
一个机器人上通常会安装许多不同的传感器，而且这些传感器之间还存在固连关系。我们举一个典型的例子。
在世界系 W 下，存在一个运动的机器人 R。按照固定的或者某些开发人员或者领导的特殊喜好， R系定义在机器人脚部的位置。但是机器人在设计的时候，又定义了 B 系（Body 系，或本体系），位于机器人头部的位置。由于沟通不畅，标定人员把一台激光传感器和一台视觉传感器标定在了 B 系下。我们称激光传感器为 L 系，视觉传感器为 C 系。现在请你完成以下工作：

问题1：说明一个激光传感器下的看到的点应该如何计算它的世界坐标。

根据标定结果将观测数据位置转化到B系下
根据B系和R系之间的相对关系，把观测数据转化到R系下
根据机器人的位置和姿态信息，将观测数据转化到W系下

问题2：给定以下数据，计算观测点在激光坐标系和世界坐标系下的坐标

$ q_{WR}=[0.55, 0.3, 0.2, 0.2], t_{WR}=[0.1, 0.2, 0.3]^{T}, $

$q_{RB} = [0.99, 0, 0, 0.01], t_{RB} = [0.05, 0, 0.5]^{T}，$

$q_{BL} = [0.3, 0.5, 0, 20.1], t_{BL} = [0.4, 0, 0.5]^T ,$

$q_{BC} = [0.8, 0.2, 0.1, 0.1], t_{BC} = [0.5, 0.1, 0.5]^T$

$p_{C}=[0.3, 0.2,1.2]^{T}$

源代码

#include<Eigen/Core>
#include<Eigen/Geometry>
#include<iostream>

int main(int argv, char** argc)
{
    //初始化四元数
	Eigen::Quaterniond q_W2R(0.55, 0.3, 0.2, 0.2);	//这种方式初始化，实部在前
	Eigen::Quaterniond q_R2B(0.99, 0, 0, 0.01);
	Eigen::Quaterniond q_B2L(0.3, 0.5, 0, 20.1);
	Eigen::Quaterniond q_B2C(0.8, 0.2, 0.1, 0.1);
    
    //四元数归一化
    q_W2R.normalize();
	q_R2B.normalize();
	q_B2L.normalize();
	q_B2C.normalize();
    
    //初始化平移向量
	Eigen::Vector3d t_W2R(0.1, 0.2, 0.3);
	Eigen::Vector3d t_R2B(0.05, 0, 0.5);
	Eigen::Vector3d t_B2L(0.4, 0, 0.5);
	Eigen::Vector3d t_B2C(0.5, 0.1, 0.5);
	
    //相机坐标系下该点坐标
	Eigen::Vector3d p_C(0.3, 0.2,1.2);
    
    //四元数转化为旋转矩阵
	Eigen::Matrix3d R_W2R = q_W2R.matrix();
	Eigen::Matrix3d R_R2B = q_R2B.matrix();
	Eigen::Matrix3d R_B2L = q_B2L.matrix();
	Eigen::Matrix3d R_B2C = q_B2C.matrix();
	
    //计算激光坐标系下该点位置 C -> B -> L
	Eigen::Vector3d p_B = R_B2C*p_C + t_B2C;
	Eigen::Vector3d p_L = R_B2L.transpose()*p_B - R_B2L.transpose()*t_B2L;		//激光坐标系下该点位置
	
    //计算世界坐标系下该点位置 B -> R -> W
	Eigen::Vector3d p_R = R_R2B*p_B + t_R2B;
	Eigen::Vector3d p_W = R_W2R*p_R + t_W2R;		//世界坐标系下该点位置
	
	
	std::cout << "激光坐标系下该点位置:" << p_L.transpose() << std::endl;
	std::cout << "世界坐标系下该点位置" << p_W.transpose() << std::endl;
    
    return 0;
}

运行结果

[外链图片转存失败,源站可能有防盗链机制,建议将图片保存下来直接上传(img-1765vWza-1645373936207)(https://pic.imgdb.cn/item/61fd564b2ab3f51d91d11428.jpg)]

运行结果

旋转的表达

课程中提到了旋转可以用旋转矩阵、旋转向量与四元数表达，其中旋转矩阵与四元数是日常应用中常见的表达方式。请根据课件知识，完成下述内容的证明。

问题1：设有旋转矩阵 R，证明 $R^TR = I$ 且 $d e t R = 1$

由旋转矩阵的计算过程可知，当一个向量在通过R完成旋转时，实际上是用这个向量与R的每一个行向量进行一次点积。也就是说，R的每一个行向量代表旋转后坐标轴的方向，因此R的每一个行向量是一个单位向量且是相互正交的，因此 $R^TR = I$ 且 $d e t R = 1$

问题2：设有四元数 q，我们把虚部记为 ε，实部记为 η，那么 q = (ε, η)。请说明 ε 和 η 的维度。

四元数的实部为一维，虚部为三维，且整体的模为1

问题3：

罗德里格斯公式的证明

问题1：罗德里格斯公式证明

[外链图片转存失败,源站可能有防盗链机制,建议将图片保存下来直接上传(img-hJwAfyfd-1645373936207)(https://pic.imgdb.cn/item/61ff86b92ab3f51d91a3e1e6.png)]

上图中，向量V绕旋转轴K旋转 $\theta$ 到达 $V_{rot}$

证明过程如下:

向量 $V$ 可分解为两个向量： $V_{//}、V_{⊥}$ ,其中 $V_{//}$ 为向量 $V$ 向向量 $K$ 的投影，即 $V_{//}=(V^TK)K、V_⊥=V-V_{//}$

向量 $V$ 的旋转过程可进一步分解成 $V_{//}、V_{⊥}$ 的旋转，即 $V_{rot}=V_{rot_{⊥}}+V_{rot_{//}}$

由于 $V_{//}$ 和旋转轴平行，因此旋转前后保持不变，即 $V_{rot_{//}}=V_{//}$

$V_{⊥}$ 旋转后的向量可进一步分解成向量 $a$ 和向量 $b$ 如上图所示，其中 $cos(\theta)V_{⊥}、a=sin(\theta)K\times V$

综上，
$V_{rot}\\=V_{rot_{⊥}}+V_{rot_{//}} \\ = a + b + V_{//} \\ = sin(\theta)K\times V + cos(\theta)V_{⊥} + (V^TK)K \\ = sin(\theta)K\times V + cos(\theta)(V-(V^TK)K) + (V^TK)K \\ = sin(\theta)K\times V + cos(\theta)V + (1 - cos(\theta))(V^TK)K \\ = sin(\theta)K\times V + cos(\theta)V + (1 - cos(\theta))KK^{T}V \\ = (sin(\theta)K^{\Lambda} + cos(\theta)I + (1 - cos(\theta))KK^{T})V \\ = RV$

因此， $sin(\theta)K^{\Lambda} + cos(\theta)I + (1 - cos(\theta))KK^{T} = R$

问题2：试用此式证明 $R^T = R^{-1}$

由定义可知， $R^{-1}$ 对应的旋转轴与 $R$ 对应的旋转轴应该相同，只是 $\theta$ 相反

$R^T = -sin(\theta)K^{\Lambda} + cos(\theta)I + (1 - cos(\theta))KK^{T} = sin(-\theta)K^{\Lambda} + cos(\theta)I + (1 - cos(\theta))KK^{T}$

由上式可以看出， $R^T$ 与 $R$ 的形式相对比，确实也只有 $\theta$ 相反，因此 $R^T = R^{-1}$ ，证毕。

四元数运算性质的验证

问题1：验证旋转后的四元数为虚四元数

$p' = qpq^{-1}=q^+p^+q^{-1}=q^+p^{-1⊕}p$

参考链接

关注

1
点赞
踩
5

收藏

觉得还不错? 一键收藏
0
评论
深蓝学院-视觉SLAM理论与实践-第十二期-第2章作业

熟悉Eigen矩阵运算设线性方程 Ax=b Ax = b Ax=b，在AAA为方阵的前提下，请回答以下问题：问题1：什么条件下，x有解且唯一？矩阵A非奇异，满秩。问题2：高斯消元法的原理是什么？高斯消元法主要可以分成三步：从上至下，以第m(1<=m<n)m(1<=m<n)m(1<=m<n)行数据作为减项，将大于mmm行的数据中的第mmm列变成0，从而使得矩阵变成上三角矩阵从下至上，以第m(1<=m<n)m(1<=m<n)m(1&l
复制链接

扫一扫