视觉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迭代算法并不一定收敛，只能试着做下，下面的程序实现过程仅仅处理了充要条件，迭代法同时有十分明显的优点——算法简单，因而编制程序比较容易，所以在实际求解问题中仍有非常大利用价值。
具体代码实现 如下:
1 #include<iostream> 2 #include<ctime> 3 #include <cmath> 4 #include <complex> 5 /* 线性方程组Ax = b的解法 ( 直接法（1,2,3,4,5）+迭代法(6) ) 其中只有2 3方法不要求方程组个数与变量个数相等 */ 6 7 //包含Eigen头文件 8 //#include <Eigen/Dense> 9 #include<Eigen/Core> 10 #include<Eigen/Geometry> 11 #include <Eigen/Eigenvalues> 12 13 //下面这两个宏的数值一样的时候 方法1 4 5 6才能正常工作 14 #define MATRIX_SIZE 3 //方程组个数 15 #define MATRIX_SIZE_ 3 //变量个数 16 //using namespace std; 17 typedef Eigen::Matrix<double,MATRIX_SIZE, MATRIX_SIZE_> Mat_A; 18 typedef Eigen::Matrix<double ,MATRIX_SIZE,1 > Mat_B; 19 20 //Jacobi迭代法的一步求和计算 21 double Jacobi_sum(Mat_A &A,Mat_B &x_k,int i); 22 23 //迭代不收敛的话 解向量是0 24 Mat_B Jacobi(Mat_A &A,Mat_B &b, int &iteration_num, double &accuracy ); 25 26 int main(int argc,char **argv) 27 { 28 //设置输出小数点后3位 29 std::cout.precision(3); 30 //设置变量 31 Eigen::Matrix<double,MATRIX_SIZE, MATRIX_SIZE_> matrix_NN = Eigen::MatrixXd::Random(MATRIX_SIZE,MATRIX_SIZE_); 32 Eigen::Matrix<double ,MATRIX_SIZE,1 > v_Nd = Eigen::MatrixXd::Random(MATRIX_SIZE,1); 33 34 //测试用例 35 matrix_NN << 10,3,1,2,-10,3,1,3,10; 36 v_Nd <<14,-5,14; 37 38 //设置解变量 39 Eigen::Matrix<double,MATRIX_SIZE_,1>x; 40 41 //时间变量 42 clock_t tim_stt = clock(); 43 44 /*1、求逆法 很可能没有解 仅仅针对方阵才能计算*/ 45 #if (MATRIX_SIZE == MATRIX_SIZE_) 46 x = matrix_NN.inverse() * v_Nd; 47 std::cout<<"直接法所用时间和解为："<< 1000*(clock() - tim_stt)/(double)CLOCKS_PER_SEC 48 <<"MS"<< std::endl << x.transpose() << std::endl; 49 #else 50 std::cout<<"直接法不能解!(提示:直接法中方程组的个数必须与变量个数相同，需要设置MATRIX_SIZE == MATRIX_SIZE_)"<<std::endl; 51 #endif 52 53 /*2、QR分解解方程组 适合非方阵和方阵 当方程组有解时的出的是真解，若方程组无解得出的是近似解*/ 54 tim_stt = clock(); 55 x = matrix_NN.colPivHouseholderQr().solve(v_Nd); 56 std::cout<<"QR分解所用时间和解为："<<1000*(clock() - tim_stt)/(double)CLOCKS_PER_SEC 57 << "MS" << std::endl << x.transpose() << std::endl; 58 59 /*3、最小二乘法 适合非方阵和方阵，方程组有解时得出真解，否则是最小二乘解(在求解过程中可以用QR分解 分解最小二成的系数矩阵) */ 60 tim_stt = clock(); 61 x = (matrix_NN.transpose() * matrix_NN ).inverse() * (matrix_NN.transpose() * v_Nd); 62 std::cout<<"最小二乘法所用时间和解为:"<< 1000*(clock() - tim_stt)/(double)CLOCKS_PER_SEC 63 << "MS" << std::endl << x.transpose() << std::endl; 64 65 /*4、LU分解方法 只能为方阵（满足分解的条件才行） */ 66 #if (MATRIX_SIZE == MATRIX_SIZE_) 67 tim_stt = clock(); 68 x = matrix_NN.lu().solve(v_Nd); 69 std::cout<< "LU分解方法所用时间和解为:" << 1000*(clock() - tim_stt)/(double)CLOCKS_PER_SEC 70 << "MS" << std::endl << x.transpose() << std::endl; 71 #else 72 std::cout<<"LU分解法不能解!(提示:直接法中方程组的个数必须与变量个数相同，需要设置MATRIX_SIZE == MATRIX_SIZE_)"<<std::endl; 73 #endif 74 75 /*5、Cholesky 分解方法 只能为方阵 (结果与其他的方法差好多)*/ 76 #if (MATRIX_SIZE == MATRIX_SIZE_) 77 tim_stt = clock(); 78 x = matrix_NN.llt().solve(v_Nd); 79 std::cout<< "Cholesky 分解方法所用时间和解为:" << 1000*(clock() - tim_stt)/(double)CLOCKS_PER_SEC 80 << "MS"<< std::endl<< x.transpose()<<std::endl; 81 #else 82 std::cout<< "Cholesky法不能解!(提示:直接法中方程组的个数必须与变量个数相同，需要设置MATRIX_SIZE == MATRIX_SIZE_)"<<std::endl; 83 #endif 84 85 /*6、Jacobi迭代法 */ 86 #if (MATRIX_SIZE == MATRIX_SIZE_) 87 int Iteration_num = 10 ; 88 double Accuracy =0.01; 89 tim_stt = clock(); 90 x= Jacobi(matrix_NN,v_Nd,Iteration_num,Accuracy); 91 std::cout<< "Jacobi 迭代法所用时间和解为:" << 1000*(clock() - tim_stt)/(double)CLOCKS_PER_SEC 92 << "MS"<< std::endl<< x.transpose()<<std::endl; 93 #else 94 std::cout<<"LU分解法不能解!(提示:直接法中方程组的个数必须与变量个数相同，需要设置MATRIX_SIZE == MATRIX_SIZE_)"<<std::endl; 95 #endif 96 97 return 0; 98 } 99 100 //迭代不收敛的话 解向量是0 101 Mat_B Jacobi(Mat_A &A,Mat_B &b, int &iteration_num, double &accuracy ) { 102 Mat_B x_k = Eigen::MatrixXd::Zero(MATRIX_SIZE_,1);//迭代的初始值 103 Mat_B x_k1; //迭代一次的解向量 104 int k,i; //i,k是迭代算法的循环次数的临时变量 105 double temp; //每迭代一次解向量的每一维变化的模值 106 double R=0; //迭代一次后，解向量每一维变化的模的最大值 107 int isFlag = 0; //迭代要求的次数后，是否满足精度要求 108 109 //判断Jacobi是否收敛 110 Mat_A D; //D矩阵 111 Mat_A L_U; //L+U 112 Mat_A temp2 = A; //临时矩阵获得A矩阵除去对角线后的矩阵 113 Mat_A B; //Jacobi算法的迭代矩阵 114 Eigen::MatrixXcd EV;//获取矩阵特征值 115 double maxev=0.0; //最大模的特征值 116 int flag = 0; //判断迭代算法是否收敛的标志 1表示Jacobi算法不一定能收敛到真值 117 118 std::cout<<std::endl<<"欢迎进入Jacobi迭代算法！"<<std::endl; 119 //對A矩陣進行分解 求取迭代矩陣 再次求取譜半徑 判斷Jacobi迭代算法是否收斂 120 for(int l=0 ;l < MATRIX_SIZE;l++){ 121 D(l,l) = A(l,l); 122 temp2(l,l) = 0; 123 if(D(l,l) == 0){ 124 std::cout<<"迭代矩阵不可求"<<std::endl; 125 flag =1; 126 break; 127 } 128 } 129 L_U = -temp2; 130 B = D.inverse()*L_U; 131 132 //求取特征值 133 Eigen::EigenSolver<Mat_A>es(B); 134 EV = es.eigenvalues(); 135 // cout<<"迭代矩阵特征值为:"<<EV << endl; 136 137 //求取矩陣的特征值 然後獲取模最大的特徵值 即爲譜半徑 138 for(int index = 0;index< MATRIX_SIZE;index++){ 139 maxev = ( maxev > __complex_abs(EV(index)) )?maxev:(__complex_abs(EV(index))); 140 } 141 std::cout<< "Jacobi迭代矩阵的谱半径为："<< maxev<<std::endl; 142 143 //谱半径大于1 迭代法则发散 144 if(maxev >= 1){ 145 std::cout<<"Jacobi迭代算法不收敛！"<<std::endl; 146 flag =1; 147 } 148 149 //迭代法收敛则进行迭代的计算 150 if (flag == 0 ){ 151 std::cout<<"Jacobi迭代算法谱半径小于1,该算法收敛"<<std::endl; 152 std::cout<<"Jacobi迭代法迭代次数和精度： "<< std::endl << iteration_num<<" "<<accuracy<<std::endl; 153 154 //迭代计算 155 for( k = 0 ;k < iteration_num ; k++ ){ 156 for(i = 0;i< MATRIX_SIZE_ ; i++){ 157 x_k1(i) = x_k(i) + ( b(i) - Jacobi_sum(A,x_k,i) )/A(i,i); 158 temp = fabs( x_k1(i) - x_k(i) ); 159 if( fabs( x_k1(i) - x_k(i) ) > R ) 160 R = temp; 161 } 162 163 //判断进度是否达到精度要求 达到进度要求后 自动退出 164 if( R < accuracy ){ 165 std::cout <<"Jacobi迭代算法迭代"<< k << "次达到精度要求."<< std::endl; 166 isFlag = 1; 167 break; 168 } 169 170 //清零R，交换迭代解 171 R = 0; 172 x_k = x_k1; 173 } 174 if( !isFlag ) 175 std::cout << std::endl <<"迭代"<<iteration_num<<"次后仍然未达到精度要求，若不满意该解，请再次运行加大循环次数！"<< std::endl; 176 return x_k1; 177 } 178 //否则返回0 179 return x_k; 180 } 181 182 //Jacobi迭代法的一步求和计算 183 double Jacobi_sum(Mat_A &A,Mat_B &x_k,int i) { 184 double sum; 185 for(int j = 0; j< MATRIX_SIZE_;j++){ 186 sum += A(i,j)*x_k(j); 187 } 188 return sum; 189 }
欢迎大家关注我的微信公众号「佛系师兄」，里面有关于 Ceres 以及 OpenCV 库等一些技术文章。
比如
「反复研究好几遍，我才发现关于 CMake 变量还可以这样理解！」
更多好的文章会优先在里面不定期分享！打开微信客户端，扫描下方二维码即可关注！