视觉SLAM十四讲第三讲课后习题

1.验证旋转矩阵的正交性，参考此博客，写得非常好
2.验证罗德里格斯公式：略
3.验证四元素旋转某个点后，参见此博客
4.后续补充
5.提取某个矩阵的3*3部分并初始化为单位矩阵

/*
 * @Author  初一
 * @Date    2020.4.28
 * @Files   main.cpp
 * @Brief   提取矩阵某一部分并初始化为单位矩阵
 * @Brief   转载需取得作者同意
 */
#include 
#include 
#include 
using namespace std;

int main() {
     
    Eigen::Matrix<double,4,4> matrix_4d = Eigen::Matrix<double,4,4>::Random();
    cout << "matrix_4d = \n" << matrix_4d <<endl;
    cout << "方法一:" << endl;
    Eigen::Matrix3d matrix_3d = matrix_4d.block(0,0,3,3);
    cout << "提取出来的部分: \n" << matrix_3d << endl;
    matrix_3d = Eigen::Matrix3d::Identity();//初始化为单位矩阵
    cout << "初始化后为: \n" << matrix_3d <<endl;

    cout << "***********方法二***********" << endl;
    for (int i = 0; i < 3; ++i) {
     
        for (int j = 0; j < 3; ++j) {
     
            matrix_3d(i,j) = matrix_4d(i,j);
        }
    }
    cout << "提取出来的部分: \n" << matrix_3d << endl;
    matrix_3d = Eigen::Matrix3d::Identity();//初始化为单位矩阵
    cout << "初始化后为: \n" << matrix_3d <<endl;
    return 0;
}

CMakeLists.txt代码

cmake_minimum_required(VERSION 3.16)
project(pratice5)

set(CMAKE_CXX_STANDARD 14)
include_directories(/usr/include/eigen3)
add_executable(pratice5 main.cpp)

6.求解线性方程$Ax=b$

/*
 * @Author  初一
 * @Date    2020.4.28
 * @Files   main.cpp
 * @Brief   使用多种方式求解Ax=b
 * @Brief   转载需取得作者同意
 */
#include 
#include 
#include 
#include 

#define MATRIX_MAXSIZE  100
#define UNKNOW_MAXSIZE  100
using namespace std;
int main() {
     
    Eigen::Matrix<double, MATRIX_MAXSIZE ,MATRIX_MAXSIZE> matrix_NN = Eigen::Matrix<double, MATRIX_MAXSIZE, MATRIX_MAXSIZE>::Random();//构造方程
    Eigen::Matrix<double, UNKNOW_MAXSIZE, 1> matrix_result = Eigen::Matrix<double, UNKNOW_MAXSIZE, 1>::Random();//构造结果
    Eigen::Matrix<double, UNKNOW_MAXSIZE, 1> x = Eigen::Matrix<double, UNKNOW_MAXSIZE, 1>::Identity();//构造未知数

    cout << "*********直接求逆法*********" << endl;//只有方阵才可以用该方法
    clock_t time = clock();
    if (MATRIX_MAXSIZE == UNKNOW_MAXSIZE)
    {
     
        x = matrix_NN.inverse() * matrix_result;
        cout <<  "直接法求解时间: " << 1000*(clock()-time)/(double)CLOCKS_PER_SEC << "ms" <<endl;
        cout << "解为: \n" << x <<endl;
    } else
    {
     
        cout << "无法求解非方阵" <<endl;
    }

    cout << "*********QR分解法*********" <<endl;
    time = clock();
    x = matrix_NN.colPivHouseholderQr().solve(matrix_result);
    cout <<  "QR法求解时间: " << 1000*(clock()-time)/(double)CLOCKS_PER_SEC << "ms" <<endl;
    cout << "解为: \n" << x <<endl;

    cout << "*********最小二乘法分解法*********" <<endl;
    time = clock();
    x = (matrix_NN.trace() * matrix_NN).inverse() * (matrix_NN.trace() * matrix_result);
    cout <<  "最小二乘法法求解时间: " << 1000*(clock()-time)/(double)CLOCKS_PER_SEC << "ms" <<endl;
    cout << "解为: \n" << x <<endl;
    return 0;
}

CMakeLists.txt跟第五题一样
7.编程实现根据四元素坐标转换

/*
 * @Author  初一
 * @Date    2020.4.28
 * @Files   main.cpp
 * @Brief   已知相机坐标系的某个坐标及变换关系,将其转换为另外一个相机坐标系的坐标
 * @Brief   转载需取得作者同意
 */
#include 
#include 
#include 

using namespace std;
int main() {
     
    Eigen::Quaterniond q1 = Eigen::Quaterniond(0.35,0.2,0.3,0.1);
    Eigen::Quaterniond q2 = Eigen::Quaterniond (-0.5,0.4,-0.1,0.2);
    //Eigen::Quaterniond q3(0.4,0.2,0.1,0.7);也可以采用这种初始化方法
    q1.normalize();
    q2.normalize();//四元数归一化
    //变换为变换矩阵
    Eigen::Isometry3d Tcw1(q1);
    Eigen::Isometry3d Tcw2(q2);

    //定义平移量
    Eigen::Vector3d pre1(0.3,0.1,0.1);
    Eigen::Vector3d pre2(-0.1,0.5,0.3);

    //对变换矩阵添加平移量
    Tcw1.pretranslate(pre1);
    Tcw2.pretranslate(pre2);

    //定义旋转点及目标点
    Eigen::Vector3d p1 = Eigen::Vector3d(0.5,0,0.2);
    Eigen::Vector3d p2 = Eigen::Vector3d::Zero();

    //进行旋转
    p2 =  Tcw2 * Tcw1.inverse() * p1;//先将p1由相机坐标系转换为世界坐标系,再由世界坐标系转换为相机坐标系,对于Isometry变换矩阵,eigen会自动将p1转换为齐次坐标
    cout << "q2下的p1坐标为: " << p2.transpose();
    return 0;
}

CMakeLists.txt跟第五题一样