视觉SLAM十四讲 -CH3 课后习题

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

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

3.验证四元数旋转某个点后,结果是一个虚四元数(实部为零),所以仍然对应到一个三维空间点(式 3.34)

  4.画表总结旋转矩阵、轴角、欧拉角、四元数的转换关系。

四元数、变换矩阵、欧拉角、轴角的转换关系详解

四元数，旋转矩阵，轴角，欧拉角的相互转换（原理+Eigen代码实现）

5.假设有一个打的Eigen矩阵，想把它的左上角3X3的块取出来，然后赋值为I3x3  。请编程实现。

#include<iostream>
#include<Eigen/Core>
#include<Eigen/Geometry>
#include <Eigen/Dense>
#include <ctime>

using namespace std;

#define MATRIX_SIZE 5
int main() {
    //STEP1: learn to use block;
    Eigen::MatrixXd matrixXd;
    matrixXd = Eigen::MatrixXd::Random(MATRIX_SIZE,MATRIX_SIZE);
    cout << matrixXd << endl;
    matrixXd.block<3,3>(0,0) =Eigen::Matrix3d::Identity();
    cout << "------------------------------------------" << endl;
    cout << matrixXd << endl;
    cout << "------------------------------------------" << endl;
    return 0;
}

6.一般线性方程Ax=b有几种解法？你能在Eigen中实现吗？

#include<iostream>
#include<Eigen/Core>
#include<Eigen/Geometry>
#include <Eigen/Dense>
#include <ctime>

using namespace std;

#define MATRIX_SIZE 5
int main() {
    //STEP2: solve the linear equations like Ax = b by lots of methods;
    Eigen::MatrixXd matrixXd;
    matrixXd = Eigen::MatrixXd::Random(MATRIX_SIZE,MATRIX_SIZE);
    cout << matrixXd << endl;
    Eigen::VectorXd v_b = Eigen::VectorXd::Random(MATRIX_SIZE ,1);

    // normal inverse
    clock_t time = clock();
    Eigen::VectorXd x = matrixXd.inverse() * v_b;
    cout << "out of normal inverse is " << x.transpose() << endl;
    cout <<"time use in normal inverse is " << 1000* (clock() - time)/(double)CLOCKS_PER_SEC << "ms"<< endl;

    // QR composition
    time = clock();
    x = matrixXd.colPivHouseholderQr().solve(v_b);
    cout << "out of QR composition is " << x.transpose() << endl;
    cout <<"time use in QR composition is " << 1000* (clock() - time)/(double)CLOCKS_PER_SEC << "ms"<< endl;

    // least-squares solution
    time = clock();
    x = matrixXd.bdcSvd(Eigen::ComputeThinU | Eigen::ComputeThinV).solve(v_b);
    cout << "out of least-squares solution is: " << x.transpose() << endl;
    cout <<"time use in least-squares solution is " << 1000* (clock() - time)/(double)CLOCKS_PER_SEC << "ms"<< endl;

    // cholesky composition (positive definite matrix)
    time = clock();
    x = matrixXd.ldlt().solve(v_b);
    cout << "out of cholesky composition is " << x.transpose() << endl;
    cout <<"time use in cholesky composition is " << 1000* (clock() - time)/(double)CLOCKS_PER_SEC << "ms"<< endl;
    return 0;
}

7.

#include<iostream>
#include<Eigen/Core>
#include<Eigen/Geometry>

using namespace std;

int main() {

    Eigen::Quaterniond q1 = {0.35,0.2,0.3,0.1};
    q1.normalize();     //归一化
    Eigen::Isometry3d Tcw1=Eigen::Isometry3d::Identity();
    Tcw1.rotate(q1);
    Tcw1.pretranslate(Eigen::Vector3d(0.3,0.1,0.1));
    Eigen::Vector3d pc (0.5,0,0.2);
    Eigen::Vector3d pw = Tcw1.inverse() * pc;   // pw = Twc * pc
    Eigen::Quaterniond q2 = {-0.5,0.4,-0.1,0.2};
    q2.normalize();     //归一化
    Eigen::Isometry3d Tcw2=Eigen::Isometry3d::Identity();
    Tcw2.rotate(q2);
    Tcw2.pretranslate(Eigen::Vector3d(-0.1,0.5,0.3));
    Eigen::Vector3d p = Tcw2 * pw;
    cout << "The coordinates of p :" << p.transpose() << endl;
    return 0;

}