视觉SLAM补充习题（来源于B站博主）

题目来源于up主全日制学生混的SLAM课程，代码链接如下：

GitHub - cckaixin/Practical_Homework_for_slambook14: This repository is used to store SLAM 14 training exercises

如果打不开github请参考下面两篇博文，亲测有效！！

解决国内 github.com 打不开的最最最准确方法https://blog.csdn.net/qq_41176055/article/details/128496628

Windows系统下以管理员身份修改系统文件，以修改hosts文件为例https://blog.csdn.net/m0_58086930/article/details/128462187

下面所有的例子截图均来源于我之前ch3的博客

视觉SLAM ch3—三维空间的刚体运动https://blog.csdn.net/Johaden/article/details/141023487

题目：

有两个右手系1和2,其中2系的x轴与1系的y轴方向相同，2系的y轴与1系z轴方向相反，2系的z轴与1系的x轴相反,两个坐标系原点重合。求R12，求1系中(1,1,1)在2系中的坐标。请自己编写一个c++程序实现它，并用Cmake编译，得到能输出答案的可执行文件

解答分析：

我们可以发现，从Ⅰ到Ⅱ，x（roll），y（pitch）分别转动了-pi/2，注意先转动的轴可以带动后转动的轴，而后转动的轴无法带动先转动的轴。

 double roll,pitch,yaw;
    roll = -PI/2;
    pitch = -PI/2;
    yaw = 0;

 下面的图片告诉我们创建旋转矩阵的元素，来源于我ch3的博客。

如何创建旋转矩阵

下图中介绍了绕Z轴旋转45度的代码，那么本题该部分的代码也类似于之前的例子

如何表示旋转？

Eigen::AngleAxisd rv_roll(roll, Eigen::Vector3d::UnitX());                  
Eigen::AngleAxisd rv_pitch(pitch, rv_roll*Eigen::Vector3d::UnitY());        
Eigen::AngleAxisd rv_yaw(yaw, rv_pitch*rv_roll*Eigen::Vector3d::UnitZ()); 

上图中的Unix()其实和截图中的表示方法差不多，可以直接写为下面的形式：

Eigen::AngleAxisd rv_roll(roll, Eigen::Vector3d(1,0,0));                  
Eigen::AngleAxisd rv_pitch(pitch, rv_roll*Eigen::Vector3d(0,1,0));        
Eigen::AngleAxisd rv_yaw(yaw, rv_pitch*rv_roll*Eigen::Vector3d(0,0,1));

 如上图所示，之前我们的代码中仅绕一个轴作为旋转轴，可以使用.toRotationMatrix来按罗德里格斯公式的形式生成旋转矩阵

        现在，我们有两、三个轴都进行了旋转操作，那么该如何进行呢？其实很简单，就是需要将三个轴的“分”旋转向量合起来就可以，用*相连。也就是当我们想要执行多个旋转时，可以通过将这些旋转的矩阵相乘来表示复合旋转。

        在数学上，旋转矩阵的乘法遵循特定的顺序。例如，如果我们先绕 X 轴旋转，然后再绕 Y 轴旋转，那么复合旋转矩阵就是 R_Y * R_X，其中 R_X 和 R_Y 分别是绕 X 轴和 Y 轴的旋转矩阵。旋转矩阵的乘法不满足交换律，即 A * B ≠ B * A。这意味着旋转的顺序很重要。

相信大家看到这里会有一个疑问：

既然要用这些旋转的矩阵相乘来表示复合旋转，那么这种相乘是什么？

        其实这种相乘并不是我们所认为的简单的点乘和叉乘。在 Eigen 库内部，Eigen::AngleAxisd 类通过一个四元数来表示旋转。当我们将两个 Eigen::AngleAxisd 对象相乘时，实际上是在将两个四元数相乘。四元数的乘法遵循一定的规则，可以用来表示复合旋转。

所以接下来我们要生成旋转矩阵：

roation_matrix = (rv_yaw*rv_pitch*rv_roll).toRotationMatrix();    // R12

 最后，我们要使用旋转矩阵将Frame1下的已知点（如（1,1,1））转换为Frame2

和例子里面一样，我们需要把点和旋转矩阵相乘

 Eigen::Vector3d point_frame1(1,1,1);   //定义frame1
 Eigen::Vector3d point_frame2;          //定义
 point_frame2 = roation_matrix.transpose() * point_frame1;

最后输出我们想关注的元素即可，如：

std::cout << "point in frame2: " << point_frame2.transpose() << std::endl;

 需要编写配置文件：

cmake_minimum_required(VERSION 2.8)

project(ch3_homework)

find_package(Eigen3)

include_directories(${EIGEN3_INCLUDE_DIRS})

add_executable(answer ch3_reference.cpp)

target_link_libraries(answer ${EIGEN3_LIBRARIES})

上面是up主给的参考代码，运用了find_package，这是为了防止如果我们不知道“包”的位置而进行了一个搜索，其实eigen的位置我们是知道的，而且是不需要写链接的。

cmake_minimum_required(VERSION 2.8)

project(ch3_homework)

include_directories("/usr/include/eigen3")

add_executable(answer ch3_reference.cpp)

运行后我们就可以知道frame2的位置了。