高翔自动驾驶SLAM--ch2代码解析

ssm1122

已于 2023-12-04 21:24:54 修改

阅读量255

点赞数

分类专栏： SLAM 文章标签：自动驾驶人工智能机器学习

于 2023-12-04 21:19:34 首次发布

本文链接：https://blog.csdn.net/ssm1122/article/details/134794047

版权

SLAM 专栏收录该内容

1 篇文章 1 订阅

订阅专栏

motion.cc

这段 C++ 代码主要用于模拟一个物体在三维空间中的运动。它使用了 Google 的 gflags 和 glog 库来处理命令行参数和日志，以及 Eigen 库来处理数学运算。

首先，代码定义了一些全局变量，包括角速度、线速度和是否使用四元数进行计算。这些变量的值可以通过命令行参数来设置。

在 `main` 函数中，首先初始化了 Google 的日志系统，并解析了命令行参数。然后，创建了一个 `PangolinWindow` 对象用于可视化。

接着，代码将角速度从角度制转换为弧度制，并初始化了一些变量，包括物体的位姿（`pose`）、角速度矢量（`omega`）和本体系速度（`v_body`）。

在主循环中，代码首先更新物体的位置，然后根据是否使用四元数来选择不同的方式更新物体的旋转。然后，将物体的位姿输出到日志，并更新可视化窗口。

最后，当退出主循环后，调用 `Quit` 方法关闭可视化窗口，并返回 0 表示程序正常结束。

这段代码展示了如何使用 Eigen 库和四元数来模拟三维空间中的运动，以及如何使用 gflags 和 glog 库来处理命令行参数和日志。

//
// Created by xiang on 22-12-29.
//

#include <gflags/gflags.h>
#include <glog/logging.h>

#include "common/eigen_types.h"
#include "common/math_utils.h"
#include "tools/ui/pangolin_window.h"

/// 本节程序演示一个正在作圆周运动的车辆
/// 车辆的角速度与线速度可以在flags中设置
/*

这段代码使用了 Google 的 gflags 库来定义了三个命令行参数：

1. `angular_velocity`：一个双精度浮点数，表示角速度（以角度为单位），默认值为 10.0。
2. `linear_velocity`：一个双精度浮点数，表示车辆的线速度（以米/秒为单位），默认值为 5.0。
3. `use_quaternion`：一个布尔值，表示是否使用四元数进行计算， 默认值为 false。

这些参数可以在程序运行时通过命令行进行设置。例如，你可以使用以下命令来运行你的程序，并设置 `angular_velocity` 为 20.0，`linear_velocity` 为 10.0，并启用四元数计算：

./your_program --angular_velocity=20.0 --linear_velocity=10.0 --use_quaternion=true

在你的程序中，你可以通过 `FLAGS_angular_velocity`、`FLAGS_linear_velocity` 和 `FLAGS_use_quaternion` 来访问这些参数的值。

*/

DEFINE_double(angular_velocity, 10.0, "角速度（角度）制");
DEFINE_double(linear_velocity, 5.0, "车辆前进线速度 m/s");
DEFINE_bool(use_quaternion, false, "是否使用四元数计算");

