1.验证旋转矩阵是正交矩阵。
2.验证四元数旋转某个点后,结果是一个虚四元数(实部为零),所以仍然对应到一个三维空间点。
注意:目前市面上所有的博客都说旋转四元数的逆是共轭除以模的平方 ,这么算很正确但是计算量很大并且复杂化了。我们前面说到,旋转四元数通常是单位四元数。这是因为旋转四元数的定义要求其模长(或长度)为1。这样的要求确保了当四元数用于表示三维空间中的旋转时,不会引入缩放(即保持物体的大小不变)。
![](https://i-blog.csdnimg.cn/direct/17f24ead281b4097af2a6734729aa017.png)
所以,既然我们假设 ( q_r ) 是一个单位四元数,那么它的逆就等于它的共轭。因此,旋转后的四元数仍然是一个虚四元数。
但如果 ( q_r ) 不是单位四元数,那么它的逆不是简单的共轭,而是共轭除以模的平方。
3.假设我有一个大的 Eigen 矩阵,我想把它的左上角 3 × 3 的块取出来,然后赋值为I_3*3。请编程实现此事。
要在 Eigen 中实现将一个大的矩阵的左上角 3x3 的块赋值为 3x3 单位矩阵I,可以使用 Eigen 提供的块操作功能。下面是一个示例代码:
#include <iostream>
#include <Eigen/Dense>
int main() {
// 创建一个大小为 5x5 的矩阵
Eigen::MatrixXd mat = Eigen::MatrixXd::Random(5, 5);
std::cout << "原始矩阵:" << std::endl;
std::cout << mat << std::endl;
// 将左上角 3x3 的子矩阵赋值为单位矩阵
mat.block<3, 3>(0, 0) = Eigen::Matrix3d::Identity();
std::cout << "修改后的矩阵:" << std::endl;
std::cout << mat << std::endl;
return 0;
}
代码说明:
1.
Eigen::MatrixXd mat = Eigen::MatrixXd::Random(5, 5);
创建一个随机的 5x5 矩阵。
2.
mat.block<3, 3>(0, 0)
使用 block
函数选择矩阵的一个子块。这里的 <3, 3>
表示块的大小,(0, 0)
表示块的起始位置(左上角)。
3.
Eigen::Matrix3d::Identity()
生成一个 3x3 的单位矩阵。
4.将这个单位矩阵赋值给 mat.block<3, 3>(0, 0)
,即修改原矩阵左上角的 3x3 子矩阵。
4.设有小萝卜一号和小萝卜二号位于世界坐标系中。小萝卜一号的位姿为:q1 = [0.35, 0.2, 0.3, 0.1], t2 = [0.3, 0.1, 0.1]T(q 的第一项为实部。请你把 q 归一化后再进行计算)。这里的 q 和 t 表达的是 Tcw,也就是世界到相机的变换关系。小萝卜二 号的位姿为 q2 = [−0.5, 0.4, −0.1, 0.2], t = [−0.1, 0.5, 0.3]T。现在,小萝卜一号看到某个点在自身的坐标系下,坐标为 p = [0.5, 0, 0.2]T,求该向量在小萝卜二号坐标系下的坐标。请编程实现此事
所需的知识基础详细请见我之前ch3的博客中1.2中的欧式变换部分:
视觉SLAM ch3—三维空间的刚体运动https://blog.csdn.net/Johaden/article/details/141023487
解题思路
步骤 1: 归一化四元数 q1
四元数 q1
的归一化可以通过计算其模长并除以模长来完成。
步骤 2: 构建变换矩阵
四元数可以用来表示旋转,而平移向量 t
可以用来表示平移。变换矩阵 T
是一个 4x4 的矩阵,它由四元数对应的旋转矩阵和平移向量构成。具体形式如下:
其中 R
是由四元数转换来的旋转矩阵。
步骤 3: 构建变换矩阵 T12
为了从一个坐标系转换到另一个坐标系,我们可以利用以下等式:
这里 (T1w)^{-1}
表示小萝卜一号坐标系到世界坐标系的变换,而 T2w
表示世界坐标系到小萝卜二号坐标系的变换。
步骤 4: 应用变换 T12
到点 p
上
将点 p
转换成齐次坐标形式 [p_x, p_y, p_z, 1]
,然后乘以变换矩阵 T12
,得到的结果就是点 p
在小萝卜二号坐标系下的坐标。
#include <iostream>
#include <Eigen/Dense>
#include<Eigen/Core>
#include<Eigen/Geometry>
using namespace std;
using namespace Eigen;
int main() {
// 步骤 1: 归一化四元数 q1
Quaterniond q1(0.35, 0.2, 0.3, 0.1);
//创建了一个 Quaterniond 类型的对象 q1,它表示一个四元数。四元数的构造函数接受四个参数,按照实部、虚部的顺序传入。在这里,实部为 0.35,虚部依次为 0.2, 0.3, 0.1。
q1.normalize(); //q1.normalize(); 将 q1 归一化,使得它的模等于 1。
// 步骤 2: 构建变换矩阵 T1w 和 T2w
Vector3d t1(0.3, 0.1, 0.1);
Vector3d t2(-0.1, 0.5, 0.3); //分别表示小萝卜一号和小萝卜二号的平移向量。
Matrix4d T1w = Matrix4d::Identity();
Matrix4d T2w = Matrix4d::Identity();
T1w.block<3, 3>(0, 0) = q1.toRotationMatrix(); //q1.toRotationMatrix() 将四元数转换为对应的旋转矩阵。
//T1w.block<3, 3>(0, 0) 选择 T1w 的左上角 3x3 子块,将旋转矩阵赋值给它。
T1w.block<3, 1>(0, 3) = t1; //T1w.block<3, 1>(0, 3) 选择 T1w 的右上角 3x1 子块,将平移向量 t1 赋值给它。
T2w.block<3, 3>(0, 0) = Quaterniond(-0.5, 0.4, -0.1, 0.2).toRotationMatrix(); //同样,这里创建了一个新的 Quaterniond 对象来表示小萝卜二号的四元数,并将其转换为旋转矩阵,赋值给 T2w 的左上角 3x3 子块。
T2w.block<3, 1>(0, 3) = t2;//平移向量 t2 赋值给 T2w 的右上角 3x1 子块。
// 步骤 3: 构建变换矩阵 T12
Matrix4d T12 = T2w * T1w.inverse();//T1w.inverse() 计算 T1w 的逆矩阵,表示从相机坐标系到世界坐标系的变换。
//T2w * T1w.inverse() 计算从小萝卜一号到小萝卜二号坐标系的变换矩阵 T12。
// 步骤 4: 应用变换 T12 到点 p 上
Vector3d p(0.5, 0, 0.2);
Vector4d p_homogeneous(p.x(), p.y(), p.z(), 1.0);//将点 p 转换为齐次坐标形式 p_homogeneous,添加了一个额外的维度,其值为 1。
Vector4d p_transformed = T12 * p_homogeneous;
//使用变换矩阵 T12 将点 p 从齐次坐标形式转换到小萝卜二号坐标系下的坐标。
// 输出结果
cout << "Transformed point in Robot 2 coordinate system: "
<< p_transformed.head<3>() << endl;//p_transformed.head<3>() 返回前三个元素,即 x, y, z 坐标。
return 0;
}
![](https://i-blog.csdnimg.cn/direct/d7cf7e95881b4f80a3a0b1ace2bd6add.png)
上面的截图表示,即使p_homogeneous(p.x(), p.y(), p.z(), 1.0)中最后那个1.0不写,理论上来说,Eigen库会将三维向量自动拓展为齐次坐标形式。