nodding_lidar
Toggle main menu visibility
読み取り中…
検索中…
一致する文字列を見つけられません
test_geometry.cpp
[詳解]
1
// geometry.hpp の単体テスト。ROSなしで動く。失敗すると、終了コードが0以外になる。
2
#include <cmath>
3
#include <cstdio>
4
#include <cstdlib>
5
6
#include "
nodding_lidar/geometry.hpp
"
7
8
using namespace
nodding_lidar
;
// NOLINT
9
10
static
int
g_failures
= 0;
11
12
static
void
check
(
bool
ok,
const
char
* what)
13
{
14
if
(!ok) {
15
std::printf(
"FAIL: %s\n"
, what);
16
++
g_failures
;
17
}
18
}
19
20
static
bool
near
(
double
a,
double
b,
double
eps = 1e-4)
21
{
22
return
std::fabs(a - b) < eps;
23
}
24
25
static
double
deg
(
double
d) {
return
d *
kPi
/ 180.0;}
26
27
int
main
()
28
{
29
const
double
h = 0.0325;
30
31
// 1. 回転角0度: 前方1mの点は (1, 0, h)
32
{
33
const
Vec3
p =
project_point
(1.0, 0.0, 0.0, h);
34
check
(
near
(p.
x
, 1.0) &&
near
(p.
y
, 0.0) &&
near
(p.
z
, h),
"phi=0 forward point"
);
35
}
36
37
// 2. 回転角15度: 前方1mの点は (0.9743, 0, -0.2274)。正の角度で前方が下がる。
38
{
39
const
Vec3
p =
project_point
(1.0, 0.0,
deg
(15.0), h);
40
check
(
near
(p.
x
, 0.9743) &&
near
(p.
y
, 0.0) &&
near
(p.
z
, -0.2274),
"phi=15deg forward point"
);
41
check
(p.
z
< 0.0,
"positive angle lowers the front"
);
42
}
43
44
// 3. 回転角-15度: 前方が上がる
45
{
46
const
Vec3
p =
project_point
(1.0, 0.0,
deg
(-15.0), h);
47
check
(p.
z
> 0.0,
"negative angle raises the front"
);
48
}
49
50
// 4. 回転軸(Y軸)上の点(真横)は、y座標が回転しても変わらない
51
{
52
const
Vec3
p =
project_point
(1.0,
deg
(90.0),
deg
(15.0), h);
53
check
(
near
(p.
y
, 1.0),
"side point y is unchanged by rotation"
);
54
}
55
56
// 5. クォータニオン+平行移動の姿勢が、行列による計算と一致する
57
{
58
const
double
phis[] = {-15.0, -7.5, 0.0, 3.0, 15.0, 40.0};
59
const
double
angs[] = {-170.0, -90.0, 0.0, 45.0, 135.0};
60
const
double
ranges[] = {0.2, 1.0, 7.5};
61
bool
all_ok =
true
;
62
for
(
double
pd : phis) {
63
for
(
double
ad : angs) {
64
for
(
double
r : ranges) {
65
const
Pose
pose =
servo_to_lidar_pose
(
deg
(pd), h);
66
const
Vec3
in_lidar{r * std::cos(
deg
(ad)), r * std::sin(
deg
(ad)), 0.0};
67
const
Vec3
a =
apply_pose
(pose, in_lidar);
68
const
Vec3
b =
project_point
(r,
deg
(ad),
deg
(pd), h);
69
if
(!(
near
(a.
x
, b.
x
, 1e-9) &&
near
(a.
y
, b.
y
, 1e-9) &&
near
(a.
z
, b.
z
, 1e-9))) {
70
all_ok =
false
;
71
}
72
}
73
}
74
}
75
check
(all_ok,
"pose (quaternion+translation) matches matrix projection"
);
76
}
77
78
// 6. 首振りの波形 (周期4秒)
79
{
80
check
(
near
(
nodding_wave
(0.0, 4.0, 0.0), 0.0),
"wave t=0"
);
81
check
(
near
(
nodding_wave
(1.0, 4.0, 0.0), 1.0),
"wave t=T/4 (triangle)"
);
82
check
(
near
(
nodding_wave
(2.0, 4.0, 0.0), 0.0),
"wave t=T/2 (triangle)"
);
83
check
(
near
(
nodding_wave
(3.0, 4.0, 0.0), -1.0),
"wave t=3T/4 (triangle)"
);
84
check
(
near
(
nodding_wave
(4.0, 4.0, 0.0), 0.0),
"wave t=T (triangle)"
);
85
check
(
near
(
nodding_wave
(0.5, 4.0, 0.0), 0.5),
"wave t=T/8 (triangle)"
);
86
check
(
near
(
nodding_wave
(1.0, 4.0, 1.0), 1.0),
"wave t=T/4 (sine)"
);
87
check
(
near
(
nodding_wave
(1.0, 4.0, 0.3), 1.0),
"wave t=T/4 (mixed)"
);
88
check
(
near
(
nodding_wave
(-1.0, 4.0, 0.0), -1.0),
"wave negative time wraps"
);
89
}
90
91
// 7. パルス幅
92
{
93
check
(
near
(
angle_to_pulse_us
(0.0, 1500.0, 11.11, 1.0, 600.0, 2400.0), 1500.0),
"pulse center"
);
94
check
(
95
near
(
angle_to_pulse_us
(
deg
(15.0), 1500.0, 11.11, 1.0, 600.0, 2400.0), 1666.65, 0.01),
96
"pulse +15deg"
);
97
check
(
98
near
(
angle_to_pulse_us
(
deg
(15.0), 1500.0, 11.11, -1.0, 600.0, 2400.0), 1333.35, 0.01),
99
"pulse sign flip"
);
100
check
(
101
near
(
angle_to_pulse_us
(
deg
(90.0), 1500.0, 20.0, 1.0, 600.0, 2400.0), 2400.0),
102
"pulse clamp"
);
103
}
104
105
// 8. 点ごとの時刻
106
{
107
check
(
near
(
point_time_offset
(50, 100, 0.0, 0.1,
false
), 0.05),
"time offset from scan_time"
);
108
check
(
near
(
point_time_offset
(0, 100, 0.0, 0.1,
true
), 0.099),
"time offset reversed"
);
109
check
(
near
(
point_time_offset
(10, 100, 0.001, 0.1,
false
), 0.01),
110
"time offset from time_increment"
);
111
check
(
near
(
point_time_offset
(0, 0, 0.0, 0.1,
false
), 0.0),
"time offset n=0"
);
112
}
113
114
// 9. スキャン内の区間の選び方 (8区間)
115
{
116
int
k = -1;
117
double
f = -1.0;
118
sample_segment
(0.0, 0.08, 8, k, f);
119
check
(k == 0 &&
near
(f, 0.0),
"segment start"
);
120
sample_segment
(0.08, 0.08, 8, k, f);
121
check
(k == 7 &&
near
(f, 1.0),
"segment end"
);
122
sample_segment
(0.025, 0.08, 8, k, f);
123
check
(k == 2 &&
near
(f, 0.5),
"segment middle"
);
124
sample_segment
(0.5, 0.08, 8, k, f);
125
check
(k == 7 &&
near
(f, 1.0),
"segment clamp"
);
126
}
127
128
if
(
g_failures
== 0) {
129
std::printf(
"ALL TESTS PASSED\n"
);
130
return
EXIT_SUCCESS;
131
}
132
std::printf(
"%d test(s) failed\n"
,
g_failures
);
133
return
EXIT_FAILURE;
134
}
geometry.hpp
首振り角度と取付寸法を使い、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::servo_to_lidar_pose
Pose servo_to_lidar_pose(double phi, double lidar_height)
定義
geometry.hpp:56
main
int main(int argc, char **argv)
定義
scan_to_cloud_node.cpp:432
nodding_lidar::Pose
定義
geometry.hpp:29
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
deg
static double deg(double d)
定義
test_geometry.cpp:25
g_failures
static int g_failures
定義
test_geometry.cpp:10
check
static void check(bool ok, const char *what)
定義
test_geometry.cpp:12
near
static bool near(double a, double b, double eps=1e-4)
定義
test_geometry.cpp:20
test
test_geometry.cpp
構築:
1.18.0