int main(int argc, char** argv) {

/*
这段代码是使用 Google 的 glog 和 gflags 库来初始化日志系统和解析命令行参数。

1. `google::InitGoogleLogging(argv[0]);` 这行代码初始化了 Google 的日志系统。`argv[0]` 通常是程序的名称，它会被用作日志消息的前缀。

2. `FLAGS_stderrthreshold = google::INFO;` 这行代码设置了日志的阈值。这意味着只有级别大于或等于 INFO 的日志消息才会被输出到标准错误输出（stderr）。

3. `FLAGS_colorlogtostderr = true;` 这行代码启用了日志的颜色输出。这意味着不同级别的日志消息会用不同的颜色显示，以便于区分。

4. `google::ParseCommandLineFlags(&argc, &argv, true);` 这行代码解析了命令行参数。这意味着你可以在命令行中设置定义在代码中的 FLAGS 变量的值。例如，你可以使用 `--stderrthreshold=WARNING` 来改变日志的阈值。

总的来说，这段代码为程序的运行提供了灵活的配置方式，可以通过命令行参数来改变程序的行为，同时也提供了强大的日志功能，帮助开发者调试程序和追踪问题。


*/

    google::InitGoogleLogging(argv[0]);
    FLAGS_stderrthreshold = google::INFO;
    FLAGS_colorlogtostderr = true;
    google::ParseCommandLineFlags(&argc, &argv, true);

    /// 可视化
/*
这段代码是在初始化一个名为 `ui` 的 `PangolinWindow` 对象，用于可视化。`PangolinWindow` 是一个类，它可能是用于创建和管理可视化窗口的类。

`ui.Init()` 是一个方法调用，它可能是用于初始化窗口的方法。如果窗口初始化失败（即 `ui.Init()` 返回 `false`），那么程序将立即返回错误代码 `-1` 并退出。这是一种常见的错误处理模式，如果在初始化阶段发生错误，那么没有必要继续执行后续的代码。

总的来说，这段代码的目的是创建并初始化一个可视化窗口，如果初始化失败，那么程序将立即退出。

*/
    sad::ui::PangolinWindow ui;
    if (ui.Init() == false) {
        return -1;
    }
/*
这段代码主要是在初始化一些变量，这些变量将用于后续的运动模拟。

1. `double angular_velocity_rad = FLAGS_angular_velocity * sad::math::kDEG2RAD;` 这行代码将角速度从角度制转换为弧度制。`FLAGS_angular_velocity` 是一个全局变量，它的值可以通过命令行参数来设置。`sad::math::kDEG2RAD` 是一个常量，表示从角度制转换到弧度制的转换因子。

2. `SE3 pose;` 这行代码定义了一个 `SE3` 类型的变量 `pose`，表示物体的位姿。`SE3` 是一个类，它可能是用于表示三维空间中的刚体运动的类。

3. `Vec3d omega(0, 0, angular_velocity_rad);` 这行代码定义了一个 `Vec3d` 类型的变量 `omega`，表示角速度矢量。`Vec3d` 是一个类，它可能是用于表示三维向量的类。

4. `Vec3d v_body(FLAGS_linear_velocity, 0, 0);` 这行代码定义了一个 `Vec3d` 类型的变量 `v_body`，表示本体系速度。`FLAGS_linear_velocity` 是一个全局变量，它的值可以通过命令行参数来设置。

5. `const double dt = 0.05;` 这行代码定义了一个常量 `dt`，表示每次更新的时间。

总的来说，这段代码的目的是初始化一些变量，这些变量将用于后续的运动模拟。

*/
    double angular_velocity_rad = FLAGS_angular_velocity * sad::math::kDEG2RAD;  // 弧度制角速度
    SE3 pose;                                                                    // TWB表示的位姿
    Vec3d omega(0, 0, angular_velocity_rad);                                     // 角速度矢量
    Vec3d v_body(FLAGS_linear_velocity, 0, 0);                                   // 本体系速度
    const double dt = 0.05;                                                      // 每次更新的时间

    while (ui.ShouldQuit() == false) {

/*
这段代码是在一个循环中，模拟物体在三维空间中的运动。

1. `Vec3d v_world = pose.so3() * v_body;` 这行代码计算了物体在世界坐标系下的速度 `v_world`。`pose.so3()` 返回物体的旋转部分，`v_body` 是物体在本体坐标系下的速度。

2. `pose.translation() += v_world * dt;` 这行代码更新了物体的位置。它将物体的速度乘以时间步长 `dt`，然后加到物体的当前位置上。

3. 接下来的代码更新了物体的旋转。如果 `FLAGS_use_quaternion` 为 `true`，则使用四元数进行计算；否则，使用 `SO3::exp` 函数进行计算。这两种方法都可以正确地处理三维空间的旋转。

4. `LOG(INFO) << "pose: " << pose.translation().transpose();` 这行代码将物体的当前位置输出到日志。

5. `ui.UpdateNavState(sad::NavStated(0, pose, v_world));` 这行代码更新了可视化窗口的状态。`sad::NavStated(0, pose, v_world)` 创建了一个新的导航状态，包含了物体的位姿和速度。

6. `usleep(dt * 1e6);` 这行代码使程序暂停一段时间，以模拟真实的时间流逝。

总的来说，这段代码在每个时间步长中，都更新了物体的位置和旋转，然后更新了可视化窗口的状态，并暂停一段时间以模拟真实的时间流逝。这是一个典型的物理模拟循环。
*/
        // 更新自身位置
        Vec3d v_world = pose.so3() * v_body;
        pose.translation() += v_world * dt;

        // 更新自身旋转
        if (FLAGS_use_quaternion) {
/*
这段代码是在使用四元数来更新物体的旋转。

1. `Quatd q = pose.unit_quaternion() * Quatd(1, 0.5 * omega[0] * dt, 0.5 * omega[1] * dt, 0.5 * omega[2] * dt);` 这行代码首先获取物体当前的旋转四元数 `pose.unit_quaternion()`，然后与一个新的四元数相乘，这个新的四元数由角速度 `omega` 和时间步长 `dt` 计算得出。这是一种常用的方法，用于根据角速度更新四元数。

2. `q.normalize();` 这行代码对四元数进行了归一化。由于浮点数的计算误差，四元数可能会逐渐偏离单位长度，归一化可以确保四元数保持单位长度，这是四元数表示旋转的一个重要性质。

3. `pose.so3() = SO3(q);` 这行代码将更新后的四元数转换为 `SO3` 类型，并设置为物体的新旋转。`SO3` 是一个类，它可能是用于表示三维空间中的旋转的类。

总的来说，这段代码的目的是根据角速度和时间步长，使用四元数来更新物体的旋转。

*/


            Quatd q = pose.unit_quaternion() * Quatd(1, 0.5 * omega[0] * dt, 0.5 * omega[1] * dt, 0.5 * omega[2] * dt);
            q.normalize();
            pose.so3() = SO3(q);
        } else {
            pose.so3() = pose.so3() * SO3::exp(omega * dt);
        }

        LOG(INFO) << "pose: " << pose.translation().transpose();
        ui.UpdateNavState(sad::NavStated(0, pose, v_world));

        usleep(dt * 1e6);
    }

    ui.Quit();
    return 0;
}