58 lines
1.8 KiB
C++
58 lines
1.8 KiB
C++
#ifndef TOOLS__MATH_TOOLS_HPP
|
||
#define TOOLS__MATH_TOOLS_HPP
|
||
|
||
#include <Eigen/Geometry>
|
||
#include <chrono>
|
||
|
||
namespace tools
|
||
{
|
||
// 将弧度值限制在(-pi, pi]
|
||
double limit_rad(double angle);
|
||
|
||
// 四元数转欧拉角
|
||
// x = 0, y = 1, z = 2
|
||
// e.g. 先绕z轴旋转，再绕y轴旋转，最后绕x轴旋转：axis0=2, axis1=1, axis2=0
|
||
// 参考：https://github.com/evbernardes/quaternion_to_euler
|
||
Eigen::Vector3d eulers(
|
||
Eigen::Quaterniond q, int axis0, int axis1, int axis2, bool extrinsic = false);
|
||
|
||
// 旋转矩阵转欧拉角
|
||
// x = 0, y = 1, z = 2
|
||
// e.g. 先绕z轴旋转，再绕y轴旋转，最后绕x轴旋转：axis0=2, axis1=1, axis2=0
|
||
Eigen::Vector3d eulers(Eigen::Matrix3d R, int axis0, int axis1, int axis2, bool extrinsic = false);
|
||
|
||
// 欧拉角转旋转矩阵
|
||
// zyx:先绕z轴旋转，再绕y轴旋转，最后绕x轴旋转
|
||
Eigen::Matrix3d rotation_matrix(const Eigen::Vector3d & ypr);
|
||
|
||
// 直角坐标系转球坐标系
|
||
// ypd为yaw、pitch、distance的缩写
|
||
Eigen::Vector3d xyz2ypd(const Eigen::Vector3d & xyz);
|
||
|
||
// 直角坐标系转球坐标系转换函数对xyz的雅可比矩阵
|
||
Eigen::MatrixXd xyz2ypd_jacobian(const Eigen::Vector3d & xyz);
|
||
|
||
// 球坐标系转直角坐标系
|
||
Eigen::Vector3d ypd2xyz(const Eigen::Vector3d & ypd);
|
||
|
||
// 球坐标系转直角坐标系转换函数对xyz的雅可比矩阵
|
||
Eigen::MatrixXd ypd2xyz_jacobian(const Eigen::Vector3d & ypd);
|
||
|
||
// 计算时间差a - b，单位：s
|
||
double delta_time(
|
||
const std::chrono::steady_clock::time_point & a, const std::chrono::steady_clock::time_point & b);
|
||
|
||
// 向量夹角 总是返回 0 ~ pi 来自SJTU
|
||
double get_abs_angle(const Eigen::Vector2d & vec1, const Eigen::Vector2d & vec2);
|
||
|
||
// 返回输入值的平方
|
||
template <typename T>
|
||
T square(T const & a)
|
||
{
|
||
return a * a;
|
||
};
|
||
|
||
double limit_min_max(double input, double min, double max);
|
||
} // namespace tools
|
||
|
||
#endif // TOOLS__MATH_TOOLS_HPP
|