高翔视觉slam十四讲书籍习题(第三讲)

- - - 目录

1. 验证旋转矩阵是正交矩阵。
2. 寻找罗德里格斯公式的推导过程并理解它。
3. 验证四元数旋转某个点后，结果是一个虚四元数（实部为零），所以仍然对应到一个三维空间点（式 3.34）。
4. 画表总结旋转矩阵、轴角、欧拉角、四元数的转换关系。
5. 假设我有一个大的 Eigen 矩阵，我想把它的左上角 3 × 3 的块取出来，然后赋值为**I**3×3。请编程实现此事。
6. 一般线程方程 Ax = b 有哪几种做法？你能在 Eigen 中实现吗？
7.设有小萝卜一号和小萝卜号位于世界坐标系中。

1. 验证旋转矩阵是正交矩阵。

总结一下：旋转矩阵是一个完美的矩阵——正交矩阵。①行列式为1，②每个列向量都是单位向量且相互正交，③它的逆等于它的转置。

2. 寻找罗德里格斯公式的推导过程并理解它。

罗德里格斯公式推导

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 中实现吗？

线性方程组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;
}

7.设有小萝卜一号和小萝卜号位于世界坐标系中。

小萝卜一号的位姿为: q 1 = [0.55, 0.3, 0.2, 0.2], t 1 =[0.7, 1.1, 0.2] T (q 的第一项为实部)。这里的 q 和 t 表达的是 T cw ,也就是世界到相机的变换关系。
小萝卜二号的位姿为 q 2 = [−0.1, 0.3, −0.7, 0.2], t 2 = [−0.1, 0.4, 0.8] T 。
现在,小萝卜一号看到某个点在自身的坐标系下,坐标为 p 1 = [0.5, −0.1, 0.2] T ,
求该向量在小萝卜二号坐标系下的坐标。请编程实现此事,并提交你的程序。

提示：
1、四元数在使用前需要归一化。
2、请注意 Eigen 在使用四元数时的虚部和实部顺序。
3、参考答案为 p 2 = [1.08228, 0.663509, 0.686957] T 。你可以用它验证程序是否正确。

代码：

#include "iostream"
#include "ctime"
using namespace std;#include "Eigen/Core"
// 包含几何模块，提供了各种旋转和平移的表示
#include "Eigen/Geometry"int main(int argc, char** argv)
{Eigen::Quaterniond q1 = {0.55, 0.3, 0.2, 0.2};//注意coeffs的顺序是(x,y,z,w),w为实部，前三者为虚部//cout .precision(3);//设置输出的有效数字位数cout << "q1 = \n" << q1.coeffs() << endl;//四元数使用前须归一化q1.normalize();cout << "normalize q1 = \n" << q1.coeffs() << endl;//Eigen::Vector3d v_vector = {0.7, 1.1, 0.2};//T是单位四元数//Eigen::Matrix3d R1 = q1.toRotationMatrix();//旋转矩阵R1，将四元数变成旋转矩阵REigen::Isometry3d T1 = Eigen::Isometry3d::Identity();//欧式变换矩阵使用 Eigen::Isometry//T1.rotate(R1);//旋转T1.rotate(q1);T1.pretranslate(Eigen::Vector3d (0.7, 1.1, 0.2));//平移cout << T1.matrix() << endl;Eigen::Quaterniond q2 = {-0.1, 0.3, -0.7, 0.2};//注意coeffs的顺序是(x,y,z,w),w为实部，前三者为虚部cout << "q2 = \n" << q2.coeffs() << endl;//四元数使用前须归一化q2.normalize();cout << "normalize q2 = \n" <<q2.coeffs() << endl;//T是单位四元数//Eigen::Matrix3d R2 = q2.toRotationMatrix();//旋转矩阵R2Eigen::Isometry3d T2 = Eigen::Isometry3d::Identity();//欧式变换矩阵使用 Eigen::Isometry//T2.rotate(R2);//旋转T2.rotate(q2);T2.pretranslate(Eigen::Vector3d (-0.1, 0.4, 0.8));//平移cout << T2.matrix() << endl;Eigen::Vector4d p1 = {0.5, -0.1, 0.2, 1};Eigen::Vector4d pw;clock_t clk_time = clock();pw = T1.matrix().colPivHouseholderQr().solve(p1);//利用QR分解求出世界坐标cout << pw.transpose() << endl;cout <<"time use in Qr decomposition is " <<1000*  (clock() - clk_time)/(double)CLOCKS_PER_SEC <<"ms" << endl;Eigen::Vector4d p2;p2 = T2*pw;cout << p2.transpose() << endl;cout <<"time use in Qr decomposition is " <<1000*  (clock() - clk_time)/(double)CLOCKS_PER_SEC <<"ms" << endl;return 0;
}

编译错误：

error: in C++98 ‘q1’ must be initialized by constructor, not by ‘{...}’

在 CMakeLists.txt 里面添加一行，重新编译，就OK了。

add_compile_options(-std=c++11)

运行结果：