题目来源于up主全日制学生混的SLAM课程,代码链接如下:
如果打不开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的博客。
![](https://i-blog.csdnimg.cn/direct/7ca16c8355a04b738f4bc503f615b8d9.png)
下图中介绍了绕Z轴旋转45度的代码,那么本题该部分的代码也类似于之前的例子
![](https://i-blog.csdnimg.cn/direct/52444571321e495186c7591c6d6a005e.png)
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的位置了。