nodding_lidar
読み取り中…
検索中…
一致する文字列を見つけられません
test_geometry.cpp
[詳解]
1// geometry.hpp の単体テスト。ROSなしで動く。失敗すると、終了コードが0以外になる。
2#include <cmath>
3#include <cstdio>
4#include <cstdlib>
5
7
8using namespace nodding_lidar; // NOLINT
9
10static int g_failures = 0;
11
12static void check(bool ok, const char * what)
13{
14 if (!ok) {
15 std::printf("FAIL: %s\n", what);
16 ++g_failures;
17 }
18}
19
20static bool near(double a, double b, double eps = 1e-4)
21{
22 return std::fabs(a - b) < eps;
23}
24
25static double deg(double d) {return d * kPi / 180.0;}
26
27int 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}
首振り角度と取付寸法を使い、LiDAR点を別の座標系へ変換する。
void sample_segment(double offset, double scan_time, int samples, int &k, double &frac)
double angle_to_pulse_us(double angle_rad, double center_us, double us_per_deg, double sign, double min_us, double max_us)
double point_time_offset(std::size_t i, std::size_t n, double time_increment, double scan_time, bool reverse)
Vec3 apply_pose(const Pose &p, const Vec3 &v)
double nodding_wave(double t, double period, double smoothness)
Vec3 project_point(double range, double scan_angle, double phi, double lidar_height)
constexpr double kPi
Pose servo_to_lidar_pose(double phi, double lidar_height)
int main(int argc, char **argv)
static double deg(double d)
static int g_failures
static void check(bool ok, const char *what)
static bool near(double a, double b, double eps=1e-4)