//
// Created by xiang on 22-12-29.
// Commented by WXL on 23-09-01.
//
#include <gflags/gflags.h>
#include <glog/logging.h>
#include "common/eigen_types.h"
#include "common/math_utils.h"
#include "tools/ui/pangolin_window.h"
/*********************************
欧拉角定义:
旋转向量转旋转矩阵 Eigen::Matrix3d R = Eigen::AngleAxisd(M_PI/2, Eigen::Vector3d(0,0,1)).toRotationMatrix();
旋转矩阵转四元数 Eigen::Quaterniond q(R); // 或者四元数(从旋转矩阵构造)
平移Eigen::Vector3d t(1,0,0); // 沿X轴平移1
SO(n) 特殊正交群 对应 n*n 的旋转矩阵 群(集合)
SE(n+1) 特殊欧式群 对应 n*n 的旋转矩阵和 n*1的平移向量组合成的变换矩阵 群(集合)
so(n) 对应的李代数为so(n) n×1 列向量,使得向量和代数一一对应可以使用代数的更新方法来更新矩阵
SO(3) 表示三维空间的 旋转矩阵 集合 3×3
SE(3) 表示三维空间的 变换矩阵 集合 4×4
李代数 so3的本质就是个三维向量,直接Eigen::Vector3d定义.3个旋转
李代数 se3的本质就是个六维向量,直接Eigen::Matrix<double,6,1>定义,3个旋转 + 3个平移
旋转向量定义的李群SO(3) Sophus::SO3 SO3_v( 0, 0, M_PI/2 ); // 亦可从旋转向量构造 这里注意,不是旋转向量的三个坐标值,有点像欧拉角构造。
旋转矩阵定义的李群SO(3) Sophus::SO3 SO3_R(R); // Sophus::SO(3)可以直接从旋转矩阵构造
四元素定义的李群SO(3) Sophus::SO3 SO3_q(q);
从旋转矩阵和平移t构造SE3 Sophus::SE3 SE3_Rt(R,t); // 从R,t构造SE(3)
从四元素和平移t构造SE3 Sophus::SE3 SE3_qt(q,t); // 从q,t构造SE(3)
李代数so3为李群SO(3)的对数映射 Eigen::Vector3d so3 = SO3_R.log();
李代数se(3)是一个6维向量,为李群SE3 的对数映射
typedef Eigen::Matrix<double,6,1> Vector6d;// Vector6d指代Eigen::Matrix<double,6,1>
Vector6d se3 = SE3_Rt.log();
李代数指数映射成旋转矩阵对应的李群
Eigen::Vector3d so33 (1, 1, 1);
Sophus::SO3 SO3 =Sophus::SO3::exp(so33);
Sophus::SO3::hat(so3) //hat为向量到反对称矩阵,相当于^运算.
Sophus::SO3::vee( Sophus::SO3::hat(so3) ).transpose() //vee为反对称矩阵到向量,相当于下尖尖运算 .
**********************************/
/// 本节程序演示一个正在作圆周运动的车辆
/// 车辆的角速度与线速度可以在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表示的位姿,初始化 pose translation (0,0,0)
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() 表示旋转矩阵 R,类似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;
}
参考资料1.
参考资料2 : https://juejin.cn/post/7062623440116826119
参考资料3 : https://www.zhihu.com/question/280293029