视觉SLAM十四讲 Chapter3 课后练习
- 1. 验证旋转矩阵是正交矩阵
- 2.验证四元数旋转某个点后,结果仍然是一个虚四元数(实部为0),所以仍然对应一个三维空间点
- 3.画表总结旋转矩阵、旋转向量(轴角)、欧拉角、四元数的转换关系
- 4.假设有一个大的Eigen矩阵,想把它的左上角3*3的块取出来,然后赋值为I~3*3~,编程实现。
- 5.设有机器人一号和机器人二号位于世界坐标系中。机器人一号的位姿为q~1~=[0.35,0.2,0.3,0.1],t~1~=[0.3,0.1,0.1]^T^(q的第一项为实部。请把q归一化后再进行计算)。这里的q和t表达的是T~cw~,是世界坐标系到相机坐标系的变换关系。机器人二号的位姿q~2~=[-0.5,0.4,-0.1,0.2],t~2~=[-0.1,0.5,0.3]^T^.现在,机器人一号看到某个点在自身的坐标系下坐标为p=[0.5,0,0.2]^T^.求该向量在机器人二号坐标系下的坐标,编程实现。
1. 验证旋转矩阵是正交矩阵
2.验证四元数旋转某个点后,结果仍然是一个虚四元数(实部为0),所以仍然对应一个三维空间点
3.画表总结旋转矩阵、旋转向量(轴角)、欧拉角、四元数的转换关系
4.假设有一个大的Eigen矩阵,想把它的左上角3*3的块取出来,然后赋值为I3*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.设有机器人一号和机器人二号位于世界坐标系中。机器人一号的位姿为q1=[0.35,0.2,0.3,0.1],t1=[0.3,0.1,0.1]T(q的第一项为实部。请把q归一化后再进行计算)。这里的q和t表达的是Tcw,是世界坐标系到相机坐标系的变换关系。机器人二号的位姿q2=[-0.5,0.4,-0.1,0.2],t2=[-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 )
运行结果: