1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
#pragma once
#include <dirent.h>
#include <Eigen/Dense>
inline std::vector<std::string> split_string(const std::string &str, char tag)
{
std::vector<std::string> list;
std::string subStr = "";
for (size_t i = 0; i < str.length(); i++)
{
if (tag == str[i])
{
if (tag != ' ' || !subStr.empty())
{
list.push_back(subStr);
subStr.clear();
}
}
else
{
subStr.push_back(str[i]);
}
}
if (!subStr.empty())
{
list.push_back(subStr);
}
return list;
}
inline void get_filenames(std::string path, std::vector<std::string> &filenames, std::string tail = "")
{
DIR *pDir;
struct dirent *ptr;
if (!(pDir = opendir(path.c_str())))
{
printf("path %s doesn't exist!\n", path.c_str());
return;
}
while ((ptr = readdir(pDir)) != 0)
{
if (strcmp(ptr->d_name, ".") != 0 && strcmp(ptr->d_name, "..") != 0)
{
bool match = true;
size_t name_len = strlen(ptr->d_name);
size_t length = tail.length();
for (size_t i = 0; i < length; i++)
{
if (!tail.empty() && ptr->d_name[name_len - length + i] != tail[i])
{
match = false;
break;
}
}
if (match)
filenames.push_back(path + "/" + ptr->d_name);
}
}
std::sort(filenames.begin(), filenames.end());
closedir(pDir);
}
//inline Eigen::Vector3d matrix_2_euler_rpy(const Eigen::Matrix3d mat)
//{
// double temp = mat(2, 1) * mat(2, 1) + mat(2, 2) * mat(2, 2);
// double roll = atan2(mat(2, 1), mat(2, 2));
// double pitch = atan2(-mat(2, 0), sqrt(temp));
// double yaw = atan2(mat(1, 0), mat(0, 0));
// return Eigen::Vector3d(roll, pitch, yaw);
//}
inline Eigen::Matrix3d euler_rpy_2_matrix(Eigen::Vector3d rpy)
{
Eigen::AngleAxisd angle_axis(Eigen::AngleAxisd(rpy[2], Eigen::Vector3d::UnitZ()) *
Eigen::AngleAxisd(rpy[1], Eigen::Vector3d::UnitY()) *
Eigen::AngleAxisd(rpy[0], Eigen::Vector3d::UnitX()));
return angle_axis.toRotationMatrix();
}
inline Eigen::Vector3d quaternion_2_angle_axis(const Eigen::Quaterniond &quaternion)
{
Eigen::Quaterniond normalized_quaternion = quaternion.normalized();
// We choose the quaternion with positive 'w', i.e., the one with a smaller
// angle that represents this orientation.
if (normalized_quaternion.w() < 0.)
{
// Multiply by -1. http://eigen.tuxfamily.org/bz/show_bug.cgi?id=560
normalized_quaternion.w() = -1. * normalized_quaternion.w();
normalized_quaternion.x() = -1. * normalized_quaternion.x();
normalized_quaternion.y() = -1. * normalized_quaternion.y();
normalized_quaternion.z() = -1. * normalized_quaternion.z();
}
// We convert the normalized_quaternion into a vector along the rotation axis
// with length of the rotation angle.
const double angle =
2. * atan2(normalized_quaternion.vec().norm(), normalized_quaternion.w());
constexpr double kCutoffAngle = 1e-7; // We linearize below this angle.
const double scale = angle < kCutoffAngle ? 2. : angle / sin(angle / 2.);
return Eigen::Vector3d(scale * normalized_quaternion.x(),
scale * normalized_quaternion.y(),
scale * normalized_quaternion.z());
}
inline Eigen::Quaterniond angle_axis_2_quaternion(const Eigen::Vector3d &angle_axis)
{
double scale = 0.5;
double w = 1.;
constexpr double kCutoffAngle = 1e-8; // We linearize below this angle.
if (angle_axis.squaredNorm() > kCutoffAngle)
{
const double norm = angle_axis.norm();
scale = sin(norm / 2.) / norm;
w = cos(norm / 2.);
}
const Eigen::Vector3d quaternion_xyz = scale * angle_axis;
return Eigen::Quaterniond(w, quaternion_xyz.x(), quaternion_xyz.y(),
quaternion_xyz.z());
}
// 函数是求 目标坐标轴to 到 当前坐标轴from 的旋转
// 这个旋转等价于 将当前坐标系中的点from 变换到 目标坐标系的点to
inline Eigen::AngleAxisd get_cross_vector_rotation(const Eigen::Vector3d to, const Eigen::Vector3d from)
{
Eigen::Vector3d normal = to.cross(from);
double angle = atan2(normal.norm(), to.transpose() * from);
/* if (to == Eigen::Vector3d::UnitZ())
{
if ((from.x() > 0 && from.y() > 0 && !(normal.x() > 0 && normal.y() < 0))
|| (from.x() > 0 && from.y() < 0 && !(normal.x() < 0 && normal.y() < 0))
|| (from.x() < 0 && from.y() < 0 && !(normal.x() < 0 && normal.y() > 0))
|| (from.x() < 0 && from.y() > 0 && !(normal.x() > 0 && normal.y() > 0)))
normal = -normal;
}*/
normal.normalize();
return Eigen::AngleAxisd(angle, normal);
}