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

---

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;
#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分解所用时间和解为："<<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;
#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 分解方法所用时间和解为:" << 1000*(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 = 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;
#else
    std::cout<<"LU分解法不能解!(提示:直接法中方程组的个数必须与变量个数相同，需要设置MATRIX_SIZE == MATRIX_SIZE_)"<<std::endl;
#endif

    return 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+U
    Mat_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;
    }
    //否则返回0
    return  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，将四元数变成旋转矩阵R
    Eigen::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();//旋转矩阵R2
    Eigen::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)

运行结果：