视觉SLAM十四讲 Chapter3 课后练习

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

2.验证四元数旋转某个点后，结果仍然是一个虚四元数(实部为0)，所以仍然对应一个三维空间点

3.画表总结旋转矩阵、旋转向量(轴角)、欧拉角、四元数的转换关系

4.假设有一个大的Eigen矩阵，想把它的左上角3*3的块取出来，然后赋值为I_3*3，编程实现。

assignment5.cpp

//2019.08.05
#include <iostream>
#include <ctime>
using namespace std;

//Eigen部分
#include <Eigen/Core>
//稠密矩阵的代数运算（逆、特征值等）
#include <Eigen/Dense>

#define MATRIX_SIZE 15

int main()
{
	//用随机数填充一个double类型的N*N矩阵
	Eigen::Matrix<double,MATRIX_SIZE ,MATRIX_SIZE > matrix_NN = Eigen::MatrixXd::Random(MATRIX_SIZE,MATRIX_SIZE);
	cout <<"matrix_NN:"<<endl<<matrix_NN<<endl;

	//定义一个3*3的double矩阵，用于盛放matrix_NN左上角3*3的内容
	Eigen::Matrix3d matrix_33;	
	
	//初始化为3*3的单位阵，用于对matrix_NN左上角进行赋值
	Eigen::Matrix3d matrix_I33 = Eigen::Matrix3d::Identity();

	//将matrix_NN左上角3*3的块取出来，放到matrix_33中
	matrix_33 = matrix_NN.topLeftCorner(3,3);
	cout <<"matrix_33:"<<endl<<matrix_33<<endl;

	//将matrix_I33 赋值到matrix_NN左上角3*3的块中
	matrix_NN.topLeftCorner(3,3) = matrix_I33;
	cout <<"matrix_NN:"<<endl<<matrix_NN<<endl;
	
	return 0;
}

CMakeLists.txt

#2019.08.05
#添加Eigen的头文件
include_directories("/usr/include/eigen3")

#添加一个可执行程序
add_executable( assignment5 assignment5.cpp )

运行结果：

5.设有机器人一号和机器人二号位于世界坐标系中。机器人一号的位姿为q₁=[0.35,0.2,0.3,0.1],t₁=[0.3,0.1,0.1]^T（q的第一项为实部。请把q归一化后再进行计算）。这里的q和t表达的是T_cw，是世界坐标系到相机坐标系的变换关系。机器人二号的位姿q₂=[-0.5,0.4,-0.1,0.2],t₂=[-0.1,0.5,0.3]^T.现在，机器人一号看到某个点在自身的坐标系下坐标为p=[0.5,0,0.2]^T.求该向量在机器人二号坐标系下的坐标，编程实现。

assignment7.cpp

//2019.08.05
#include <iostream>
#include <cmath>
using namespace std;

#include <Eigen/Core>
//Eigen几何模块
#include <Eigen/Geometry>

//先根据q1和t1，将pc1转换到世界坐标系pw中，然后根据q2和t2,将pw转换到pc2
int main()
{
	Eigen::Quaterniond q1(0.35,0.2,0.3,0.1);
	q1.normalize();//对q1进行归一化
	Eigen::Vector3d t1(0.3,0.1,0.1);
	Eigen::Quaterniond q2(-0.5,0.4,-0.1,0.2);
	q2.normalize();//对q2进行归一化
	Eigen::Vector3d t2(-0.1,0.5,0.3);
	Eigen::Vector3d pc1(0.5,0,0.2);
	Eigen::Vector3d pc2,pw;

	//将pc1转换成pw
	pw = q1.inverse()*(pc1 - t1);
	cout<<"pw:"<<pw.transpose()<<endl;

	//将pw转换成pc2
	pc2 = q2*pw + t2;
	cout<<"pc2:"<<pc2.transpose()<<endl;

	return 0;
}

CMakeLists.txt

#2019.08.05
#添加Eigen的头文件
include_directories("/usr/include/eigen3")

#添加一个可执行程序
add_executable( assignment7 assignment7.cpp )

运行结果：