#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中设置

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::InitGoogleLogging(argv[0]);
    FLAGS_stderrthreshold = google::INFO;
    FLAGS_colorlogtostderr = true;
    google::ParseCommandLineFlags(&argc, &argv, true);

    /// 可视化
    sad::ui::PangolinWindow ui;
    if (ui.Init() == false) {
        return -1;
    }

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

    while (ui.ShouldQuit() == false) {
        // 更新自身位置
        //直线速度矢量经过旋转矩阵变换后的速度矢量
        //pose.so3() 应该和tf::Quaternion是同类型，表示旋转矩阵
        Vec3d v_world = pose.so3() * v_body;
        //经过变换的直线速度矢量乘以时间，就得到了距离
        pose.translation() += v_world * dt;

        // 更新自身旋转
        if (FLAGS_use_quaternion) {
            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 {
            //当前旋转位姿乘以旋转矩阵，就得到了当前的旋转位姿
            //omega*dt的到三个旋转轴的变化量，通过so3::exp()得到变化的旋转矩阵
            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;
}
复制

参考文章：http://c.miaowlabs.com/B07.html?q=;