SLAM十四讲ch3课后习题

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

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

注意：目前市面上所有的博客都说旋转四元数的逆是共轭除以模的平方 ，这么算很正确但是计算量很大并且复杂化了。我们前面说到，旋转四元数通常是单位四元数。这是因为旋转四元数的定义要求其模长（或长度）为1。这样的要求确保了当四元数用于表示三维空间中的旋转时，不会引入缩放（即保持物体的大小不变）。

前面ch3的博客中的部分截图

      所以，既然我们假设 ( 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;
}

之前ch3中的博客截图

        上面的截图表示，即使p_homogeneous(p.x(), p.y(), p.z(), 1.0)中最后那个1.0不写，理论上来说，Eigen库会将三维向量自动拓展为齐次坐标形式。