视觉slam十四讲习题ch3-6
题目回顾:
一般解线性方程Ax=b有哪几种做法?你能在Eigen中实现吗?
解: 线性方程组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, > 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();
//设置变量
Eigen::Matrix<double,MATRIX_SIZE, MATRIX_SIZE_> matrix_NN = Eigen::MatrixXd::Random(MATRIX_SIZE,MATRIX_SIZE_);
Eigen::Matrix<double ,MATRIX_SIZE, > v_Nd = Eigen::MatrixXd::Random(MATRIX_SIZE,); //测试用例
matrix_NN << ,,,,-,,,,;
v_Nd <<,-,; //设置解变量
Eigen::Matrix<double,MATRIX_SIZE_,>x; //时间变量
clock_t tim_stt = clock(); /*1、求逆法 很可能没有解 仅仅针对方阵才能计算*/
#if (MATRIX_SIZE == MATRIX_SIZE_)
x = matrix_NN.inverse() * v_Nd;
std::cout<<"直接法所用时间和解为:"<< *(clock() - tim_stt)/(double)CLOCKS_PER_SEC
<<"MS"<< std::endl << x.transpose() << std::endl;
#else
std::cout<<"直接法不能解!(提示:直接法中方程组的个数必须与变量个数相同,需要设置MATRIX_SIZE == MATRIX_SIZE_)"<<std::endl;
#endif /*2、QR分解解方程组 适合非方阵和方阵 当方程组有解时的出的是真解,若方程组无解得出的是近似解*/
tim_stt = clock();
x = matrix_NN.colPivHouseholderQr().solve(v_Nd);
std::cout<<"QR分解所用时间和解为:"<<*(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<<"最小二乘法所用时间和解为:"<< *(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分解方法所用时间和解为:" << *(clock() - tim_stt)/(double)CLOCKS_PER_SEC
<< "MS" << std::endl << x.transpose() << std::endl;
#else
std::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 分解方法所用时间和解为:" << *(clock() - tim_stt)/(double)CLOCKS_PER_SEC
<< "MS"<< std::endl<< x.transpose()<<std::endl;
#else
std::cout<< "Cholesky法不能解!(提示:直接法中方程组的个数必须与变量个数相同,需要设置MATRIX_SIZE == MATRIX_SIZE_)"<<std::endl;
#endif /*6、Jacobi迭代法 */
#if (MATRIX_SIZE == MATRIX_SIZE_)
int Iteration_num = ;
double Accuracy =0.01;
tim_stt = clock();
x= Jacobi(matrix_NN,v_Nd,Iteration_num,Accuracy);
std::cout<< "Jacobi 迭代法所用时间和解为:" << *(clock() - tim_stt)/(double)CLOCKS_PER_SEC
<< "MS"<< std::endl<< x.transpose()<<std::endl;
#else
std::cout<<"LU分解法不能解!(提示:直接法中方程组的个数必须与变量个数相同,需要设置MATRIX_SIZE == MATRIX_SIZE_)"<<std::endl;
#endif return ;
} //迭代不收敛的话 解向量是0
Mat_B Jacobi(Mat_A &A,Mat_B &b, int &iteration_num, double &accuracy ) {
Mat_B x_k = Eigen::MatrixXd::Zero(MATRIX_SIZE_,);//迭代的初始值
Mat_B x_k1; //迭代一次的解向量
int k,i; //i,k是迭代算法的循环次数的临时变量
double temp; //每迭代一次解向量的每一维变化的模值
double R=; //迭代一次后,解向量每一维变化的模的最大值
int isFlag = ; //迭代要求的次数后,是否满足精度要求 //判断Jacobi是否收敛
Mat_A D; //D矩阵
Mat_A L_U; //L+U
Mat_A temp2 = A; //临时矩阵获得A矩阵除去对角线后的矩阵
Mat_A B; //Jacobi算法的迭代矩阵
Eigen::MatrixXcd EV;//获取矩阵特征值
double maxev=0.0; //最大模的特征值
int flag = ; //判断迭代算法是否收敛的标志 1表示Jacobi算法不一定能收敛到真值 std::cout<<std::endl<<"欢迎进入Jacobi迭代算法!"<<std::endl;
//對A矩陣進行分解 求取迭代矩陣 再次求取譜半徑 判斷Jacobi迭代算法是否收斂
for(int l= ;l < MATRIX_SIZE;l++){
D(l,l) = A(l,l);
temp2(l,l) = ;
if(D(l,l) == ){
std::cout<<"迭代矩阵不可求"<<std::endl;
flag =;
break;
}
}
L_U = -temp2;
B = D.inverse()*L_U; //求取特征值
Eigen::EigenSolver<Mat_A>es(B);
EV = es.eigenvalues();
// cout<<"迭代矩阵特征值为:"<<EV << endl; //求取矩陣的特征值 然後獲取模最大的特徵值 即爲譜半徑
for(int index = ;index< MATRIX_SIZE;index++){
maxev = ( maxev > __complex_abs(EV(index)) )?maxev:(__complex_abs(EV(index)));
}
std::cout<< "Jacobi迭代矩阵的谱半径为:"<< maxev<<std::endl; //谱半径大于1 迭代法则发散
if(maxev >= ){
std::cout<<"Jacobi迭代算法不收敛!"<<std::endl;
flag =;
} //迭代法收敛则进行迭代的计算
if (flag == ){
std::cout<<"Jacobi迭代算法谱半径小于1,该算法收敛"<<std::endl;
std::cout<<"Jacobi迭代法迭代次数和精度: "<< std::endl << iteration_num<<" "<<accuracy<<std::endl; //迭代计算
for( k = ;k < iteration_num ; k++ ){
for(i = ;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 = ;
break;
} //清零R,交换迭代解
R = ;
x_k = x_k1;
}
if( !isFlag )
std::cout << std::endl <<"迭代"<<iteration_num<<"次后仍然未达到精度要求,若不满意该解,请再次运行加大循环次数!"<< std::endl;
return x_k1;
}
//否则返回0
return x_k;
} //Jacobi迭代法的一步求和计算
double Jacobi_sum(Mat_A &A,Mat_B &x_k,int i) {
double sum;
for(int j = ; j< MATRIX_SIZE_;j++){
sum += A(i,j)*x_k(j);
}
return sum;
}
欢迎大家关注我的微信公众号「佛系师兄」,里面有关于 Ceres 以及 OpenCV 库等一些技术文章。
比如
「反复研究好几遍,我才发现关于 CMake 变量还可以这样理解!」
更多好的文章会优先在里面不定期分享!打开微信客户端,扫描下方二维码即可关注!
视觉slam十四讲习题ch3-6的更多相关文章
- 视觉slam十四讲第七章课后习题6
版权声明:本文为博主原创文章,转载请注明出处: http://www.cnblogs.com/newneul/p/8545450.html 6.在PnP优化中,将第一个相机的观测也考虑进来,程序应如何 ...
- 视觉slam十四讲第七章课后习题7
版权声明:本文为博主原创文章,转载请注明出处:http://www.cnblogs.com/newneul/p/8544369.html 7.题目要求:在ICP程序中,将空间点也作为优化变量考虑进来 ...
- 高博-《视觉SLAM十四讲》
0 讲座 (1)SLAM定义 对比雷达传感器和视觉传感器的优缺点(主要介绍视觉SLAM) 单目:不知道尺度信息 双目:知道尺度信息,但测量范围根据预定的基线相关 RGBD:知道深度信息,但是深度信息对 ...
- 浅读《视觉SLAM十四讲:从理论到实践》--操作1--初识SLAM
下载<视觉SLAM十四讲:从理论到实践>源码:https://github.com/gaoxiang12/slambook 第二讲:初识SLAM 2.4.2 Hello SLAM(书本P2 ...
- 《视觉SLAM十四讲》第2讲
目录 一 视觉SLAM中的传感器 二 经典视觉SLAM框架 三 SLAM问题的数学表述 注:原创不易,转载请务必注明原作者和出处,感谢支持! 本讲主要内容: (1) 视觉SLAM中的传感器 (2) 经 ...
- 《视觉SLAM十四讲》第1讲
目录 一 视觉SLAM 注:原创不易,转载请务必注明原作者和出处,感谢支持! 一 视觉SLAM 什么是视觉SLAM? SLAM是Simultaneous Localization and Mappin ...
- 视觉SLAM十四讲:从理论到实践 两版 PDF和源码
视觉SLAM十四讲:从理论到实践 第一版电子版PDF 链接:https://pan.baidu.com/s/1SuuSpavo_fj7xqTYtgHBfw提取码:lr4t 源码github链接:htt ...
- 高翔《视觉SLAM十四讲》从理论到实践
目录 第1讲 前言:本书讲什么:如何使用本书: 第2讲 初始SLAM:引子-小萝卜的例子:经典视觉SLAM框架:SLAM问题的数学表述:实践-编程基础: 第3讲 三维空间刚体运动 旋转矩阵:实践-Ei ...
- 《视觉SLAM十四讲》学习日志(一)——预备知识
SLAM简介 : SLAM是 Simultaneous Localization and Mapping 的缩写,中文译作 " 同时定位与地图构建 ".它是指搭载特定传感器的主题, ...
随机推荐
- Bandit Wargame Level18 Writeup(interactive shell and .bashrc )
Bandit Level 18 → Level 19 Level Goal The password for the next level is stored in a file readme in ...
- sqlserver 存储过程 修改
CREATE PROCEDURE [dbo].[UpdateMessage] @strTable varchar(), --要修改的表 @strColumn varchar(),--要修改的列名(如果 ...
- iOS 电脑新装的系统, 使用sourceTree 创建本地仓库的时候, 总是提示, 无效路径
把qq聊天记录分享出来: 我电脑新装的系统, 使用sourceTree 创建本地仓库的时候, 总是提示, 无效路径请问哪位遇到过求指教群里有产品经理没有? ssh 配制的不对重装系统过后,重新生成一下 ...
- ABP PUT、DELETE请求错误405.0 - Method Not Allowed 因为使用了无效方法(HTTP 谓词) 引发客户端错误 No 'Access-Control-Allow-Origin' header is present on the requested resource
先请检查是否是跨域配置问题,请参考博客:http://www.cnblogs.com/donaldtdz/p/7882225.html 一.问题描述 ABP angular前端部署后,查询,新增都没问 ...
- SSH框架完全整合
大三学期渐末,事情也挺多的,上周就开始着手整合SSH框架,到现在才真正的完成,过程中碰到了许多小问题(小问题大折腾,哭脸.jpg).本着善始善终的原则,最终把它给完成了. 本篇文章就在: win7 6 ...
- c#访问oracle数据库
想在c#中访问oracle数据库,毕竟是开发,想要轻量级访问oracle,客户机上无需安装oracle环境就能正常运行程序. 在网上找了相关资料,只需要引用一个dll即可实现. 访问代码(需引用dll ...
- String源码图
String StringBuffer StringBuilder 均为对字符数组的操作. 实现了不同的接口,导致不同的覆写. 实现了同样的接口,适应不同的场景.
- Webpack 2 视频教程 013 - 自动分离 CSS 到独立文件
原文发表于我的技术博客 这是我免费发布的高质量超清「Webpack 2 视频教程」. Webpack 作为目前前端开发必备的框架,Webpack 发布了 2.0 版本,此视频就是基于 2.0 的版本讲 ...
- PHP动态图像处理
相关代码见:https://www.github.com/lozybean/learn_www/ 目录 1. 画布管理: imagecreate():创建一个基于调色板的画布,指定画布的长.宽 ...
- this与base关键字
this关键字 this关键字代表当前对象,通过this关键字可以访问当前对象的成员.(当前对象的成员:自己本身的成员+从父类继承过来的所有的成员.) this关键字可以访问:本类的所有成员和父类的非 ...