nodding_lidar
読み取り中…
検索中…
一致する文字列を見つけられません
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
17{
18
19constexpr double kPi = 3.14159265358979323846;
20
21struct Vec3
22{
23 double x{0.0};
24 double y{0.0};
25 double z{0.0};
26};
27
28struct Pose
29{
30 double qx{0.0};
31 double qy{0.0};
32 double qz{0.0};
33 double qw{1.0};
35};
36
37// Y軸まわりに phi [rad] 回転
38inline 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 として回転させる。
47inline 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の位置も回転軸のまわりを動く)
56inline 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
68inline 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: 正弦波 (折り返しがなめらか)
83inline 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] に変換 (範囲を超えないよう制限する)
106inline 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 のときは、時刻の進む向きと配列の向きが逆。
117inline 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)か。
131inline 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
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
Vec3 rotate_y(double phi, const Vec3 &v)
Pose servo_to_lidar_pose(double phi, double lidar_height)