nodding_lidar
Toggle main menu visibility
読み取り中…
検索中…
一致する文字列を見つけられません
geometry.hpp
[詳解]
1
// ROSに依存しない幾何計算。角度ノードと点群ノードの両方から使い、単体テストもできる。
2
//
3
// 座標系の約束 (servo_link):
4
// X: 前方, Y: 左 (= 回転軸), Z: 上
5
// 角度 phi は Y軸まわりの右手系回転 [rad]。正の角度で、前方 (+X) が下がる向き。
6
// R_y(phi) = [[ c, 0, s],
7
// [ 0, 1, 0],
8
// [-s, 0, c]]
9
// LiDARのセンサ原点は、回転軸から lidar_height だけ上 (回転する側の +Z) にある。
10
#pragma once
11
12
#include <algorithm>
13
#include <cmath>
14
#include <cstddef>
15
16
namespace
nodding_lidar
17
{
18
19
constexpr
double
kPi
= 3.14159265358979323846;
20
21
struct
Vec3
22
{
23
double
x
{0.0};
24
double
y
{0.0};
25
double
z
{0.0};
26
};
27
28
struct
Pose
29
{
30
double
qx
{0.0};
31
double
qy
{0.0};
32
double
qz
{0.0};
33
double
qw
{1.0};
34
Vec3
t
;
35
};
36
37
// Y軸まわりに phi [rad] 回転
38
inline
Vec3
rotate_y
(
double
phi,
const
Vec3
& v)
39
{
40
const
double
c = std::cos(phi);
41
const
double
s = std::sin(phi);
42
return
{c * v.
x
+ s * v.
z
, v.
y
, -s * v.
x
+ c * v.
z
};
43
}
44
45
// LiDAR平面内の点 (距離 range, スキャン角 scan_angle) を servo_link の座標に変換する。
46
// センサは回転軸から lidar_height だけ上にあるので、平面内の点の高さを lidar_height として回転させる。
47
inline
Vec3
project_point
(
double
range,
double
scan_angle,
double
phi,
double
lidar_height)
48
{
49
const
Vec3
p{range * std::cos(scan_angle), range * std::sin(scan_angle), lidar_height};
50
return
rotate_y
(phi, p);
51
}
52
53
// servo_link -> lidar_link の姿勢。
54
// 回転: R_y(phi) (クォータニオン (0, sin(phi/2), 0, cos(phi/2)))
55
// 平行移動: R_y(phi) * (0, 0, lidar_height) (LiDARの位置も回転軸のまわりを動く)
56
inline
Pose
servo_to_lidar_pose
(
double
phi,
double
lidar_height)
57
{
58
Pose
p;
59
p.
qx
= 0.0;
60
p.
qy
= std::sin(phi / 2.0);
61
p.
qz
= 0.0;
62
p.
qw
= std::cos(phi / 2.0);
63
p.
t
=
rotate_y
(phi,
Vec3
{0.0, 0.0, lidar_height});
64
return
p;
65
}
66
67
// 姿勢を点に適用: v' = R(q) * v + t
68
inline
Vec3
apply_pose
(
const
Pose
& p,
const
Vec3
& v)
69
{
70
const
double
tx = 2.0 * (p.
qy
* v.
z
- p.
qz
* v.
y
);
71
const
double
ty = 2.0 * (p.
qz
* v.
x
- p.
qx
* v.
z
);
72
const
double
tz = 2.0 * (p.
qx
* v.
y
- p.
qy
* v.
x
);
73
Vec3
r;
74
r.
x
= v.
x
+ p.
qw
* tx + (p.
qy
* tz - p.
qz
* ty) + p.
t
.
x
;
75
r.
y
= v.
y
+ p.
qw
* ty + (p.
qz
* tx - p.
qx
* tz) + p.
t
.
y
;
76
r.
z
= v.
z
+ p.
qw
* tz + (p.
qx
* ty - p.
qy
* tx) + p.
t
.
z
;
77
return
r;
78
}
79
80
// 首振りの波形。-1 〜 +1 を返す。t=0 で0から始まり、上向き(正)に進む。
81
// smoothness = 0: 三角波 (一定の速さで往復)
82
// smoothness = 1: 正弦波 (折り返しがなめらか)
83
inline
double
nodding_wave
(
double
t,
double
period,
double
smoothness)
84
{
85
if
(period <= 0.0) {
86
return
0.0;
87
}
88
double
ph = std::fmod(t, period) / period;
89
if
(ph < 0.0) {
90
ph += 1.0;
91
}
92
double
tri = 0.0;
93
if
(ph < 0.25) {
94
tri = 4.0 * ph;
95
}
else
if
(ph < 0.75) {
96
tri = 2.0 - 4.0 * ph;
97
}
else
{
98
tri = 4.0 * ph - 4.0;
99
}
100
const
double
s = std::clamp(smoothness, 0.0, 1.0);
101
const
double
sine = std::sin(2.0 *
kPi
* ph);
102
return
(1.0 - s) * tri + s * sine;
103
}
104
105
// 角度 [rad] をサーボのパルス幅 [us] に変換 (範囲を超えないよう制限する)
106
inline
double
angle_to_pulse_us
(
107
double
angle_rad,
double
center_us,
double
us_per_deg,
double
sign,
108
double
min_us,
double
max_us)
109
{
110
const
double
deg = angle_rad * 180.0 /
kPi
;
111
return
std::clamp(center_us + sign * deg * us_per_deg, min_us, max_us);
112
}
113
114
// スキャン内のi番目の点の、スキャン開始からの経過時刻 [s]。
115
// time_increment > 0 のときはそれを使い、0のときは scan_time / 点数 で決める。
116
// reverse = true のときは、時刻の進む向きと配列の向きが逆。
117
inline
double
point_time_offset
(
118
std::size_t i, std::size_t n,
double
time_increment,
double
scan_time,
bool
reverse)
119
{
120
if
(n == 0) {
121
return
0.0;
122
}
123
const
std::size_t k = reverse ? (n - 1 - i) : i;
124
if
(time_increment > 0.0) {
125
return
static_cast<
double
>
(k) * time_increment;
126
}
127
return
scan_time *
static_cast<
double
>
(k) /
static_cast<
double
>
(n);
128
}
129
130
// スキャンを samples 個の区間に分けたとき、offset [s] がどの区間(k)の、どの位置(frac, 0〜1)か。
131
inline
void
sample_segment
(
double
offset,
double
scan_time,
int
samples,
int
& k,
double
& frac)
132
{
133
if
(samples < 1) {
134
samples = 1;
135
}
136
double
pos = scan_time > 0.0 ? offset / scan_time *
static_cast<
double
>
(samples) : 0.0;
137
pos = std::clamp(pos, 0.0,
static_cast<
double
>
(samples));
138
k =
static_cast<
int
>
(std::floor(pos));
139
if
(k >= samples) {
140
k = samples - 1;
141
}
142
frac = pos -
static_cast<
double
>
(k);
143
}
144
145
}
// namespace nodding_lidar
nodding_lidar
定義
geometry.hpp:17
nodding_lidar::sample_segment
void sample_segment(double offset, double scan_time, int samples, int &k, double &frac)
定義
geometry.hpp:131
nodding_lidar::angle_to_pulse_us
double angle_to_pulse_us(double angle_rad, double center_us, double us_per_deg, double sign, double min_us, double max_us)
定義
geometry.hpp:106
nodding_lidar::point_time_offset
double point_time_offset(std::size_t i, std::size_t n, double time_increment, double scan_time, bool reverse)
定義
geometry.hpp:117
nodding_lidar::apply_pose
Vec3 apply_pose(const Pose &p, const Vec3 &v)
定義
geometry.hpp:68
nodding_lidar::nodding_wave
double nodding_wave(double t, double period, double smoothness)
定義
geometry.hpp:83
nodding_lidar::project_point
Vec3 project_point(double range, double scan_angle, double phi, double lidar_height)
定義
geometry.hpp:47
nodding_lidar::kPi
constexpr double kPi
定義
geometry.hpp:19
nodding_lidar::rotate_y
Vec3 rotate_y(double phi, const Vec3 &v)
定義
geometry.hpp:38
nodding_lidar::servo_to_lidar_pose
Pose servo_to_lidar_pose(double phi, double lidar_height)
定義
geometry.hpp:56
nodding_lidar::Pose
定義
geometry.hpp:29
nodding_lidar::Pose::qx
double qx
定義
geometry.hpp:30
nodding_lidar::Pose::qz
double qz
定義
geometry.hpp:32
nodding_lidar::Pose::qy
double qy
定義
geometry.hpp:31
nodding_lidar::Pose::t
Vec3 t
定義
geometry.hpp:34
nodding_lidar::Pose::qw
double qw
定義
geometry.hpp:33
nodding_lidar::Vec3
定義
geometry.hpp:22
nodding_lidar::Vec3::x
double x
定義
geometry.hpp:23
nodding_lidar::Vec3::z
double z
定義
geometry.hpp:25
nodding_lidar::Vec3::y
double y
定義
geometry.hpp:24
include
nodding_lidar
geometry.hpp
構築:
1.18.0