《视觉SLAM十四讲 第二版》笔记及课后习题（第三讲）
读书笔记：三维空间刚体运动
本讲介绍视觉 SLAM 的基本问题之一：一个刚体在三维空间中的运动是如何描述的。我们当然知道这由一次旋转加一次平移组成。平移确实没有太大问题，但旋转的处理是件麻烦事。我们将介绍旋转矩阵、四元数、欧拉角的意义,以及它们是如何运算和转换的。
旋转矩阵
一些线性代数的基本知识。
坐标系：
坐标变化：
变换矩阵：
这是一个数学技巧:我们把一个三维向量的末尾添加 1,变成了四维向量,称为齐次坐标。对于这个四维向量,我们可以把旋转和平移写在一个矩阵里面，使得整个关系变成了线性关系。该式中，矩阵 T 称为变换矩阵(Transform Matrix)。我们暂时用 ã 表示 a 的齐次坐标。
关于变换矩阵 T ,它具有比较特别的结构:左上角为旋转矩阵,右侧为平移向量,左下角为 0 向量,右下角为 1。这种矩阵又称为特殊欧氏群(Special Euclidean Group):
旋转向量和欧拉角
任意旋转都可以用一个旋转轴和一个旋转角来刻画。于是,我们可以使用一个向量,其方向与旋转轴一致,而长度等于旋转角。这种向量,称为旋转向量(或轴角, Axis-Angle)。这种表示法只需一个三维向量即可描述旋转。同样,对于变换矩阵,我们使用一个旋转向量和一个平移向量即可表达一次变换。这时的维数正好是六维。由旋转向量到旋转矩阵的过程由罗德里格斯公式(Rodrigues’s Formula )表明，由于推导过程比较复杂,我们不作描述，只给出转换的结果：
符号∧是向量到反对称的转换符，反之,我们也可以计算从一个旋转矩阵到旋转向量的转换。对于转角 θ，有：
关于转轴 n,由于旋转轴上的向量在旋转后不发生改变,说明
因此,转轴 n 是矩阵 R 特征值 1 对应的特征向量。求解此方程,再归一化,就得到了旋转轴。读者也可以从“旋转轴经过旋转之后不变”的几何角度看待这个方程。仍然剧透几句,这里的两个转换公式在下一章仍将出现,你会发现它们正是 SO(3) 上李群与李代数的对应关系。
欧拉角
ZY X 转角相当于把任意旋转分解成以下三个轴上的转角：
- 绕物体的 Z 轴旋转,得到偏航角 yaw；
- 绕旋转之后的 Y 轴旋转,得到俯仰角 pitch；
- 绕旋转之后的 X 轴旋转,得到滚转角 roll。
此时,我们可以使用 [r, p, y] T 这样一个三维的向量描述任意旋转。这个向量十分的直观,我们可以从这个向量想象出旋转的过程。其他的欧拉角亦是通过这种方式,把旋转分解到三个轴上,得到一个三维的向量,只不过选用的轴,以及选用的顺序不一样。这里介绍的 rpy 角是比较常用的一种,只有很少的欧拉角种类会有 rpy 那样脍炙人口的名字。
欧拉角的一个重大缺点是会碰到著名的万向锁问题(Gimbal Lock)：
有关万向锁的更多详解
四元数
四元数是 Hamilton 找到的一种扩展的复数.。它既是紧凑的，也没有奇异性。如果说缺点的话，四
元数不够直观，其运算稍为复杂一些。有关四元数的部分，推荐直接看3Blue1Brown大神的四元数的可视化。我们只要掌握四元数的基本概念以及四元数到旋转矩阵的转换就行了，我们省略过程中的推导,直接给出四元数到旋转矩阵的转换方式。
实践部分
这一章要使用Eigen库，建议使用使用最新的Eigen库，因为下一章的带模板类的sophus应用需要新版本的eigen（3.3版本以上）支持，有关eigen的安装参考这篇博客。
此外，需要安装pangolin，以下是我在安装时候的错误：
用第一版的slambook的pangolin编译安装通过。
另一种解决方案是：去pangplin的官方github下载，然后按照相应步骤编译即可：
（可能是百度网盘的里的文件是之前的版本导致的吧！真是坑爹！）
useEigen：
代码没有问题，运行结果如下：
/home/wh/shenlan/slambook2/ch3/useEigen/cmake-build-debug/eigenMatrix
matrix 2x3 from 1 to 6:
1 2 3
4 5 6
print matrix 2x3:
1 2 3
4 5 6
[1,2,3;4,5,6]*[3,2,1]=10 28
[1,2,3;4,5,6]*[4,5,6]: 32 77
random matrix: 0.680375 0.59688 -0.329554
-0.211234 0.823295 0.5364590.566198 -0.604897 -0.444451
transpose: 0.680375 -0.211234 0.5661980.59688 0.823295 -0.604897
-0.329554 0.536459 -0.444451
sum: 1.61307
trace: 1.05922
times 10: 6.80375 5.9688 -3.29554
-2.11234 8.23295 5.364595.66198 -6.04897 -4.44451
inverse:
-0.198521 2.22739 2.83571.00605 -0.555135 -1.41603-1.62213 3.59308 3.28973
det: 0.208598
Eigen values =
0.02428990.9921541.80558
Eigen vectors =
-0.549013 -0.735943 0.3961980.253452 -0.598296 -0.760134
-0.796459 0.316906 -0.514998
time of normal inverse is 0.089ms
x = -55.7896 -298.793 130.113 -388.455 -159.312 160.654 -40.0416 -193.561 155.844 181.144 185.125 -62.7786 19.8333 -30.8772 -200.746 55.8385 -206.604 26.3559 -14.6789 122.719 -221.449 26.233 -318.95 -78.6931 50.1446 87.1986 -194.922 132.319 -171.78 -4.19736 11.876 -171.779 48.3047 84.1812 -104.958 -47.2103 -57.4502 -48.9477 -19.4237 28.9419 111.421 92.1237 -288.248 -23.3478 -275.22 -292.062 -92.698 5.96847 -93.6244 109.734
time of Qr decomposition is 0.054ms
x = -55.7896 -298.793 130.113 -388.455 -159.312 160.654 -40.0416 -193.561 155.844 181.144 185.125 -62.7786 19.8333 -30.8772 -200.746 55.8385 -206.604 26.3559 -14.6789 122.719 -221.449 26.233 -318.95 -78.6931 50.1446 87.1986 -194.922 132.319 -171.78 -4.19736 11.876 -171.779 48.3047 84.1812 -104.958 -47.2103 -57.4502 -48.9477 -19.4237 28.9419 111.421 92.1237 -288.248 -23.3478 -275.22 -292.062 -92.698 5.96847 -93.6244 109.734
time of ldlt decomposition is 0.023ms
x = -55.7896 -298.793 130.113 -388.455 -159.312 160.654 -40.0416 -193.561 155.844 181.144 185.125 -62.7786 19.8333 -30.8772 -200.746 55.8385 -206.604 26.3559 -14.6789 122.719 -221.449 26.233 -318.95 -78.6931 50.1446 87.1986 -194.922 132.319 -171.78 -4.19736 11.876 -171.779 48.3047 84.1812 -104.958 -47.2103 -57.4502 -48.9477 -19.4237 28.9419 111.421 92.1237 -288.248 -23.3478 -275.22 -292.062 -92.698 5.96847 -93.6244 109.734Process finished with exit code 0
useGeometry：
运行结果如下：
/home/wh/shenlan/slambook2/ch3/useGeometry/cmake-build-debug/eigenGeometry
rotation matrix =0.707 -0.707 00.707 0.707 00 0 1
(1,0,0) after rotation (by angle axis) = 0.707 0.707 0
(1,0,0) after rotation (by matrix) = 0.707 0.707 0
yaw pitch roll = 0.785 -0 0
Transform matrix = 0.707 -0.707 0 10.707 0.707 0 30 0 1 40 0 0 1
v tranformed = 1.71 3.71 4
quaternion from rotation vector = 0 0 0.383 0.924
quaternion from rotation matrix = 0 0 0.383 0.924
(1,0,0) after rotation = 0.707 0.707 0
should be equal to 0.707 0.707 0 0Process finished with exit code 0
visualizeGeometry：
这个需要Pangolin的支持，在3rdparty文件夹中去编译安装即可。值得一提的是，我用git clone命令下载的时候，3rdparty文件夹是空的，只得又去相应的github上安装下载编译，真是有够麻烦的。所以也建议直接下载github首页的百度网盘的压缩包，那里面的都是全的。
编译后可以显示3D窗口中相机的位姿：
examples轨迹显示：
这里出现了警告：
/opt/clion-2019.2.5/bin/cmake/linux/bin/cmake -DCMAKE_BUILD_TYPE=Debug -G "CodeBlocks - Unix Makefiles" /home/wh/shenlan/slambook2/ch3/examples
CMake Warning (dev) in CMakeLists.txt:No project() command is present. The top-level CMakeLists.txt file mustcontain a literal, direct call to the project() command. Add a line ofcode such asproject(ProjectName)near the top of the file, but after cmake_minimum_required().CMake is pretending there is a "project(Project)" command on the firstline.
This warning is for project developers. Use -Wno-dev to suppress it.-- The C compiler identification is GNU 5.4.0
-- The CXX compiler identification is GNU 5.4.0
-- Check for working C compiler: /usr/bin/cc
-- Check for working C compiler: /usr/bin/cc -- works
-- Detecting C compiler ABI info
-- Detecting C compiler ABI info - done
-- Detecting C compile features
-- Detecting C compile features - done
-- Check for working CXX compiler: /usr/bin/c++
-- Check for working CXX compiler: /usr/bin/c++ -- works
-- Detecting CXX compiler ABI info
-- Detecting CXX compiler ABI info - done
-- Detecting CXX compile features
-- Detecting CXX compile features - done
CMake Warning (dev) in CMakeLists.txt:No cmake_minimum_required command is present. A line of code such ascmake_minimum_required(VERSION 3.15)should be added at the top of the file. The version specified may be lowerif you wish to support older CMake versions for this project. For moreinformation run "cmake --help-policy CMP0000".
This warning is for project developers. Use -Wno-dev to suppress it.-- Configuring done
CMake Warning (dev) at CMakeLists.txt:6 (add_executable):Policy CMP0003 should be set before this line. Add code such asif(COMMAND cmake_policy)cmake_policy(SET CMP0003 NEW)endif(COMMAND cmake_policy)as early as possible but after the most recent call tocmake_minimum_required or cmake_policy(VERSION). This warning appearsbecause target "plotTrajectory" links to some libraries for which thelinker must search:EGL, xkbcommon, rt, pthreadand other libraries with known full path:/usr/local/lib/libpangolin.soCMake is adding directories in the second list to the linker search path incase they are needed to find libraries from the first list (for backwardscompatibility with CMake 2.4). Set policy CMP0003 to OLD or NEW to enableor disable this behavior explicitly. Run "cmake --help-policy CMP0003" formore information.
This warning is for project developers. Use -Wno-dev to suppress it.-- Generating done
-- Build files have been written to: /home/wh/shenlan/slambook2/ch3/examples/cmake-build-debug[Finished]
先直接忽略，编译看看，首先是coordinateTransform ，没有问题：
接着是plotTrajectory，出现了错误：
error: #error This file requires compiler and library support for the ISO C++ 2011 standard. This support must be enabled with the -std=c++11 or -std=gnu++11 compiler options.#error This file requires compiler and library support \
可以看出是没有设置c++11的标准导致的，所以在CMakeLists.txt中加入：
set(CMAKE_CXX_FLAGS "-std=c++11 -O2")
再进行编译。可以看见编译通过：
但是，在执行的时候出现问题，cannot find trajectory file at ./examples/trajectory.txt：
这是因为路径设置的不对，直接查看trajectory.txt文件属性：
将路径修改为以上路径：
string trajectory_file = "/home/wh/shenlan/slambook2/ch3/examples/trajectory.txt";
然后再进行编译执行即可：
方法二：
需要配置文件路径，也就是ch3目录，如下：
运行后也可得到结果：
课后习题
1. 验证旋转矩阵是正交矩阵。
直接看这一篇的证明
2. * 寻找罗德里格斯公式的推导过程并理解它。
翻译自维基百科
假设v是R3中的一个向量，并且k是一个描述旋转轴的单位向量，v根据右手规则旋转角度θ，则Rodrigues公式为：
另一种说法是将轴向量作为定义旋转平面的任意两个非零向量a和b的交叉乘积a×b，并且角度θ的方向是从a向b测量的。令α表示这些向量之间的角度，两个角度θ和α不需要等同，但是它们的测量方式相同。 然后可以单位轴向量可以写为
当涉及定义平面的两个向量时，这种形式可能更有用。物理学中的一个例子是托马斯岁差，它包括由罗德里格斯公式给出的旋转，用两个非共线助推速度表示，旋转轴垂直于它们的平面。
求导
令k是定义旋转轴的单位矢量，令v是以角度θ围绕k旋转的任意向量（右手定则，图中逆时针）。
使用点乘和叉乘，向量v可以分解成与轴k平行和垂直的分量，
与k平行的分量是
称为v在k上的向量投影，垂直于k的分量为
称为k从v的向量拒绝。
矢量k×v可以看作是v⊥逆时针旋转90°左右的副本，因为它们的大小相等，但是方向是垂直的。同样，向量k×(k×v），v⊥的一个副本逆时针旋转180°到k左右，使得k×（k×v）和v⊥的大小相等，但方向相反（即它们是互为负数，因此减号）。扩展矢量三重乘积建立了平行分量和垂直分量之间的连接，参考公式为a×（b×c）=（a·c）b - （a·b）c 给定任意三个向量a，b，c。
平行于轴的分量在旋转下不会改变幅度和方向，
根据以上分析，只有垂直分量才会改变方向，但保持其大小
并且由于k和v||是平行的，所以它们的叉积是零 k×v|| = 0，因此
并且
这种旋转是正确的，因为矢量v⊥和k×v具有相同的长度，并且k×v是v⊥围绕k逆时针旋转90°。使用三角函数正弦和余弦对v⊥和k×v进行适当的缩放给出旋转的垂直分量。旋转分量的形式类似于笛卡尔基的2D平面极坐标（r，θ）中的径向向量
其中ex，ey是它们指示方向上的单位向量。
现在完整的旋转矢量是
用等式结果中的v ||rot和v⊥rot的定义代替
罗德里格斯（Rodrigues）的旋转公式通过将v分解为与k平行且垂直的分量，并仅旋转垂直分量，使v围绕矢量k旋转角度θ。
Rodrigues旋转公式的矢量几何，以及分解为平行和垂直分量。
矩阵表示
将v和k×v表示为列矩阵，叉积可以表示为矩阵乘积
令K表示单位向量k的“叉积矩阵
矩阵方程可以表示为
对于任何向量v（实际上，K是具有这个性质的独特矩阵，它具有特征值0和±i）。
迭代右边的交叉乘积相当于乘以左边的交叉乘积矩阵，特别是
而且，由于k是单位向量，所以K具有单位2-范数。 因此矩阵语言中的上一个旋转公式是
请注意，在这个表示法中，主要术语的系数现在是1。
将v分解可以实现紧凑表达式
这里
是围绕轴k逆时针旋转角度θ的旋转矩阵，I是3×3单位矩阵。矩阵R是旋转群SO（3）的一个元素，K是李代数的一个元素，所以so(3)生成李群（注意K是负对称的，这表征了so(3)的特性）。 就矩阵指数而言，
为了理解最后的恒等式，要注意到
单参数子群的特征，即指数，并且公式与无穷小θ相匹配。
对于基于这种指数关系的替代推导，请参阅从so（3）到SO（3）的指数映射。 对于逆映射，请参见从SO（3）到so（3）的日志映射。
3. 验证四元数旋转某个点后,结果是一个虚四元数(实部为零),所以仍然对应到一个三维空间点(式 3.34)
参见这篇博客
4. 画表总结旋转矩阵、轴角、欧拉角、四元数的转换关系。
5. 假设我有一个大的 Eigen 矩阵,我想把它的左上角 3 × 3 的块取出来,然后赋值为I3×3 。请编程实现此事。
提取大矩阵左上角3x3矩阵，有两种方式:
1、直接从0-2循环遍历大矩阵的前三行和三列
2、用矩阵变量.block(0,0,3,3)//从左上角00位置开始取3行3列
具体代码实现:
#include<iostream>//包含Eigen头文件
#include<Eigen/Core>
#include<Eigen/Geometry>#define MATRIX_SIZE 30
using namespace std;int main(int argc,char **argv)
{//设置输出小数点后3位cout.precision(3);Eigen::Matrix<double,MATRIX_SIZE, MATRIX_SIZE> matrix_NN = Eigen::MatrixXd::Random(MATRIX_SIZE,MATRIX_SIZE);Eigen::Matrix<double,3,3>matrix_3d1 = Eigen::MatrixXd::Random(3,3);//3x3矩阵变量Eigen::Matrix3d matrix_3d = Eigen::Matrix3d::Random();//两种方式都可以
/*方法1：循环遍历矩阵的三行三列 */for(int i = 0;i < 3; i ++){for(int j = 0 ;j < 3;j++){matrix_3d(i,j) = matrix_NN(i,j);cout<<matrix_NN(i,j)<<" ";}cout<<endl;}matrix_3d = Eigen::Matrix3d::Identity();cout<<"赋值后的矩阵为:"<<matrix_3d<<endl;/*方法2：用.block函数 */
/*cout<<"提取出来的矩阵块为:"<<endl;cout<< matrix_NN.block(0,0,3,3) <<endl;//提取后赋值为新的元素matrix_3d = matrix_NN.block(0,0,3,3);matrix_3d = Eigen::Matrix3d::Identity();cout<<"赋值后的矩阵为:"<<endl<<matrix_3d;
*/return 0;
}
6. * 一般线程方程 Ax = b 有哪几种做法?你能在 Eigen 中实现吗?
转自这位优秀的同学！用了6种方法，太秀了！
线性方程组Ax = b的解法 :
1、直接法：(1,2,3,4,5）
2、迭代法:如Jacobi迭代法(6)
其中只有2 3方法不要求方程组个数与变量个数相等
下面简略说明下Jacobi迭代算法:
由迭代法求解线性方程组的基本思想是将联立方程组的求解归结为重复计算一组彼此独立的线性表达式，这就使问题得到了简化，类似简单迭代法转换方程组中每个方程式可得到雅可比迭代式
迭代法求解方程组有一定的局限性，比如下面Jacobi函数程序实现的过程中，会判断迭代矩阵的谱半径是不是小于1，如果小于1表示Jacobi迭代法收敛，如果求不出来迭代矩阵即D矩阵不可逆的话，无法通过收敛的充要条件来判断是不是收敛，此时可以试着迭代多次，看看输出结果是否收敛，此时Jacobi迭代算法并不一定收敛，只能试着做下，下面的程序实现过程仅仅处理了充要条件，迭代法同时有十分明显的优点——算法简单，因而编制程序比较容易，所以在实际求解问题中仍有非常大利用价值。
具体代码实现 如下：
#include<iostream>
#include<ctime>
#include <cmath>
#include <complex>
/*线性方程组Ax = b的解法(直接法（1,2,3,4,5）+迭代法(6))其中只有2 3方法不要求方程组个数与变量个数相等*///包含Eigen头文件
//#include <Eigen/Dense>
#include<Eigen/Core>
#include<Eigen/Geometry>
#include <Eigen/Eigenvalues>//下面这两个宏的数值一样的时候 方法1 4 5 6才能正常工作
#define MATRIX_SIZE 3 //方程组个数
#define MATRIX_SIZE_ 3 //变量个数
//using namespace std;
typedef Eigen::Matrix<double,MATRIX_SIZE, MATRIX_SIZE_> Mat_A;
typedef Eigen::Matrix<double ,MATRIX_SIZE,1 > Mat_B;//Jacobi迭代法的一步求和计算
double Jacobi_sum(Mat_A &A,Mat_B &x_k,int i);//迭代不收敛的话 解向量是0
Mat_B Jacobi(Mat_A &A,Mat_B &b, int &iteration_num, double &accuracy );int main(int argc,char **argv)
{//设置输出小数点后3位std::cout.precision(3);//设置变量Eigen::Matrix<double,MATRIX_SIZE, MATRIX_SIZE_> matrix_NN = Eigen::MatrixXd::Random(MATRIX_SIZE,MATRIX_SIZE_);Eigen::Matrix<double ,MATRIX_SIZE,1 > v_Nd = Eigen::MatrixXd::Random(MATRIX_SIZE,1);//测试用例matrix_NN << 10,3,1,2,-10,3,1,3,10;v_Nd <<14,-5,14;//设置解变量Eigen::Matrix<double,MATRIX_SIZE_,1>x;//时间变量clock_t tim_stt = clock();/*1、求逆法 很可能没有解 仅仅针对方阵才能计算*/
#if (MATRIX_SIZE == MATRIX_SIZE_)x = matrix_NN.inverse() * v_Nd;std::cout<<"直接法所用时间和解为："<< 1000*(clock() - tim_stt)/(double)CLOCKS_PER_SEC<<"MS"<< std::endl << x.transpose() << std::endl;
#elsestd::cout<<"直接法不能解!(提示:直接法中方程组的个数必须与变量个数相同，需要设置MATRIX_SIZE == MATRIX_SIZE_)"<<std::endl;
#endif/*2、QR分解解方程组 适合非方阵和方阵 当方程组有解时的出的是真解，若方程组无解得出的是近似解*/tim_stt = clock();x = matrix_NN.colPivHouseholderQr().solve(v_Nd);std::cout<<"QR分解所用时间和解为："<<1000*(clock() - tim_stt)/(double)CLOCKS_PER_SEC<< "MS" << std::endl << x.transpose() << std::endl;/*3、最小二乘法 适合非方阵和方阵，方程组有解时得出真解，否则是最小二乘解(在求解过程中可以用QR分解 分解最小二成的系数矩阵) */tim_stt = clock();x = (matrix_NN.transpose() * matrix_NN ).inverse() * (matrix_NN.transpose() * v_Nd);std::cout<<"最小二乘法所用时间和解为:"<< 1000*(clock() - tim_stt)/(double)CLOCKS_PER_SEC<< "MS" << std::endl << x.transpose() << std::endl;/*4、LU分解方法 只能为方阵（满足分解的条件才行） */
#if (MATRIX_SIZE == MATRIX_SIZE_)tim_stt = clock();x = matrix_NN.lu().solve(v_Nd);std::cout<< "LU分解方法所用时间和解为:" << 1000*(clock() - tim_stt)/(double)CLOCKS_PER_SEC<< "MS" << std::endl << x.transpose() << std::endl;
#elsestd::cout<<"LU分解法不能解!(提示:直接法中方程组的个数必须与变量个数相同，需要设置MATRIX_SIZE == MATRIX_SIZE_)"<<std::endl;
#endif/*5、Cholesky 分解方法 只能为方阵 (结果与其他的方法差好多)*/
#if (MATRIX_SIZE == MATRIX_SIZE_)tim_stt = clock();x = matrix_NN.llt().solve(v_Nd);std::cout<< "Cholesky 分解方法所用时间和解为:" << 1000*(clock() - tim_stt)/(double)CLOCKS_PER_SEC<< "MS"<< std::endl<< x.transpose()<<std::endl;
#elsestd::cout<< "Cholesky法不能解!(提示:直接法中方程组的个数必须与变量个数相同，需要设置MATRIX_SIZE == MATRIX_SIZE_)"<<std::endl;
#endif/*6、Jacobi迭代法 */
#if (MATRIX_SIZE == MATRIX_SIZE_)int Iteration_num = 10 ;double Accuracy =0.01;tim_stt = clock();x= Jacobi(matrix_NN,v_Nd,Iteration_num,Accuracy);std::cout<< "Jacobi 迭代法所用时间和解为:" << 1000*(clock() - tim_stt)/(double)CLOCKS_PER_SEC<< "MS"<< std::endl<< x.transpose()<<std::endl;
#elsestd::cout<<"LU分解法不能解!(提示:直接法中方程组的个数必须与变量个数相同，需要设置MATRIX_SIZE == MATRIX_SIZE_)"<<std::endl;
#endifreturn 0;
}//迭代不收敛的话 解向量是0
Mat_B Jacobi(Mat_A &A,Mat_B &b, int &iteration_num, double &accuracy ) {Mat_B x_k = Eigen::MatrixXd::Zero(MATRIX_SIZE_,1);//迭代的初始值Mat_B x_k1; //迭代一次的解向量int k,i; //i,k是迭代算法的循环次数的临时变量double temp; //每迭代一次解向量的每一维变化的模值double R=0; //迭代一次后，解向量每一维变化的模的最大值int isFlag = 0; //迭代要求的次数后，是否满足精度要求//判断Jacobi是否收敛Mat_A D; //D矩阵Mat_A L_U; //L+UMat_A temp2 = A; //临时矩阵获得A矩阵除去对角线后的矩阵Mat_A B; //Jacobi算法的迭代矩阵Eigen::MatrixXcd EV;//获取矩阵特征值double maxev=0.0; //最大模的特征值int flag = 0; //判断迭代算法是否收敛的标志 1表示Jacobi算法不一定能收敛到真值std::cout<<std::endl<<"欢迎进入Jacobi迭代算法！"<<std::endl;//對A矩陣進行分解 求取迭代矩陣 再次求取譜半徑 判斷Jacobi迭代算法是否收斂for(int l=0 ;l < MATRIX_SIZE;l++){D(l,l) = A(l,l);temp2(l,l) = 0;if(D(l,l) == 0){std::cout<<"迭代矩阵不可求"<<std::endl;flag =1;break;}}L_U = -temp2;B = D.inverse()*L_U;//求取特征值Eigen::EigenSolver<Mat_A>es(B);EV = es.eigenvalues();
// cout<<"迭代矩阵特征值为:"<<EV << endl;//求取矩陣的特征值 然後獲取模最大的特徵值 即爲譜半徑for(int index = 0;index< MATRIX_SIZE;index++){maxev = ( maxev > __complex_abs(EV(index)) )?maxev:(__complex_abs(EV(index)));}std::cout<< "Jacobi迭代矩阵的谱半径为："<< maxev<<std::endl;//谱半径大于1 迭代法则发散if(maxev >= 1){std::cout<<"Jacobi迭代算法不收敛！"<<std::endl;flag =1;}//迭代法收敛则进行迭代的计算if (flag == 0 ){std::cout<<"Jacobi迭代算法谱半径小于1,该算法收敛"<<std::endl;std::cout<<"Jacobi迭代法迭代次数和精度： "<< std::endl << iteration_num<<" "<<accuracy<<std::endl;//迭代计算for( k = 0 ;k < iteration_num ; k++ ){for(i = 0;i< MATRIX_SIZE_ ; i++){x_k1(i) = x_k(i) + ( b(i) - Jacobi_sum(A,x_k,i) )/A(i,i);temp = fabs( x_k1(i) - x_k(i) );if( fabs( x_k1(i) - x_k(i) ) > R )R = temp;}//判断进度是否达到精度要求 达到进度要求后 自动退出if( R < accuracy ){std::cout <<"Jacobi迭代算法迭代"<< k << "次达到精度要求."<< std::endl;isFlag = 1;break;}//清零R，交换迭代解R = 0;x_k = x_k1;}if( !isFlag )std::cout << std::endl <<"迭代"<<iteration_num<<"次后仍然未达到精度要求，若不满意该解，请再次运行加大循环次数！"<< std::endl;return x_k1;}//否则返回0return x_k;
}//Jacobi迭代法的一步求和计算
double Jacobi_sum(Mat_A &A,Mat_B &x_k,int i) {double sum;for(int j = 0; j< MATRIX_SIZE_;j++){sum += A(i,j)*x_k(j);}return sum;
}
《视觉SLAM十四讲 第二版》笔记及课后习题（第三讲）相关推荐
- 视觉SLAM十四讲第二版踩坑总结
寒假花了点时间把slam第二版过了一遍,安装库文件实在太麻烦,我又总是因为内存问题把ubuntu系统搞坏,前前后后安了三四次.在此,记录第二版安装踩坑,如果未来不幸又要重装,留个参考. 视觉SLAM十 ...
- 视觉SLAM十四讲第二章学习与课后题与随笔日记
视觉SLAM十四讲第二章 主要内容是linux下c++编程,cpp,lib,smakelist这类文件关联与使用. 前面出现过的问题: P31-P32 make: *** 没有指明目标并且找不到 ma ...
- 《视觉SLAM十四讲 第二版》笔记及课后习题（第一讲）
前言 之所以想要写这个系列的博客,是因为想要总结一下高博的<SLAM视觉十四讲第二版>的各章内容以及自己对书后习题的一些做法,也算是对自己学习过程的一个总结和回顾.博客分为两个大部分,即读 ...
- 《视觉SLAM十四讲 第二版》笔记及课后习题（第七讲）
读书笔记:视觉里程计1 之前的内容,介绍了运动方程和观测方程的具体形式,并讲解了以非线性优化为主的求解方法.从本讲开始,我们结束了基础知识的铺垫,开始步入正题:按照第二讲的内容,分别介绍视觉里程计.优 ...
- 《视觉SLAM十四讲 第二版》课后习题
本文为<视觉SLAM十四讲>(第二版)的课后习题解答,为本人学习时参考着网上的资源所写的答案,可能有所纰漏,希望大家指出. 文章目录 第1讲 预备知识 第2讲 初始SLAM 第3讲 三维空 ...
- 视觉SLAM十四讲CH6代码解析及课后习题详解
gaussNewton.cpp #include <iostream> #include <chrono> #include <opencv2/opencv.hpp> ...
- 视觉SLAM十四讲CH10代码解析及课后习题详解
g2o_viewer问题解决 在进行位姿图优化时候,如果出现g2o_viewer: command not found,说明你的g2o_viewer并没有安装上,打开你之前安装的g2o文件夹,打开bi ...
- 视觉SLAM十四讲CH8代码解析及课后习题详解
第一版的代码: direct_semidense.cpp #include <iostream> #include <fstream> #include <list> ...
- 视觉SLAM十四讲CH3代码解析及课后习题详解
eigenMatrix.cpp #include <iostream>using namespace std;#include <ctime> // Eigen 核心部分 #i ...
最新文章
- 通过css类/选择器选取元素 文档结构和遍历 元素树的文档
- C2059 语法错误:“)”
- 前端学习（1494）：表格案例--axios-搜索功能
- Android 之 ProgressDialog用法介绍
- mysql 变量赋值 in_MySQL 存储过程传参数实现where id in(1,2,3,...)实例效果
- 罗克韦尔Studio5000遇上西门子Process Simulate：数字化仿真与虚拟调试案例
- Linux 虚拟网卡技术：Macvlan
- 【超实用的浏览器插件】目前Google Chrome最好用的插件，为什么你还在犹豫不决？
- x64dbg入门学习
- SVN更新或提交时出现冲突该如何解决
- 玩机搞机----mtk芯片机型 另类制作备份线刷包的方式 读写分区等等
- Kaggle word2vec NLP 教程 第三部分：词向量的更多乐趣
- POJ2404:Jogging Trails
- SEO链接为什么要用nofollow，nofollow属性的作用是什么，nofollow的用法
- 数博会”为何落户贵州？
- smart gesture安装失败_CAXA 2018软件下载和安装教程
- 安利10个让你爽到爆的IDEA必备插件
- kafka-go源码解析四（Writer）
- python下载地址到迅雷qq旋风下载
- 高等数学——简单直观地了解定积分